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

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

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

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

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

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

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

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

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

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

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

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

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

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

14、0 xEX 2xPbbXnx102200整理ppt29三、標(biāo)量卡爾曼濾波器時(shí)變信號(hào)三、標(biāo)量卡爾曼濾波器時(shí)變信號(hào) 主要作用：主要作用： 對(duì)摻雜有噪聲的隨機(jī)信號(hào)進(jìn)行線性估計(jì)。對(duì)摻雜有噪聲的隨機(jī)信號(hào)進(jìn)行線性估計(jì)。整理ppt30 1、模型、模型 1） 信號(hào)模型信號(hào)模型 設(shè)要估計(jì)的隨機(jī)信號(hào)為由均值為設(shè)要估計(jì)的隨機(jī)信號(hào)為由均值為0，方差為，方差為2w的白噪聲激的白噪聲激勵(lì)的一個(gè)一階遞歸過(guò)程，即信號(hào)對(duì)時(shí)間變化滿(mǎn)足動(dòng)態(tài)方程：勵(lì)的一個(gè)一階遞歸過(guò)程，即信號(hào)對(duì)時(shí)間變化滿(mǎn)足動(dòng)態(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該過(guò)程該過(guò)程 稱(chēng)作一階自回歸過(guò)程。稱(chēng)作一階自回歸過(guò)程。x(k)的均值和方差分別為：的均值和方差分別為： 22221)0()()(0)(aPkxEkxDkxEwxx自相關(guān)函數(shù)自相關(guān)函數(shù) )0()()()(|xjxPajPjkxkxE整理ppt322） 觀測(cè)模型觀測(cè)模型觀測(cè)模型由下式給出：觀測(cè)模型由下式給出： z(k)=cx(k)+v(k) 式中：式中：c測(cè)量因子；測(cè)量因子； v(k)E()=0，D()=2n的白噪聲。的白噪聲。最優(yōu)遞推估值器的信號(hào)和觀測(cè)模型如圖所示。最優(yōu)遞推估值器的信號(hào)和觀測(cè)模型如圖所示。 整理p

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

17、ckzkbkXakX整理ppt35b(k)為濾波器增益為濾波器增益 12121)()()(nkPckcPkb其中其中, 221) 1()(wkPakP均方誤差均方誤差 )(1)(2kbckPn 對(duì)于給定的信號(hào)模型和觀測(cè)模型，上述一組方程便稱(chēng)為一對(duì)于給定的信號(hào)模型和觀測(cè)模型，上述一組方程便稱(chēng)為一維標(biāo)量卡爾曼濾波器，其結(jié)構(gòu)如圖所示。維標(biāo)量卡爾曼濾波器，其結(jié)構(gòu)如圖所示。 整理ppt36標(biāo)量卡爾曼濾波器結(jié)構(gòu)標(biāo)量卡爾曼濾波器結(jié)構(gòu) b(k)z1acz(k)(kX整理ppt37 3、標(biāo)量卡爾曼預(yù)測(cè)器、標(biāo)量卡爾曼預(yù)測(cè)器 標(biāo)量卡爾曼濾波是對(duì)摻雜有噪聲的隨機(jī)信號(hào)進(jìn)行線性估計(jì)。標(biāo)量卡爾曼濾波是對(duì)摻雜有噪聲的隨機(jī)信號(hào)

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

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

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

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

22、，X(k-1)，W(k-1)都是都是q維向量，維向量，A是個(gè)是個(gè)qq階矩陣，即階矩陣，即 qaaaA00000021 如果信號(hào)不滿(mǎn)足一階遞歸差分方程，而滿(mǎn)足二階遞歸差分如果信號(hào)不滿(mǎn)足一階遞歸差分方程，而滿(mǎn)足二階遞歸差分方程，即方程，即 x(k)=ax(k-1)+bx(k-2)+w(k-1) 整理ppt44定義兩個(gè)分量定義兩個(gè)分量 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) 結(jié)果把一個(gè)二階差分方程變成了一個(gè)一階二維向量方程，結(jié)果把一個(gè)二

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

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

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

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

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

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

29、1/()(1TTkAKkRCkkCPCkkAPkG預(yù)測(cè)均方誤差預(yù)測(cè)均方誤差: )() 1/()()/1(TkQAkkPCkGAkkP 它們與標(biāo)量的情況是一一對(duì)應(yīng)的，只是用它們與標(biāo)量的情況是一一對(duì)應(yīng)的，只是用G(k)代替了代替了(k)。就可以將濾波和預(yù)測(cè)用同一個(gè)方框圖表示出來(lái)。就可以將濾波和預(yù)測(cè)用同一個(gè)方框圖表示出來(lái)。 整理ppt54 5、總結(jié)、總結(jié) 卡爾曼濾波器應(yīng)用廣泛，卡爾曼濾波器應(yīng)用廣泛， 這里只對(duì)其進(jìn)行簡(jiǎn)單歸納。這里只對(duì)其進(jìn)行簡(jiǎn)單歸納。 1） 卡爾曼濾波器的主要特性卡爾曼濾波器的主要特性 卡爾曼濾波器是一個(gè)遞歸、卡爾曼濾波器是一個(gè)遞歸、 線性、無(wú)偏和方差最小的濾波線性、無(wú)偏和方差最小的濾

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

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

32、卡爾曼濾波器的應(yīng)用五、卡爾曼濾波器的應(yīng)用 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用標(biāo)準(zhǔn)符號(hào)用標(biāo)準(zhǔn)符號(hào)x1，x2 ，x3, x4分別表示分別表示 R，R, , 。式中，。式中， A為系統(tǒng)為系統(tǒng)矩陣，矩陣，W(k)為噪聲項(xiàng)。為噪聲項(xiàng)。 .整理ppt61 2. 觀測(cè)矩陣觀測(cè)矩陣 假定觀測(cè)值只有距離和方位兩個(gè)，即假定觀測(cè)值只有距離和方位兩個(gè)，即R和和，分別用，分別用z1和和z2來(lái)表示。它們是由狀態(tài)值和測(cè)量噪聲組成的，且測(cè)量噪聲是來(lái)表示。它們是由狀態(tài)值和測(cè)量噪聲組成的，且測(cè)量噪聲是相互獨(dú)立的零均值的白噪聲。相互獨(dú)立的零均值的白噪聲。 測(cè)量方程：測(cè)量方程： )()()()()()(2211kvkkzkvkrkz則有則有 )()()(kVkCXkZ整理ppt62)()()()()()(01000001)()(21432121kvkvkxkxkxkxkzkz其中，其中， x1(k)=r(k)，x3(k)=(k)。 以上兩個(gè)問(wèn)題實(shí)際上是建立模型問(wèn)題。以上兩個(gè)問(wèn)題實(shí)際上是建立模型問(wèn)題。 整理ppt63 3. 觀測(cè)噪聲協(xié)方差矩陣觀測(cè)噪聲協(xié)方差矩陣 在計(jì)算濾波器增益時(shí)，需知觀測(cè)噪聲的協(xié)方差矩陣。由于在計(jì)算濾波器增益時(shí)，需知觀測(cè)噪聲的協(xié)方差矩陣。由于只有兩個(gè)參數(shù)，因此只有兩個(gè)參數(shù)，因此 22221