狀態(tài)估計卡爾曼濾波

狀態(tài)估計卡爾曼濾波第1頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)2狀態(tài)估計的主要內(nèi)容

應(yīng)用：通過數(shù)學(xué)方法尋求與觀測數(shù)據(jù)最佳擬合的狀態(tài)向量。1、確定運(yùn)動目標(biāo)的當(dāng)前位置與速度；2、確定運(yùn)動目標(biāo)的未來位置與速度；3、確定運(yùn)動目標(biāo)的固有特征或特征參數(shù)。狀態(tài)估計主要內(nèi)容：位置與速度估計位置估計：距離、方位和高度或仰角的估計速度估計：速度、加速度估計第2頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)31、α-β濾波2、α-β-γ濾波3、卡爾曼濾波這些方法針對勻速或勻加速目標(biāo)提出，如目標(biāo)真實運(yùn)動與采用的目標(biāo)模型不致，濾波器發(fā)散。狀態(tài)估計的主要方法：：算法的改進(jìn)及適應(yīng)性狀態(tài)估計難點：機(jī)動目標(biāo)的跟蹤1、自適應(yīng)α-β濾波和自適應(yīng)Kalman濾波均改善對機(jī)動目標(biāo)的跟蹤能力。2、擴(kuò)展Kalman濾波應(yīng)用于時間非線性的動態(tài)系統(tǒng)。第3頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)4卡爾曼濾波器卡爾曼濾波器的應(yīng)用:通信、雷達(dá)、導(dǎo)航、自動控制等領(lǐng)域航天器的軌道計算、雷達(dá)目標(biāo)跟蹤、生產(chǎn)過程的自動控制等：卡爾曼濾波器的應(yīng)用特點對機(jī)動目標(biāo)跟蹤中具有良好的性能為最佳估計并能夠進(jìn)行遞歸計算只需當(dāng)前的一個測量值和前一個采樣周期的預(yù)測值就能進(jìn)行狀態(tài)估計卡爾曼濾波器的局限性：卡爾曼濾波器解決運(yùn)動目標(biāo)或?qū)嶓w的狀態(tài)估計問題時，動態(tài)方程和測量方程均為線性。第4頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)5一、數(shù)字濾波器作估值器

1、非遞歸估值器2、遞歸估值器第5頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)61、非遞歸估值器

采樣平均估值器：采用時域分析方法在摻雜有噪聲的測量信號中估計信號x。第6頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)7根據(jù)數(shù)字信號處理技術(shù)，所謂非遞歸數(shù)字濾波器是一種只有前饋而沒有反饋的濾波器。

zk=x+nk

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

nk—觀測噪聲采樣假定，E(x)=x0，D(x)=σ2x，E(nk)=0，E(n2k)=σ2n。假定用zk表示觀測值第7頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)8h1,h2,…,hm是濾波器的脈沖響應(yīng)hj的采樣，或稱濾波器的加權(quán)系數(shù)。濾波器的輸出h1=h2=…=hm=1/m

該式表明，估計是用m個采樣值的平均值作為被估參量x的近似值的，故稱其為采樣平均估值器。第8頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)9均方誤差估計第9頁，共67頁，2023年，2月20日，星期日①估計值是用m個采樣值的平均值作為被估參量x的近似值；智能信息處理技術(shù)10結(jié)論②估值器的均方誤差隨著m的增加而減少；③該估值器是一個無偏估值器第10頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)112、遞歸估值器一階遞歸估值器：

a為濾波器的加權(quán)系數(shù)，a<1。一階遞歸濾波器輸入輸出信號關(guān)系式中，zk與非遞歸情況相同；a是一個小于1的濾波器加權(quán)系數(shù)，如果它大于或等于1，該濾波器就不穩(wěn)定了。

第11頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)12k時刻的輸出：yk=ak-1z1+ak-2z2+…+azk-1+zk

將zk中的信號和噪聲分開，并代入，有輸出由于│a│＜1，故隨著k值的增加，yk趨近于x/(1-a)。這樣，如果以(1-a)yk作為x的估計值，則第12頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)13當(dāng)k值較大時，估值的均方誤差而一次取樣的均方誤差故這一結(jié)果的均方誤差約為一次采樣的（1-a）/（1+a）倍。第13頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)14二、線性均方估計1、最優(yōu)非遞歸估計2、遞歸估計第14頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)15

1.最優(yōu)非遞歸估計

非遞歸濾波器的估計值及其估計誤差可分別表示為m個參數(shù)逐一求導(dǎo)，令等于零均值為零的白噪聲b=σ2n/σ2x在b<<m時最優(yōu)非遞歸估計近似于采樣平均在噪聲方差σ2n較大時性能明顯優(yōu)于非最佳情況這種最小均方誤差準(zhǔn)則下的線性濾波，通常稱作標(biāo)量維納濾波。第15頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)16

2、由最優(yōu)非遞歸估計導(dǎo)出遞歸估計由前可知，非遞歸估值器可以表示為k+1次取樣誤差估計誤差估計第16頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)17b=σ2n/σ2x及hi(k)=1/（k+b）第17頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)18分成二項

第一項同時乘、除一個bkOR第18頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)19最優(yōu)遞歸估計器遞歸公式第19頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)20最優(yōu)遞歸估計器遞歸公式第20頁，共67頁，2023年，2月20日，星期日為使最佳，遞推初始條件智能信息處理技術(shù)21若E(x)=0從零開始遞推應(yīng)用時要注意初始條件，即遞推開始時的初始值第21頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)22三、標(biāo)量卡爾曼濾波器

主要作用：對摻雜有噪聲的隨機(jī)信號進(jìn)行線性估計。第22頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)231、模型

x(k)=ax(k-1)+w(k-1)

如果令x(0)=0，E［w(k)］=0，則1）信號模型設(shè)要估計的隨機(jī)信號為由均值為0，方差為σ2w的白噪聲激勵的一個一階遞歸過程，即信號對時間變化滿足動態(tài)方程：式中，a——系統(tǒng)參數(shù)

w(k-1)——白噪聲采樣。第23頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)24x(k)的均值和方差分別為：自相關(guān)函數(shù)第24頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)25觀測模型由下式給出：z(k)=cx(k)+v(k)式中：c——測量因子；

v(k)——E(·)=0，

D(·)=σ2n的白噪聲。2）觀測模型第25頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)262、標(biāo)量卡爾曼濾波器

均方誤差分別對a(k)和b(k)求導(dǎo)，并令其等于0，求其最佳估計，得出a(k)與b(k)的關(guān)系?a(k)=a［1-cb(k)］最后有遞歸估值器：由前將遞歸估計的形式寫成：第26頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)27濾波器增益b(k)？均方誤差對于給定的信號模型和觀測模型，上述一組方程便稱為一維標(biāo)量卡爾曼濾波器，其結(jié)構(gòu)如圖所示。第27頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)28標(biāo)量卡爾曼濾波器結(jié)構(gòu)

第28頁，共67頁，2023年，2月20日，星期日標(biāo)量卡爾曼濾波是對摻雜有噪聲的隨機(jī)信號進(jìn)行線性估計。但經(jīng)常要對信號的未來值進(jìn)行預(yù)測，特別是在控制系統(tǒng)中。根據(jù)預(yù)測提前時間的多少，把預(yù)測分成1步、2步、…、m步預(yù)測，通常把1步預(yù)測記作。預(yù)測的步數(shù)越多，誤差越大。這里討論1步預(yù)測問題。智能信息處理技術(shù)29

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

信號模型和觀測模型同前：第29頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)30根據(jù)前一節(jié)，有一步線性預(yù)測遞推公式：其中，α(k)和β(k)可以通過使預(yù)測均方誤差最小來確定。預(yù)測的均方誤差可表示為將預(yù)測方程代入該式，并求導(dǎo)，就會得到一組正交方程：第30頁，共67頁，2023年，2月20日，星期日a(k)=？智能信息處理技術(shù)31解之，得a(k)=a-cβ(k)將其代入預(yù)測方程，有進(jìn)一步可求出：由以上表達(dá)式可以看出，可根據(jù)預(yù)測均方誤差Pε(k/k-1)計算β(k)，然后再給出Pε(k+1/k)的預(yù)測均方誤差。第31頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)32最優(yōu)一步預(yù)測器第32頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)33最優(yōu)一步預(yù)測及濾波器第33頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)34

四、向量卡爾曼濾波器

每個wα過程都是白的，零均值的，與其它過程的采樣是獨立的。于是把q個信號與q個白噪聲組成的q維向量分別表示成1、信號向量和數(shù)據(jù)向量如果要求對q個信號進(jìn)行同時估計，這q個信號在k時刻的采樣值記作x1(k)、x2(k)、…、xq(k)。假設(shè)每個信號都是由一階自回歸過程產(chǎn)生的，即第α個信號在時刻k的采樣值為:xα(k)=aαxα(k-1)+wα(k-1)α=1，2，…，q

第34頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)35顯然X(k)=AX(k-1)+W(k-1)如果信號不滿足一階遞歸差分方程，而滿足二階遞歸差分方程，即x(k)=ax(k-1)+bx(k-2)+w(k-1)

q維向量qq第35頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)36定義兩個分量x1(k)=x(k)x2(k)=x1(k-1)=x(k-1)X(k)=AX(k-1)+W(k-1)結(jié)果把一個二階差分方程變成了一個一階二維向量方程，該方程用起來更簡單方便。第36頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)37用R(k)表示k時刻的距離，R(k)表示k時刻的速度，U(k)表示k時刻的加速度，T表示采樣周期，則寫成一般形式：.第37頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)38寫成向量形式：在對信號向量進(jìn)行估計的過程中，同時產(chǎn)生r個含有噪聲的測量值，記作z1(k)，

z2(k)，…,zr(k)。則得到一組觀測方程:一階向量形式第38頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)39Z(k)=CX(k)+V(k)

C即是觀測矩陣。rq維向量r×q階矩陣第i個測量參數(shù)ifr==q附加噪聲第39頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)40

2、向量問題的表示

采用標(biāo)量運(yùn)算和矩陣運(yùn)算的等價關(guān)系，推廣到多維情況：根據(jù)前面的討論，我們完全可以把前面的信號模型動態(tài)方程和觀測方程寫成如下形式:第40頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)41據(jù)此，可以將觀測噪聲的方差變成協(xié)方差矩陣對兩個信號的情況，則有同理，也可以把系統(tǒng)噪聲的方差變成協(xié)方差矩陣，即由于系統(tǒng)噪聲采樣互不相關(guān)，該協(xié)方差矩陣的非對角線元素的值均為零。單一信號均方誤差也可變成協(xié)方差矩陣，第41頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)42

3、向量卡爾曼濾波器

濾波器增益：實際上，它是預(yù)測協(xié)方差。誤差協(xié)方差矩陣：利用前面的概念，直接把標(biāo)量卡爾曼濾波器公式變成向量卡爾曼濾波器公式：第42頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)43向量卡爾曼濾波器結(jié)構(gòu)第43頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)44增益矩陣K(k)的計算流程如圖所示：增益矩陣計算流程

P(k-1)根據(jù)k-1時刻的誤差協(xié)方差矩陣p(k-1)計算預(yù)測協(xié)方差p1(k),進(jìn)而計算得到濾波器增益K(k)，通過濾波器增益K(k)，預(yù)測協(xié)方差p1(k),計算出k時刻的誤差協(xié)方差矩陣p(k)，為計算k+1時刻的p1(k+1)創(chuàng)造了條件。這樣就形成了一個遞歸計算流程，可以計算不同時刻的增益矩陣。第44頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)45

4、向量卡爾曼預(yù)測器

預(yù)測增益:預(yù)測均方誤差:它們與標(biāo)量的情況是一一對應(yīng)的，只是用G(k)代替了β(k)。就可以將濾波和預(yù)測用同一個方框圖表示出來。根據(jù)相同的推導(dǎo)方法，可以獲得卡爾曼預(yù)測器方程組。預(yù)測方程：第45頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)46

5、總結(jié)

卡爾曼濾波器應(yīng)用廣泛，這里只對其進(jìn)行簡單歸納?？柭鼮V波器的主要特性卡爾曼濾波器是一個遞歸、線性、無偏和方差最小的濾波器，如果過程噪聲和觀測噪聲是正態(tài)高斯白噪聲，則它保持最佳特性。第46頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)47卡爾曼濾波器模型

位置測量模型：目標(biāo)運(yùn)動模型：第47頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)48狀態(tài)方程: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］第48頁，共67頁，2023年，2月20日，星期日假定系統(tǒng)矩陣是四維矩陣，即距離、速度、方位角及其變化率,它們分別由R，，θ和表示，距離方向上的加速度和角度方向的加速度分別由ur(k)和uθ(k)表示。狀態(tài)方程為智能信息處理技術(shù)49

五、卡爾曼濾波器的應(yīng)用

1.系統(tǒng)矩陣第49頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)50則系統(tǒng)方程為用標(biāo)準(zhǔn)符號x1(k)，x2(k)，x3(k),x4(k)分別表、、、。式中，A為系統(tǒng)矩陣，W(k)為噪聲項。.第50頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)51

2.觀測矩陣假定觀測值只有距離和方位兩個，即R和θ，分別用z1和z2來表示。它們是由狀態(tài)值和測量噪聲組成的，且測量噪聲是相互獨立的零均值的白噪聲。測量方程第51頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)52x1(k)=R(k)x3(k)=θ(k)以上兩個問題實際上是建立模型問題。第52頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)53

3.觀測噪聲協(xié)方差矩陣在計算濾波器增益時，需知觀測噪聲的協(xié)方差矩陣。由于只有兩個參數(shù)，因此方位和距離觀測噪聲相互獨立的條件第53頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)54

4.系統(tǒng)噪聲協(xié)方差矩陣假定目標(biāo)作勻速運(yùn)動，由于大氣湍流等因素的影響，目標(biāo)產(chǎn)生隨機(jī)加速度，在距離和方位上都存在隨機(jī)擾動,于是有and系統(tǒng)噪聲的協(xié)方差矩陣第54頁，共67頁，2023年，2月20日，星期日濾波器初始化時,先利用一種比較簡單的方法確定，可利用時刻1和時刻2兩點的距離和方位測量值，即z1(1)，z1(2)，z2(1)，z2(2)，建立，而忽略隨機(jī)加速度。智能信息處理技術(shù)55

5.濾波器的初值第55頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)566.均方誤差矩陣濾波器初值誤差矢量第56頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)57初始誤差的協(xié)方差矩陣u，v相互獨立，均值為0，各噪聲采樣之間獨立條件迭代所需參數(shù)第57頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)58

常系數(shù)α-β和α-β-γ濾波器對于勻速和勻加速運(yùn)動的目標(biāo)，有目標(biāo)運(yùn)動模型式中，w(k)為均值為0、方差為σ2的高斯白噪聲，T為對目標(biāo)的采樣周期。一、目標(biāo)運(yùn)動模型第58頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)59

二、常系數(shù)α-β和α-β-γ濾波器

預(yù)測方程：濾波方程：常系數(shù)α-β濾波器定義如下對于勻速運(yùn)動的目標(biāo)，可以采用均方誤差最小的準(zhǔn)則進(jìn)行濾波和預(yù)測，即α-β和α-β-γ濾波器。第59頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)60濾波方程和預(yù)測方程也可以分別寫成如下形式：第60頁，共67頁，2023年，2月20日，星期日智能信息處理技術(shù)61常系數(shù)α-β-γ濾波器定義如下：預(yù)測方程：

濾波方程：第61頁，共67頁，2023年，2月20日，星期日智能信息處理