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

1、自適應α-β濾波和自適應Kalman濾波均改善對機動目標的跟蹤能力。

2、擴展Kalman濾波針對卡爾曼濾波在笛卡兒坐標系中才能使用的局限而提出。5第五頁，共六十九頁，2022年，8月28日卡爾曼濾波器卡爾曼濾波器的應用:通信、雷達、導航、自動控制等領域；航天器的軌道計算、雷達目標跟蹤、生產過程的自動控制等。6第六頁，共六十九頁，2022年，8月28日卡爾曼濾波器的應用特點對機動目標跟蹤中具有良好的性能；為最佳估計并能夠進行遞推計算；只需當前的一個測量值和前一個采樣周期的預測值就能進行狀態估計。7第七頁，共六十九頁，2022年，8月28日卡爾曼濾波器的局限性卡爾曼濾波器解決運動目標或實體的狀態估計問題時，動態方程和測量方程均為線性。8第八頁，共六十九頁，2022年，8月28日一、數字濾波器作估值器1、非遞歸估值器

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

zk=x+nk

式中：x—恒定信號或稱被估參量

nk—觀測噪聲采樣

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

h1,h2,…,hm是濾波器的脈沖響應hj的采樣，或稱濾波器的加權系數。濾波器的輸出當h1=h2=…=hm=1/m時，

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

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

最后得：

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

a為濾波器的加權系數，a<1。15第十五頁，共六十九頁，2022年，8月28日遞歸數字濾波器是一種帶有反饋的濾波器，它有無限的脈沖響應，有階數少的優點，但其暫態過程較長。關于信號和噪聲的基本假設與非遞歸情況相同。上圖給出的一階遞歸濾波器輸入輸出信號關系如下：式中，zk與非遞歸情況相同；a是一個小于1的濾波器加權系數，如果它大于或等于1，該濾波器就不穩定了。

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

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

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

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

18第十八頁，共六十九頁，2022年，8月28日二、線性均方估計1、最優非遞歸估計（標量維納濾波）2、遞歸估計19第十九頁，共六十九頁，2022年，8月28日

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

hj與非最優情況的不同，這里的濾波器的加權系數為21第二十一頁，共六十九頁，2022年，8月28日

2、由最優非遞推估計導出遞歸估計由前可知，非遞歸估值器可以表示為

條件與前面相同。對k+1次取樣，相應的估計量

相應的估計誤差

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

所以有

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

最后有

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

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

1）信號模型設要估計的隨機信號為由均值為0，方差為σ2w的白噪聲激勵的一個一階遞歸過程，即信號對時間變化滿足動態方程：x(k)=ax(k-1)+w(k-1)

式中，a——系統參數；

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

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

自相關函數

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

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

32第三十二頁，共六十九頁，2022年，8月28日最優遞推估值器的信號和觀測模型33第三十三頁，共六十九頁，2022年，8月28日2、標量卡爾曼濾波器由前將遞歸估計的形式寫成：均方誤差

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

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

其中,

均方誤差

對于給定的信號模型和觀測模型，上述一組方程便稱為一維標量卡爾曼濾波器，其結構如圖所示。35第三十五頁，共六十九頁，2022年，8月28日標量卡爾曼濾波器結構

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

3、標量卡爾曼預測器

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

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

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

進一步可求出：

其中，

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

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

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

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

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

最后，有

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

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

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

其中，

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

最后,有

即可寫成一階向量的形式。在對信號向量進行估計的過程中，同時產生r個含有噪聲的測量值，記作z1(k)，z2(k)，…,zr(k)。則得到一組觀測方程:

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

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

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

2、向量問題的表示根據前面的討論，我們完全可以把前面的信號模型動態方程和觀測方程寫成如下形式:采用標量運算和矩陣運算的等價關系，推廣到多維情況：48第四十八頁，共六十九頁，2022年，8月28日據此，可以將觀測噪聲的方差變成協方差矩陣

對兩個信號的情況，則有

同理，也可以把系統噪聲的方差變成協方差矩陣，即

由于系統噪聲采樣互不相關，該協方差矩陣的非對角線元素的值均為零。單一信號均方誤差也可變成協方差矩陣，

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

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

式中，

實際上，它是預測協方差。

誤差協方差矩陣：

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

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

增益矩陣計算流程

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

它們與標量的情況是一一對應的，只是用G(k)代替了β(k)。就可以將濾波和預測用同一個方框圖表示出來。

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

5、總結卡爾曼濾波器應用廣泛，這里只對其進行簡單歸納。

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

位置測量模型：

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

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

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

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

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

預測方程:

狀態估計:

卡爾曼濾波器增益:

57第五十七頁，共六十九頁，2022年，8月28日預測協方差:

估計協方差:

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

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

用標準符號x1，x2

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

2.觀測矩陣