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

信息融合狀態(tài)估計(jì)卡爾曼濾波1第一頁，共六十九頁，2022年，8月28日狀態(tài)估計(jì)的主要內(nèi)容應(yīng)用：通過數(shù)學(xué)方法尋求與觀測(cè)數(shù)據(jù)最佳擬合的狀態(tài)向量。1、確定運(yùn)動(dòng)目標(biāo)的當(dāng)前位置與速度；2、確定運(yùn)動(dòng)目標(biāo)的未來位置與速度；3、確定運(yùn)動(dòng)目標(biāo)的固有特征或特征參數(shù)。2第二頁，共六十九頁，2022年，8月28日狀態(tài)估計(jì)主要內(nèi)容：位置與速度估計(jì)。位置估計(jì)：距離、方位和高度或仰角的估計(jì)；速度估計(jì)：速度、加速度估計(jì)。3第三頁，共六十九頁，2022年，8月28日狀態(tài)估計(jì)的主要方法1、α-β濾波2、α-β-γ濾波3、卡爾曼濾波這些方法針對(duì)勻速或勻加速目標(biāo)提出，如目標(biāo)真實(shí)運(yùn)動(dòng)與采用的目標(biāo)模型不一致，濾波器發(fā)散。4第四頁，共六十九頁，2022年，8月28日算法的改進(jìn)及適應(yīng)性狀態(tài)估計(jì)難點(diǎn)：機(jī)動(dòng)目標(biāo)的跟蹤

1、自適應(yīng)α-β濾波和自適應(yīng)Kalman濾波均改善對(duì)機(jī)動(dòng)目標(biāo)的跟蹤能力。

2、擴(kuò)展Kalman濾波針對(duì)卡爾曼濾波在笛卡兒坐標(biāo)系中才能使用的局限而提出。5第五頁，共六十九頁，2022年，8月28日卡爾曼濾波器卡爾曼濾波器的應(yīng)用:通信、雷達(dá)、導(dǎo)航、自動(dòng)控制等領(lǐng)域；航天器的軌道計(jì)算、雷達(dá)目標(biāo)跟蹤、生產(chǎn)過程的自動(dòng)控制等。6第六頁，共六十九頁，2022年，8月28日卡爾曼濾波器的應(yīng)用特點(diǎn)對(duì)機(jī)動(dòng)目標(biāo)跟蹤中具有良好的性能；為最佳估計(jì)并能夠進(jìn)行遞推計(jì)算；只需當(dāng)前的一個(gè)測(cè)量值和前一個(gè)采樣周期的預(yù)測(cè)值就能進(jìn)行狀態(tài)估計(jì)。7第七頁，共六十九頁，2022年，8月28日卡爾曼濾波器的局限性卡爾曼濾波器解決運(yùn)動(dòng)目標(biāo)或?qū)嶓w的狀態(tài)估計(jì)問題時(shí)，動(dòng)態(tài)方程和測(cè)量方程均為線性。8第八頁，共六十九頁，2022年，8月28日一、數(shù)字濾波器作估值器1、非遞歸估值器

2、遞歸估值器9第九頁，共六十九頁，2022年，8月28日1、非遞歸估值器采樣平均估值器：采用時(shí)域分析方法在摻雜有噪聲的測(cè)量信號(hào)中估計(jì)信號(hào)x。10第十頁，共六十九頁，2022年，8月28日根據(jù)數(shù)字信號(hào)處理我們知道，所謂非遞歸數(shù)字濾波器是一種只有前饋而沒有反饋的濾波器，它的沖擊脈沖響應(yīng)是有限的，在許多領(lǐng)域有著廣泛的應(yīng)用。假定用zk表示觀測(cè)值，

zk=x+nk

式中：x—恒定信號(hào)或稱被估參量

nk—觀測(cè)噪聲采樣

假定，E(x)=x0，D(x)=σ2x，E(nk)=0，E(n2k)=σ2n。11第十一頁，共六十九頁，2022年，8月28日

h1,h2,…,hm是濾波器的脈沖響應(yīng)hj的采樣，或稱濾波器的加權(quán)系數(shù)。濾波器的輸出當(dāng)h1=h2=…=hm=1/m時(shí)，

該式表明，估計(jì)是用m個(gè)采樣值的平均值作為被估參量x的近似值的，故稱其為采樣平均估值器。

12第十二頁，共六十九頁，2022年，8月28日估計(jì)的均方誤差以Pε表示，有當(dāng)i=j時(shí)δij=1，當(dāng)i≠j時(shí)δij=0，有

最后得：

13第十三頁，共六十九頁，2022年，8月28日結(jié)論①估計(jì)值是用m個(gè)采樣值的平均值作為被估參量x的近似值；②估值器的均方誤差隨著m的增加而減少；③該估值器是一個(gè)無偏估值器。14第十四頁，共六十九頁，2022年，8月28日2、遞歸估值器一階遞歸估值器：

a為濾波器的加權(quán)系數(shù)，a<1。15第十五頁，共六十九頁，2022年，8月28日遞歸數(shù)字濾波器是一種帶有反饋的濾波器，它有無限的脈沖響應(yīng)，有階數(shù)少的優(yōu)點(diǎn)，但其暫態(tài)過程較長(zhǎng)。關(guān)于信號(hào)和噪聲的基本假設(shè)與非遞歸情況相同。上圖給出的一階遞歸濾波器輸入輸出信號(hào)關(guān)系如下：式中，zk與非遞歸情況相同；a是一個(gè)小于1的濾波器加權(quán)系數(shù)，如果它大于或等于1，該濾波器就不穩(wěn)定了。

16第十六頁，共六十九頁，2022年，8月28日k時(shí)刻的輸出：

yk=ak-1z1+ak-2z2+…+azk-1+zk

將zk中的信號(hào)和噪聲分開，并代入，有輸出由于│a│＜1，故隨著k值的增加，yk趨近于x/(1-a)。這樣，如果以(1-a)yk作為x的估計(jì)值，則17第十七頁，共六十九頁，2022年，8月28日此時(shí)信號(hào)x和估值之間只差一個(gè)噪聲項(xiàng)。當(dāng)k值較大時(shí)，估值的均方誤差而一次取樣的均方誤差

故上一結(jié)果的均方誤差約為一次采樣的（1-a）/（1+a）倍。

18第十八頁，共六十九頁，2022年，8月28日二、線性均方估計(jì)1、最優(yōu)非遞歸估計(jì)（標(biāo)量維納濾波）2、遞歸估計(jì)19第十九頁，共六十九頁，2022年，8月28日

1.最優(yōu)非遞歸估計(jì)非遞歸濾波器的估計(jì)值及其估計(jì)誤差可分別表示為20第二十頁，共六十九頁，2022年，8月28日對(duì)m個(gè)參數(shù)逐一求導(dǎo)，令等于零，在均值為零的白噪聲的情況下，可得到最小均方誤差和估計(jì)：其中，b=σ2n/σ2x，在b<<m時(shí)，這種估計(jì)近似于采樣平均。在噪聲方差σ2n較大時(shí)，其性能明顯優(yōu)于非最佳情況。這種最小均方誤差準(zhǔn)則下的線性濾波，通常稱作標(biāo)量維納濾波。

hj與非最優(yōu)情況的不同，這里的濾波器的加權(quán)系數(shù)為21第二十一頁，共六十九頁，2022年，8月28日

2、由最優(yōu)非遞推估計(jì)導(dǎo)出遞歸估計(jì)由前可知，非遞歸估值器可以表示為

條件與前面相同。對(duì)k+1次取樣，相應(yīng)的估計(jì)量

相應(yīng)的估計(jì)誤差

22第二十二頁，共六十九頁，2022年，8月28日由b=σ2n/σ2x及hi(k)=1/（k+b），有

所以有

23第二十三頁，共六十九頁，2022年，8月28日于是,分成二項(xiàng)：將第一項(xiàng)同時(shí)乘、除一個(gè)bk，則24第二十四頁，共六十九頁，2022年，8月28日或

最后有

25第二十五頁，共六十九頁，2022年，8月28日最優(yōu)遞歸估計(jì)器遞推公式26第二十六頁，共六十九頁，2022年，8月28日最優(yōu)遞歸估計(jì)器遞推公式27第二十七頁，共六十九頁，2022年，8月28日

遞推開始時(shí)的初始條件應(yīng)滿足：以使為最佳值。解之，得 ，這時(shí)的 如果E(x)=0，可從零開始遞推運(yùn)算，即28第二十八頁，共六十九頁，2022年，8月28日三、標(biāo)量卡爾曼濾波器－時(shí)變信號(hào)主要作用：對(duì)摻雜有噪聲的隨機(jī)信號(hào)進(jìn)行線性估計(jì)。29第二十九頁，共六十九頁，2022年，8月28日1、模型

1）信號(hào)模型設(shè)要估計(jì)的隨機(jī)信號(hào)為由均值為0，方差為σ2w的白噪聲激勵(lì)的一個(gè)一階遞歸過程，即信號(hào)對(duì)時(shí)間變化滿足動(dòng)態(tài)方程：x(k)=ax(k-1)+w(k-1)

式中，a——系統(tǒng)參數(shù)；

w(k-1)——白噪聲采樣。如果令x(0)=0，E［w(k)］=0，則

30第三十頁，共六十九頁，2022年，8月28日該過程稱作一階自回歸過程。x(k)的均值和方差分別為：

自相關(guān)函數(shù)

31第三十一頁，共六十九頁，2022年，8月28日2）觀測(cè)模型觀測(cè)模型由下式給出：z(k)=cx(k)+v(k)式中：c——測(cè)量因子；

v(k)——E(·)=0，D(·)=σ2n的白噪聲。最優(yōu)遞推估值器的信號(hào)和觀測(cè)模型如圖所示。

32第三十二頁，共六十九頁，2022年，8月28日最優(yōu)遞推估值器的信號(hào)和觀測(cè)模型33第三十三頁，共六十九頁，2022年，8月28日2、標(biāo)量卡爾曼濾波器由前將遞歸估計(jì)的形式寫成：均方誤差

分別對(duì)a(k)和b(k)求導(dǎo)，并令其等于0，求其最佳估計(jì)，得出a(k)與b(k)的關(guān)系：a(k)=a［1-cb(k)］最后有遞歸估值器：

34第三十四頁，共六十九頁，2022年，8月28日b(k)為濾波器增益

其中,

均方誤差

對(duì)于給定的信號(hào)模型和觀測(cè)模型，上述一組方程便稱為一維標(biāo)量卡爾曼濾波器，其結(jié)構(gòu)如圖所示。35第三十五頁，共六十九頁，2022年，8月28日標(biāo)量卡爾曼濾波器結(jié)構(gòu)

36第三十六頁，共六十九頁，2022年，8月28日

3、標(biāo)量卡爾曼預(yù)測(cè)器

標(biāo)量卡爾曼濾波是對(duì)摻雜有噪聲的隨機(jī)信號(hào)進(jìn)行線性估計(jì)。但經(jīng)常要對(duì)信號(hào)的未來值進(jìn)行預(yù)測(cè)，特別是在控制系統(tǒng)中。根據(jù)預(yù)測(cè)提前時(shí)間的多少，把預(yù)測(cè)分成1步、2步、…、m步預(yù)測(cè)，通常把1步預(yù)測(cè)記作。預(yù)測(cè)的步數(shù)越多，誤差越大。這里討論1步預(yù)測(cè)問題。信號(hào)模型和觀測(cè)模型同前：37第三十七頁，共六十九頁，2022年，8月28日根據(jù)前一節(jié)，有一步線性預(yù)測(cè)遞推公式：

其中，a(k)和β(k)可以通過使均方預(yù)測(cè)誤差最小來確定。預(yù)測(cè)的均方誤差可表示為將預(yù)測(cè)方程代入該式，并求導(dǎo)，就會(huì)得到一組正交方程：38第三十八頁，共六十九頁，2022年，8月28日解之，得

a(k)=a-cβ(k)將其代入預(yù)測(cè)方程，有

進(jìn)一步可求出：

其中，

由以上表達(dá)式可以看出，可根據(jù)均方預(yù)測(cè)誤差Pε(k/k-1)計(jì)算β(k)，然后再給出Pε(k+1/k)的均方預(yù)測(cè)誤差。39第三十九頁，共六十九頁，2022年，8月28日最優(yōu)一步預(yù)測(cè)器40第四十頁，共六十九頁，2022年，8月28日最優(yōu)一步預(yù)測(cè)及濾波器41第四十一頁，共六十九頁，2022年，8月28日四、向量卡爾曼濾波器1、信號(hào)向量和數(shù)據(jù)向量如果要求對(duì)q個(gè)信號(hào)進(jìn)行同時(shí)估計(jì)，這q個(gè)信號(hào)在k時(shí)刻的采樣值記作x1(k)、x2(k)、…、xq(k)。假設(shè)每個(gè)信號(hào)都是由一階自回歸過程產(chǎn)生的，即第α個(gè)信號(hào)在時(shí)刻k的采樣值為:xα(k)=aαxα(k-1)+wα(k-1)α=1，2，…，q

每個(gè)wα過程都是白的，零均值的，與其它過程的采樣是獨(dú)立的。于是把q個(gè)信號(hào)與q個(gè)白噪聲組成的q維向量分別表示成42第四十二頁，共六十九頁，2022年，8月28日顯然，

X(k)=AX(k-1)+W(k-1)式中，X(k)，X(k-1)，W(k-1)都是q維向量，A是個(gè)q×q階矩陣，即如果信號(hào)不滿足一階遞歸差分方程，而滿足二階遞歸差分方程，即x(k)=ax(k-1)+bx(k-2)+w(k-1)

43第四十三頁，共六十九頁，2022年，8月28日定義兩個(gè)分量

x1(k)=x(k)x2(k)=x1(k-1)=x(k-1)于是，有

最后，有

X(k)=AX(k-1)+W(k-1)

結(jié)果把一個(gè)二階差分方程變成了一個(gè)一階二維向量方程，該方程用起來更簡(jiǎn)單方便。44第四十四頁，共六十九頁，2022年，8月28日

用R(k)表示k時(shí)刻的距離，R(k)表示k時(shí)刻的速度，U(k)表示k時(shí)刻的加速度，T表示采樣周期，則寫成一般形式：

其中，

.45第四十五頁，共六十九頁，2022年，8月28日寫成向量形式：

最后,有

即可寫成一階向量的形式。在對(duì)信號(hào)向量進(jìn)行估計(jì)的過程中，同時(shí)產(chǎn)生r個(gè)含有噪聲的測(cè)量值，記作z1(k)，z2(k)，…,zr(k)。則得到一組觀測(cè)方程:

46第四十六頁，共六十九頁，2022年，8月28日其中，vi(k)表示附加噪聲，ci表示第i個(gè)測(cè)量參數(shù)，于是有Z(k)=CX(k)+V(k)

式中，Z(k)，V(k)是r維向量，X(k)是q維向量，C是r×q階矩陣。對(duì)于r=q，有C即是觀測(cè)矩陣。

47第四十七頁，共六十九頁，2022年，8月28日

2、向量問題的表示根據(jù)前面的討論，我們完全可以把前面的信號(hào)模型動(dòng)態(tài)方程和觀測(cè)方程寫成如下形式:采用標(biāo)量運(yùn)算和矩陣運(yùn)算的等價(jià)關(guān)系，推廣到多維情況：48第四十八頁，共六十九頁，2022年，8月28日據(jù)此，可以將觀測(cè)噪聲的方差變成協(xié)方差矩陣

對(duì)兩個(gè)信號(hào)的情況，則有

同理，也可以把系統(tǒng)噪聲的方差變成協(xié)方差矩陣，即

由于系統(tǒng)噪聲采樣互不相關(guān)，該協(xié)方差矩陣的非對(duì)角線元素的值均為零。單一信號(hào)均方誤差也可變成協(xié)方差矩陣，

49第四十九頁，共六十九頁，2022年，8月28日

3、向量卡爾曼濾波器利用前面的概念，直接把標(biāo)量卡爾曼濾波器公式變成向量卡爾曼濾波器公式：濾波器增益：

式中，

實(shí)際上，它是預(yù)測(cè)協(xié)方差。

誤差協(xié)方差矩陣：

50第五十頁，共六十九頁，2022年，8月28日用K(k)代替了B(k)，因K(k)是通用符號(hào)，如圖：向量卡爾曼濾波器結(jié)構(gòu)

51第五十一頁，共六十九頁，2022年，8月28日增益矩陣K(k)的計(jì)算流程如圖所示：

增益矩陣計(jì)算流程

52第五十二頁，共六十九頁，2022年，8月28日4、向量卡爾曼預(yù)測(cè)器根據(jù)相同的推導(dǎo)方法，可以獲得卡爾曼預(yù)測(cè)器方程組。預(yù)測(cè)方程：預(yù)測(cè)增益:預(yù)測(cè)均方誤差:

它們與標(biāo)量的情況是一一對(duì)應(yīng)的，只是用G(k)代替了β(k)。就可以將濾波和預(yù)測(cè)用同一個(gè)方框圖表示出來。

53第五十三頁，共六十九頁，2022年，8月28日

5、總結(jié)卡爾曼濾波器應(yīng)用廣泛，這里只對(duì)其進(jìn)行簡(jiǎn)單歸納。

1）卡爾曼濾波器的主要特性卡爾曼濾波器是一個(gè)遞歸、線性、無偏和方差最小的濾波器，如果過程噪聲和觀測(cè)噪聲是正態(tài)高斯白噪聲，則它保持最佳特性。54第五十四頁，共六十九頁，2022年，8月28日2）卡爾曼濾波器模型目標(biāo)運(yùn)動(dòng)模型：

位置測(cè)量模型：

55第五十五頁，共六十九頁，2022年，8月28日狀態(tài)方程:

X(t+T)=Φ(t)X(t)+W(t)Q(t)=E［W(t)W(t)T］

觀測(cè)方程:Z(t)=HX(t)+V(t)

R(t)=E［V(t)V(t)T］

56第五十六頁，共六十九頁，2022年，8月28日3)卡爾曼濾波器方程組殘差:

預(yù)測(cè)方程:

狀態(tài)估計(jì):

卡爾曼濾波器增益:

57第五十七頁，共六十九頁，2022年，8月28日預(yù)測(cè)協(xié)方差:

估計(jì)協(xié)方差:

58第五十八頁，共六十九頁，2022年，8月28日五、卡爾曼濾波器的應(yīng)用

1.系統(tǒng)矩陣假定系統(tǒng)矩陣是四維矩陣，即距離、速度、方位角及其變化率,它們分別由R，，θ和表示，距離方向上的加速度和角度方向的加速度分別由ur(k)和uθ(k)表示。狀態(tài)方程為59第五十九頁，共六十九頁，2022年，8月28日則系統(tǒng)方程為

用標(biāo)準(zhǔn)符號(hào)x1，x2

，x3,x4分別表示R，R,θ,θ。式中，A為系統(tǒng)矩陣，W(k)為噪聲項(xiàng)。..60第六十頁，共六十九頁，2022年，8月28日

2.觀測(cè)矩陣