信息融合狀態(tài)估計爾曼濾波

1、整理ppt第七講第七講狀態(tài)估計狀態(tài)估計卡爾曼濾波卡爾曼濾波整理ppt2狀態(tài)估計的主要內容狀態(tài)估計的主要內容應用：應用： 通過數(shù)學方法尋求與觀測數(shù)據(jù)最佳擬合的狀態(tài)通過數(shù)學方法尋求與觀測數(shù)據(jù)最佳擬合的狀態(tài)向量。向量。 1 1、確定運動目標的當前位置與速度；、確定運動目標的當前位置與速度； 2 2、確定運動目標的未來位置與速度；、確定運動目標的未來位置與速度； 3 3、確定運動目標的固有特征或特征參數(shù)。、確定運動目標的固有特征或特征參數(shù)。整理ppt3狀態(tài)估計主要內容狀態(tài)估計主要內容：位置與速度估計。：位置與速度估計。位置估計位置估計：距離、方位和高度或仰角的估計；：距離、方位和高度或仰角的估計；速度

2、估計速度估計：速度、加速度估計。：速度、加速度估計。 整理ppt4狀態(tài)估計的主要方法狀態(tài)估計的主要方法 1 1、-濾波濾波 2 2、-濾波濾波 3 3、卡爾曼濾波、卡爾曼濾波 這些方法針對勻速或勻加速目標提出，如目標這些方法針對勻速或勻加速目標提出，如目標真實運動與真實運動與采用的目標模型采用的目標模型不一致，濾波器發(fā)散。不一致，濾波器發(fā)散。 整理ppt5算法的改進及適應性算法的改進及適應性狀態(tài)估計難點：狀態(tài)估計難點：機動目標的跟蹤機動目標的跟蹤 1 1、自適應、自適應-濾波和自適應濾波和自適應KalmanKalman濾波均改善濾波均改善對機動目標的跟蹤能力。對機動目標的跟蹤能力。 2 2、擴

3、展、擴展KalmanKalman濾波針對卡爾曼濾波在笛卡兒坐濾波針對卡爾曼濾波在笛卡兒坐標系中才能使用的局限而提出。標系中才能使用的局限而提出。整理ppt6卡爾曼濾波器卡爾曼濾波器 卡爾曼濾波器的應用卡爾曼濾波器的應用: : 通信、雷達、導航、自動控制等領域；通信、雷達、導航、自動控制等領域； 航天器的軌道計算、雷達目標跟蹤、生產(chǎn)過程的自航天器的軌道計算、雷達目標跟蹤、生產(chǎn)過程的自動控制等。動控制等。整理ppt7卡爾曼濾波器的應用特點卡爾曼濾波器的應用特點 對機動目標跟蹤中具有良好的性能；對機動目標跟蹤中具有良好的性能； 為最佳估計并能夠進行遞推計算；為最佳估計并能夠進行遞推計算； 只需當前的

4、一個測量值和前一個采樣周期的預測只需當前的一個測量值和前一個采樣周期的預測值就能進行狀態(tài)估計。值就能進行狀態(tài)估計。整理ppt8卡爾曼濾波器的局限性卡爾曼濾波器的局限性 卡爾曼濾波器解決運動目標或實體的狀態(tài)估計問卡爾曼濾波器解決運動目標或實體的狀態(tài)估計問題時，題時，動態(tài)方程和測量方程均為線性動態(tài)方程和測量方程均為線性。整理ppt9一、數(shù)字濾波器作估值器一、數(shù)字濾波器作估值器 1 1、非遞歸估值器、非遞歸估值器 2 2、遞歸估值器、遞歸估值器 整理ppt10 1 1、非遞歸估值器、非遞歸估值器 z1z1hmh3h2h1z1z1z2z3X 采樣平均估值器：采樣平均估值器： 采用時域分析方法在摻雜有噪

5、聲的測量信號中采用時域分析方法在摻雜有噪聲的測量信號中估計信號估計信號x x。整理ppt11 根據(jù)數(shù)字信號處理我們知道，所謂非遞歸數(shù)字濾波器是一根據(jù)數(shù)字信號處理我們知道，所謂非遞歸數(shù)字濾波器是一種只有前饋而沒有反饋的濾波器，它的沖擊脈沖響應是有限的，種只有前饋而沒有反饋的濾波器，它的沖擊脈沖響應是有限的， 在許多領域有著廣泛的應用。在許多領域有著廣泛的應用。 假定用假定用zk表示觀測值，表示觀測值， zk=x+nk 式中：式中： x 恒定信號或稱被估參量恒定信號或稱被估參量 nk 觀測噪聲采樣觀測噪聲采樣 假定，假定，E(x)=x0，D(x)=2x，E(nk)=0，E(n2k)=2n。整理pp

6、t12 h1, h2, , hm是濾波器的脈沖響應是濾波器的脈沖響應hj的采樣，或稱濾波器的的采樣，或稱濾波器的加權系數(shù)。濾波器的輸出加權系數(shù)。濾波器的輸出 miiizhX1當當h1=h2=hm=1/m時，時， miizmX11該式表明，估計該式表明，估計 是用是用m個采樣值的平均值作為被估參量個采樣值的平均值作為被估參量x的近的近似值的，故稱其為采樣平均估值器。似值的，故稱其為采樣平均估值器。 X整理ppt13 估計的均方誤差以估計的均方誤差以P表示，有表示，有 jijinmjmiijmnnmExXEEP221122211)()(當當i=j時時ij=1，當，當ij時時ij=0，有，有 mij

7、mPn2最后得：最后得： 整理ppt14結論結論X 估計值估計值 是用是用m m個采樣值的平均值作為被個采樣值的平均值作為被估參量估參量x x的近似值；的近似值； 估值器的均方誤差隨著估值器的均方誤差隨著m m的增加而減少；的增加而減少； 該估值器是一個無偏估值器。該估值器是一個無偏估值器。01)()(1)(xxEnxmEXEmii整理ppt152 2、遞歸估值器、遞歸估值器z1zka1 aykkX一階遞歸估值器：一階遞歸估值器： a a為濾波器的加權系數(shù)，為濾波器的加權系數(shù)，a1a1。 整理ppt16 遞歸數(shù)字濾波器是一種帶有反饋的濾波器，它有無限的脈遞歸數(shù)字濾波器是一種帶有反饋的濾波器，它

8、有無限的脈沖響應，有階數(shù)少的優(yōu)點，沖響應，有階數(shù)少的優(yōu)點， 但其暫態(tài)過程較長。關于信號和噪但其暫態(tài)過程較長。關于信號和噪聲的基本假設與非遞歸情況相同。上圖給出的一階遞歸濾波器聲的基本假設與非遞歸情況相同。上圖給出的一階遞歸濾波器輸入輸出信號關系如下：輸入輸出信號關系如下： kkkkknxzzayy1式中，式中，zk與非遞歸情況相同；與非遞歸情況相同；a是一個小于是一個小于1的濾波器加權系數(shù)，的濾波器加權系數(shù)， 如果它大于或等于如果它大于或等于1， 該濾波器就不穩(wěn)定了。該濾波器就不穩(wěn)定了。 整理ppt17k時刻的輸出：時刻的輸出： yk=ak-1z1+ak-2z2+azk-1+zk 將將zk中的

9、信號和噪聲分開，并代入，有輸出中的信號和噪聲分開，并代入，有輸出 kiiikkknaxaay111由于由于a1，故隨著，故隨著k值的增加，值的增加，yk趨近于趨近于x/(1-a)。這樣，如。這樣，如果以果以(1-a)yk作為作為x的估計值，的估計值， kkyaX)1 (則則 ikiikkknaaxaX1)1 ()1 (整理ppt18 此時信號此時信號x和估值之間只差一個噪聲項。當和估值之間只差一個噪聲項。當k值較大時，值較大時， 估值的均方誤差估值的均方誤差 2211)(nkaaxXEP而一次取樣的均方誤差而一次取樣的均方誤差 2221)()(nkknExnxEP故上一結果的均方誤差約為一次采

10、樣的（故上一結果的均方誤差約為一次采樣的（1-a）/（1+a）倍。）倍。 整理ppt19二、線性均方估計二、線性均方估計1 1、最優(yōu)非遞歸估計（標量維納濾波）、最優(yōu)非遞歸估計（標量維納濾波）2 2、遞歸估計、遞歸估計整理ppt20 1. 最優(yōu)非遞歸估計最優(yōu)非遞歸估計 非遞歸濾波器的估計值及其估計誤差可分別表示為非遞歸濾波器的估計值及其估計誤差可分別表示為 )()(2121miiimiiixzhExXEPzhX整理ppt21 對對m個參數(shù)逐一求導，令等于零，在均值為零的白噪聲的情個參數(shù)逐一求導，令等于零，在均值為零的白噪聲的情況下，可得到最小均方誤差和估計：況下，可得到最小均方誤差和估計： 22

11、1)(nbmxXEPmiizbmX11其中，其中，b=2n/2x，在，在bm時，這種估計近似于采樣平均。在噪時，這種估計近似于采樣平均。在噪聲方差聲方差2n較大時，其性能明顯優(yōu)于非最佳情況。這種最小均方較大時，其性能明顯優(yōu)于非最佳情況。這種最小均方誤差準則下的線性濾波，通常稱作標量維納濾波。誤差準則下的線性濾波，通常稱作標量維納濾波。 hj與非最優(yōu)情況的不同，這里的濾波器的加權系數(shù)為與非最優(yōu)情況的不同，這里的濾波器的加權系數(shù)為bmhhhm121整理ppt22 2、由最優(yōu)非遞推估計導出遞歸估計、由最優(yōu)非遞推估計導出遞歸估計由前可知，由前可知， 非遞歸估值器可以表示為非遞歸估值器可以表示為 kii

12、ikiiikzkhzhX11)(條件與前面相同。對條件與前面相同。對k+1次取樣，相應的估計量次取樣，相應的估計量 11111) 1(kiiikiiikzkhzhX相應的估計誤差相應的估計誤差 21)(nbkkP2) 1(1) 1(nbkkP整理ppt23由由b=2n/2x及及hi(k)=1/（k+b），有），有 122) 1() 1()()(kniknibkPkhbkPkh所以有所以有 )/(11111bkbkbkbbkkkkkbbb11整理ppt24于是于是, 1111kiikkzbX分成二項：分成二項： 111111kkkiikkzbzbX將第一項同時乘、除一個將第一項同時乘、除一個bk

13、，則，則 111111111kkkkkkkkiikkkkzbXbbzbzbbbX整理ppt25或或 )(111kkkkkXZbXX11111kkkkkkzbbXbX最后有最后有 整理ppt26最優(yōu)遞歸估計器最優(yōu)遞歸估計器11111kkkkkkzbbXbX遞推公式遞推公式整理ppt27最優(yōu)遞歸估計器最優(yōu)遞歸估計器)(111kkkkkXzbXX遞推公式遞推公式整理ppt28 遞推開始時的初始條件遞推開始時的初始條件 應滿足應滿足 ：0X0)(020XXxE以使以使 為最佳值。為最佳值。 解之，得解之，得，這時的，這時的 如果如果E(x)=0，可從零開始遞推運算，即，可從零開始遞推運算，即 0X)(

14、0 xEX 2xPbbXnx102200整理ppt29三、標量卡爾曼濾波器時變信號三、標量卡爾曼濾波器時變信號 主要作用：主要作用： 對摻雜有噪聲的隨機信號進行線性估計。對摻雜有噪聲的隨機信號進行線性估計。整理ppt30 1、模型、模型 1） 信號模型信號模型 設要估計的隨機信號為由均值為設要估計的隨機信號為由均值為0，方差為，方差為2w的白噪聲激的白噪聲激勵的一個一階遞歸過程，即信號對時間變化滿足動態(tài)方程：勵的一個一階遞歸過程，即信號對時間變化滿足動態(tài)方程： x(k)=ax(k-1)+w(k-1) 式中，式中，a系統(tǒng)參數(shù)；系統(tǒng)參數(shù)； w(k-1)白噪聲采樣。白噪聲采樣。如果令如果令x(0)=

15、0，Ew(k)=0， 則則 jkjkjwkwEjPww20)()()(整理ppt31該過程該過程 稱作一階自回歸過程。稱作一階自回歸過程。x(k)的均值和方差分別為：的均值和方差分別為： 22221)0()()(0)(aPkxEkxDkxEwxx自相關函數(shù)自相關函數(shù) )0()()()(|xjxPajPjkxkxE整理ppt322） 觀測模型觀測模型觀測模型由下式給出：觀測模型由下式給出： z(k)=cx(k)+v(k) 式中：式中：c測量因子；測量因子； v(k)E()=0，D()=2n的白噪聲。的白噪聲。最優(yōu)遞推估值器的信號和觀測模型如圖所示。最優(yōu)遞推估值器的信號和觀測模型如圖所示。 整理p

16、pt33最優(yōu)遞推估值器的信號和觀測模型最優(yōu)遞推估值器的信號和觀測模型 cz(k)V(k)x(k)z1aW(k1)整理ppt34 2、標量卡爾曼濾波器、標量卡爾曼濾波器由前將遞歸估計的形式寫成：由前將遞歸估計的形式寫成： )()() 1()()(kzkbkXkakX均方誤差均方誤差 )()()() 1()()()()(22kxkzkbkXkaEkxkXEkP分別對分別對a(k)和和b(k)求導，并令其等于求導，并令其等于0，求其最佳估計，得出，求其最佳估計，得出a(k)與與b(k)的關系：的關系： a(k)=a1-cb(k) 最后有遞歸估值器：最后有遞歸估值器： )1()()() 1()(kXa

17、ckzkbkXakX整理ppt35b(k)為濾波器增益為濾波器增益 12121)()()(nkPckcPkb其中其中, 221) 1()(wkPakP均方誤差均方誤差 )(1)(2kbckPn 對于給定的信號模型和觀測模型，上述一組方程便稱為一對于給定的信號模型和觀測模型，上述一組方程便稱為一維標量卡爾曼濾波器，其結構如圖所示。維標量卡爾曼濾波器，其結構如圖所示。 整理ppt36標量卡爾曼濾波器結構標量卡爾曼濾波器結構 b(k)z1acz(k)(kX整理ppt37 3、標量卡爾曼預測器、標量卡爾曼預測器 標量卡爾曼濾波是對摻雜有噪聲的隨機信號進行線性估計。標量卡爾曼濾波是對摻雜有噪聲的隨機信號

18、進行線性估計。但經(jīng)常要對信號的未來值進行預測，特別是在控制系統(tǒng)中。根但經(jīng)常要對信號的未來值進行預測，特別是在控制系統(tǒng)中。根據(jù)預測提前時間的多少，把預測分成據(jù)預測提前時間的多少，把預測分成1步、步、2步、步、 m步預測，步預測， 通常把通常把1步預測記作步預測記作 。預測的步數(shù)越多，。預測的步數(shù)越多， 誤差誤差越大。越大。 這里討論這里討論1步預測問題。步預測問題。 信號模型和觀測模型同前：信號模型和觀測模型同前： )()()() 1() 1()(kvkcxkzkwkaxkx)/1(kkX整理ppt38根據(jù)前一節(jié)，根據(jù)前一節(jié)， 有一步線性預測遞推公式：有一步線性預測遞推公式： )()() 1/(

19、)()/1(kzkkkXkakkX其中，其中，a(k)和和(k)可以通過使均方預測誤差最小來確定。預測的可以通過使均方預測誤差最小來確定。預測的均方誤差可表示為均方誤差可表示為 22)/1()/1()/1()/1(kkXkkxEkkeEkkP將預測方程代入該式，并求導，就會得到一組正交方程：將預測方程代入該式，并求導，就會得到一組正交方程： 0)()/1(0)1/()/1(kzkkeEkkXkkeE整理ppt39解之，得解之，得 a(k)=a-c(k) 將其代入預測方程，有將其代入預測方程，有 )1/()()() 1/()/1(kkXckzkkkXakkX進一步可求出：進一步可求出： 22)(

20、)/1(wnkcakkP其中，其中， 22) 1/() 1/()(nkkPckkacPk 由以上表達式可以看出，可根據(jù)均方預測誤差由以上表達式可以看出，可根據(jù)均方預測誤差P(k/k-1)計計算算(k)，然后再給出，然后再給出P(k+1/k)的均方預測誤差。的均方預測誤差。 整理ppt40最優(yōu)一步預測器最優(yōu)一步預測器整理ppt41最優(yōu)一步預測及濾波器最優(yōu)一步預測及濾波器整理ppt42 四、向量卡爾曼濾波器四、向量卡爾曼濾波器1、信號向量和數(shù)據(jù)向量、信號向量和數(shù)據(jù)向量 如果要求對如果要求對q個信號進行同時估計，這個信號進行同時估計，這q個信號在個信號在k時刻的時刻的采樣值記作采樣值記作x1(k)、

21、x2(k) 、xq(k)。假設每個信號都是由一階。假設每個信號都是由一階自回歸過程產(chǎn)生的，自回歸過程產(chǎn)生的， 即第即第個信號在時刻個信號在時刻k的采樣值為的采樣值為: x(k)=ax(k-1)+w(k-1) =1，2， ，q 每個每個w過程都是白的，零均值的，與其它過程的采樣是獨立的。過程都是白的，零均值的，與其它過程的采樣是獨立的。 于是把于是把q個信號與個信號與q個白噪聲組成的個白噪聲組成的q維向量分別表示成維向量分別表示成 ,)()()()(21kxkxkxkXq)()()()(21kwkwkwkWq整理ppt43顯然，顯然， X(k)=AX(k-1)+W(k-1) 式中，式中，X(k)

22、，X(k-1)，W(k-1)都是都是q維向量，維向量，A是個是個qq階矩陣，即階矩陣，即 qaaaA00000021 如果信號不滿足一階遞歸差分方程，而滿足二階遞歸差分如果信號不滿足一階遞歸差分方程，而滿足二階遞歸差分方程，即方程，即 x(k)=ax(k-1)+bx(k-2)+w(k-1) 整理ppt44定義兩個分量定義兩個分量 x1(k)=x(k)x2(k)=x1(k-1)=x(k-1)于是，有于是，有 0) 1() 1() 1(01)()(2121kwkxkxbakxkx最后，有最后，有 X(k)=AX(k-1)+W(k-1) 結果把一個二階差分方程變成了一個一階二維向量方程，結果把一個二

23、階差分方程變成了一個一階二維向量方程， 該該方程用起來更簡單方便方程用起來更簡單方便。 整理ppt45 用用R(k)表示表示k時刻的距離，時刻的距離，R (k)表示表示k時刻的速度，時刻的速度，U(k)表表示示k時刻的加速度，時刻的加速度，T表示采樣周期，則表示采樣周期，則 )()() 1()()() 1(kTUkRkRkRTkRkR寫成一般形式：寫成一般形式： )()() 1()()() 1(122211kukxkxkTxkxkx其中，其中， )()(1kTUku.整理ppt46寫成向量形式：寫成向量形式： )(0)()(101) 1() 1(12121kukxkxTkxkx最后最后, 有有

24、 )()() 1(kWkAXkX即可寫成一階向量的形式。即可寫成一階向量的形式。 在對信號向量進行估計的過程中，同時產(chǎn)生在對信號向量進行估計的過程中，同時產(chǎn)生r個含有噪聲的個含有噪聲的測量值，記作測量值，記作z1(k)，z2(k)，, zr(k)。則得到一組觀測方程。則得到一組觀測方程: )()()()()()()()()(22221111kvkxckzkvkxckzkvkxckzrrrr整理ppt47其中，其中，vi(k)表示附加噪聲，表示附加噪聲，ci表示第表示第i個測量參數(shù)，于是有個測量參數(shù)，于是有 Z(k)=CX(k)+V(k) 式中，式中，Z(k)，V(k)是是r維向量，維向量，X(

25、k)是是q維向量，維向量，C是是rq階矩陣。階矩陣。 對于對于r=q，有，有 rcccC00000021C即是觀測矩陣。即是觀測矩陣。 整理ppt48 2、向量問題的表示、向量問題的表示 根據(jù)前面的討論，我們完全可以把前面的信號模型動態(tài)方根據(jù)前面的討論，我們完全可以把前面的信號模型動態(tài)方程和觀測方程寫成如下形式程和觀測方程寫成如下形式: )()()()() 1()(kVkCXkYkWkAXkX 采用標量運算和矩陣運算的等價關系，推廣到多維情況：采用標量運算和矩陣運算的等價關系，推廣到多維情況：1T2)()/(1BAbaABAbaABabBAba矩陣標量整理ppt49據(jù)此，可以將觀測噪聲的方差變

26、成協(xié)方差矩陣據(jù)此，可以將觀測噪聲的方差變成協(xié)方差矩陣 )()()(TkVkVEkR對兩個信號的情況，則有對兩個信號的情況，則有 2222221221212,21 ,22, 11 , 1)()()()()()()(vvvvkvEkvkvEkvkvEkvEkR同理，也可以把系統(tǒng)噪聲的方差變成協(xié)方差矩陣，同理，也可以把系統(tǒng)噪聲的方差變成協(xié)方差矩陣， 即即 )()()(TkWkWEkQ由于系統(tǒng)噪聲采樣互不相關，該協(xié)方差矩陣的非對角線元素的由于系統(tǒng)噪聲采樣互不相關，該協(xié)方差矩陣的非對角線元素的值均為零。值均為零。 單一信號均方誤差也可變成協(xié)方差矩陣，單一信號均方誤差也可變成協(xié)方差矩陣， )()()(Tk

27、EkEEkP整理ppt50 3、向量卡爾曼濾波器、向量卡爾曼濾波器 利用前面的概念，直接把標量卡爾曼濾波器公式變成向量利用前面的概念，直接把標量卡爾曼濾波器公式變成向量卡爾曼濾波器公式：卡爾曼濾波器公式： )1()()() 1()(kXCAkZkKkXAkX濾波器增益：濾波器增益： 1T1T1)()()()(kRCkCPCkPkK式中，式中， ) 1() 1()(11kQAkAPkPT實際上，實際上， 它是預測協(xié)方差。它是預測協(xié)方差。 誤差協(xié)方差矩陣：誤差協(xié)方差矩陣： )()()()(11kCPkKkPkP整理ppt51用用K(k)代替了代替了B(k)，因，因K(k)是通用符號，如圖：是通用符

28、號，如圖：向量卡爾曼濾波器結構向量卡爾曼濾波器結構 z1Acz(k)(kXK(k)整理ppt52增益矩陣增益矩陣K(k)的計算流程如圖所示：的計算流程如圖所示： 增益矩陣計算流程增益矩陣計算流程 計算P(k)計算K(k)遲延T計算P1(k)P (k)K(k)C(k)P(k1)C(k)R(k)A(k , k1)Q(k1)整理ppt53 4、向量卡爾曼預測器、向量卡爾曼預測器根據(jù)相同的推導方法，根據(jù)相同的推導方法， 可以獲得卡爾曼預測器方程組?？梢垣@得卡爾曼預測器方程組。預測方程：預測方程： )1/()()() 1/()/1(kkXCkZkGkkXAkkX預測增益預測增益: )()() 1/()

29、1/()(1TTkAKkRCkkCPCkkAPkG預測均方誤差預測均方誤差: )() 1/()()/1(TkQAkkPCkGAkkP 它們與標量的情況是一一對應的，只是用它們與標量的情況是一一對應的，只是用G(k)代替了代替了(k)。就可以將濾波和預測用同一個方框圖表示出來。就可以將濾波和預測用同一個方框圖表示出來。 整理ppt54 5、總結、總結 卡爾曼濾波器應用廣泛，卡爾曼濾波器應用廣泛， 這里只對其進行簡單歸納。這里只對其進行簡單歸納。 1） 卡爾曼濾波器的主要特性卡爾曼濾波器的主要特性 卡爾曼濾波器是一個遞歸、卡爾曼濾波器是一個遞歸、 線性、無偏和方差最小的濾波線性、無偏和方差最小的濾

30、波器，器，如果過程噪聲和觀測噪聲是正態(tài)高斯白噪聲如果過程噪聲和觀測噪聲是正態(tài)高斯白噪聲，則它保持最，則它保持最佳特性。佳特性。 整理ppt552） 卡爾曼濾波器模型卡爾曼濾波器模型目標運動模型：目標運動模型： )()()()()()()()(10000100010001)()()()(212121212121tWtWtWtWtXtXtXtXTTTtXTtXTtXTtX位置測量模型：位置測量模型： )()()()()()(00100001)()(21212121tVtVtXtXtXtXtZtZ整理ppt56狀態(tài)方程狀態(tài)方程: X(t+T)=(t)X(t)+W(t) Q(t)=EW(t)W(t)T

31、 觀測方程觀測方程:Z(t)=HX(t)+V(t) R(t)=EV(t)V(t)T 整理ppt573) 卡爾曼濾波器方程組卡爾曼濾波器方程組殘差殘差: )()()()(kXkHkZkE 預測方程預測方程: )()() 1(kXkkX狀態(tài)估計狀態(tài)估計: )1()()()()() 1()()()()()(kXkkHkZkKkXkkEkKkXkX卡爾曼濾波器增益卡爾曼濾波器增益: 1TT)()()()()()()(kRkHkPkHkHkPkK整理ppt58 預測協(xié)方差預測協(xié)方差: )()()()() 1(TkQkkPkkP估計協(xié)方差估計協(xié)方差: )()()()(kPkHkKIkP整理ppt59 五、

32、卡爾曼濾波器的應用五、卡爾曼濾波器的應用 1. 系統(tǒng)矩陣系統(tǒng)矩陣 假定系統(tǒng)矩陣是四維矩陣，即距離、速度、方位角及其變假定系統(tǒng)矩陣是四維矩陣，即距離、速度、方位角及其變化率化率, 它們分別由它們分別由R， ，和和 表示，距離方向上的加速度和表示，距離方向上的加速度和角度方向的加速度分別由角度方向的加速度分別由ur(k)和和u(k)表示。狀態(tài)方程為表示。狀態(tài)方程為 )()()()() 1()()() 1()()()()() 1()()() 1(21kukkTukkkTkkkukRkTukRkRkRTkRkRrR整理ppt60則系統(tǒng)方程為則系統(tǒng)方程為 )(0)(010001000010001) 1(

33、) 1() 1() 1()()() 1(2143214321kukuxxxxTTkxkxkxkxkWkAXkX用標準符號用標準符號x1，x2 ，x3, x4分別表示分別表示 R，R, , 。式中，。式中， A為系統(tǒng)為系統(tǒng)矩陣，矩陣，W(k)為噪聲項。為噪聲項。 .整理ppt61 2. 觀測矩陣觀測矩陣 假定觀測值只有距離和方位兩個，即假定觀測值只有距離和方位兩個，即R和和，分別用，分別用z1和和z2來表示。它們是由狀態(tài)值和測量噪聲組成的，且測量噪聲是來表示。它們是由狀態(tài)值和測量噪聲組成的，且測量噪聲是相互獨立的零均值的白噪聲。相互獨立的零均值的白噪聲。 測量方程：測量方程： )()()()()()(2211kvkkzkvkrkz則有則有 )()()(kVkCXkZ整理ppt62)()()()()()(01000001)()(21432121kvkvkxkxkxkxkzkz其中，其中， x1(k)=r(k)，x3(k)=(k)。 以上兩個問題實際上是建立模型問題。以上兩個問題實際上是建立模型問題。 整理ppt63 3. 觀測噪聲協(xié)方差矩陣觀測噪聲協(xié)方差矩陣 在計算濾波器增益時，需知觀測噪聲的協(xié)方差矩陣。由于在計算濾波器增益時，需知觀測噪聲的協(xié)方差矩陣。由于只有兩個參數(shù)，因此只有兩個參數(shù)，因此 22221