卡爾曼濾波器及matlab代碼

1、信息融合大作業(yè)維納最速卜降法濾波器，卡爾曼濾波器設(shè)計(jì)及Matlab仿真時(shí)間：2010-12-6專(zhuān)業(yè)：信息工程班級(jí)：09030702學(xué)號(hào)：2007302171姓名：馬志強(qiáng)1. 濾波問(wèn)題淺談估計(jì)器或?yàn)V波器這一術(shù)語(yǔ)通常用來(lái)稱(chēng)呼一個(gè)系統(tǒng)，設(shè)計(jì)這樣的系統(tǒng)是為了從 含有噪聲的數(shù)據(jù)中提取人們感興趣的，接近規(guī)定質(zhì)最的信息。由于這樣一個(gè)寬 目標(biāo)，估計(jì)理論應(yīng)用于諸如通信、雷達(dá)、聲納、導(dǎo)航、地震學(xué)、生物醫(yī)學(xué)工程、 金融工程等眾多不同的領(lǐng)域。例如，考慮一個(gè)數(shù)字通信系統(tǒng)，其基本形式由發(fā) 射機(jī)、信道和接收機(jī)連接組成。發(fā)射機(jī)的作用是把數(shù)字源(例如計(jì)算機(jī))產(chǎn)生的0、 1符號(hào)序列組成的消息信號(hào)變換成為適合于信道上傳送的波形。而

2、由丁符號(hào)間 干擾和噪聲的存在，信道輸出端收到的信號(hào)是含有噪聲的或失真的發(fā)送信號(hào)。 接收機(jī)的作用是，操作接收信號(hào)并把原消息信號(hào)的一個(gè)可靠估值傳遞給系統(tǒng)輸 出端的某個(gè)用戶(hù)。隨著通信系統(tǒng)復(fù)雜度的提高，對(duì)原消息信號(hào)的還原成為通信 系統(tǒng)中最為重要的環(huán)節(jié)，而噪聲是接收端需要排除的最主要的干擾，人們也設(shè) 計(jì)出了針對(duì)各種不同條件應(yīng)用的濾波器，其中最速下降算法是一種古老的最優(yōu) 化技術(shù)，而卡爾曼濾波器隨著應(yīng)用條件的精簡(jiǎn)成為了普適性的高效濾波器。2. 維納最速下降算法濾波器2.1最速下降算法的基本思想考慮一個(gè)代價(jià)函數(shù)丿(w),它是某個(gè)未知向量w的連續(xù)可微分函數(shù)。函數(shù)丿(w) 將W的元素映射為實(shí)數(shù)。這里，我們要尋找一

3、個(gè)最優(yōu)解使它滿(mǎn)足如下條件 /(Wo) <J(w)(2.1) 這也是無(wú)約束最優(yōu)化的數(shù)學(xué)表示。特別適合于自適應(yīng)濾波的一類(lèi)無(wú)約束最優(yōu)化算法基于局部迭代下降的算法: 從某一初始猜想w(0)出發(fā)，產(chǎn)生一系列權(quán)向frw(l),w(2),，使得代價(jià)函 數(shù)/(w)在其法的每一次迭代祁是下降的.即；(w(n+l)</(w(n)其中W(“)是權(quán)向研的過(guò)去值，而W(71+l)是其更新值"我們希里算法最終收斂到最優(yōu)值W。迭代下降的一種簡(jiǎn)單形式是最速下降 法，該方法是沿最速下降方向連續(xù)調(diào)整權(quán)向量。為方便起見(jiàn)，我們將梯度向量表示為173)=dw(2.2)因此，最速下降法可以表示為1w(n + 1)

4、= w(n) 一-jU<7(n)(23)其中n代表進(jìn)程，“是正常數(shù)，稱(chēng)為步長(zhǎng)參數(shù),1/2因子的引入是為了數(shù)學(xué)上處 理方便。在從71到H+1的迭代中，權(quán)向量的調(diào)整量為15w(n) = w(ji + 1) w(n) = c/(n)乙(2.4)為了證明最速下降算法滿(mǎn)足式(2.1),在w(町處進(jìn)行一階泰勒展開(kāi)，得到J(w(n + 1) a J(w(n) + gH(n)6w(n)(2.5) 此式對(duì)于“較小時(shí)是成立的。在式(2.4)中設(shè)w為負(fù)值向量，因而梯度向験/也為 負(fù)值向量，所以使用埃爾米特轉(zhuǎn)置。將式(2.4)用到式(2.5)中，得到丿3(/1 + 1)乂 丿3(勿)一扣 |g(n)|2此式表明

5、當(dāng)卩為正數(shù)時(shí)，/(w(n+l)</(w(n)o因此，隨著71的增加，代價(jià)函 數(shù)丿5)減小，當(dāng)71 = 8時(shí)，代價(jià)函數(shù)趨于最小值J8。2.2最速下降算法應(yīng)用于維納濾波器考慮一個(gè)橫向?yàn)V波器，其抽頭輸入為u(n),u(n-l),.-,u(n-M + 1),對(duì)應(yīng) 的抽頭權(quán)值為w°(n),wi(n),Wmt(h)。抽頭輸入是來(lái)自零均值、相關(guān)矩陣為R 的廣義半穩(wěn)隨機(jī)過(guò)程的抽樣值。除了這些輸入外，濾波器還要一個(gè)期與響應(yīng)d), 以便為最優(yōu)濾波提供一個(gè)參考。在時(shí)刻71抽頭輸入向量表示為“(n),濾波器輸 出端期望響應(yīng)的估計(jì)值為d(n|/n),其中人是由抽頭輸u(n),u(n- l), -,u(n

6、- M + 1)所張成的空間?？者^(guò)比較期望響應(yīng)d(n)及其估計(jì)值，可以得到一個(gè)估計(jì) 誤差e(n),即e(n) = d(h) d(n|l/n) = d(n) wH(h)u(h)(2.6)這里wH(n)u(n)是抽頭權(quán)向量w(m)與抽頭輸入向5：u(n)的內(nèi)積。w(?i)可以進(jìn)一 步表示為w(n) = w0(n)f wx(n)f , wM.x(n)r同樣，抽頭輸入向©:u(n)可表示為u(n) = u(n)fu(n I),-,u(n M + l)r如果抽頭輸入向暈U(n)和期望響應(yīng)d(7l)是聯(lián)合平穩(wěn)的，此時(shí)均方誤差或者 在時(shí)刻"的代價(jià)函數(shù)丿(町是抽頭權(quán)向量的二次函數(shù)，于是可以

7、得到/(n) = oI - wH(ji)p - pHw(n) + wHn)Rw(n)(2 刀 其中，於為目標(biāo)函數(shù)d(7l)的方差，p抽頭輸入向量"(7!)與期望響應(yīng)d(M)的互相 關(guān)向量，及R為抽頭輸入向S：u(n)的相關(guān)矩陣c從而梯度向星可以寫(xiě)為r/(n)=da0(ri) 可(n)0ai(n)4-+.dJW可(n)7 血15)=2p + 2Rw(n')dJW+可(n)0切-心)(2.8)dak(n) dbk(n)其中在列向量中字哄和弈分別是代價(jià)函數(shù)/(”對(duì)應(yīng)第k個(gè)抽頭權(quán)值叭(町的 實(shí)部H(M)和虛部/(")的偏導(dǎo)數(shù)。對(duì)最速下降算法應(yīng)用而言，假設(shè)式(2. 8)中相關(guān)

8、矩陣R和互相關(guān)向量P己知，則對(duì)于給定的抽頭權(quán)向量W(7i + 1)為w(n + 1) = w(n) + np Rw(ri)(2.9)它描述了為那濾波中最速下降法的數(shù)學(xué)表達(dá)式。3 卡爾曼濾波器3.1卡爾曼濾波器的基本思想卡爾曼濾波器是用狀態(tài)空間概念描述共數(shù)學(xué)公式的，另外新穎的特點(diǎn)是，他的解遞歸運(yùn)算，可以不加修改地應(yīng)用于平穩(wěn)和非平穩(wěn)環(huán)境。尤其是，其狀態(tài)的每一次更新估計(jì)都由前一次估計(jì)和新的輸入數(shù)據(jù)計(jì)算得到，因此只需存儲(chǔ)前 一次估計(jì)。除了不需要存儲(chǔ)過(guò)去的所有觀測(cè)數(shù)據(jù)外，卡爾曼濾波計(jì)算比直接根 據(jù)濾波過(guò)程中每一步所有過(guò)去數(shù)據(jù)進(jìn)行估值的方法都更加有效。Vi(n)x(n +1)x(n)y(n)ouo 二 尸

9、二o巾)圖3.1線(xiàn)性動(dòng)態(tài)離散時(shí)間系統(tǒng)的信號(hào)流圖表示“狀態(tài)”的概念是這種表示的基礎(chǔ)。狀態(tài)向量，簡(jiǎn)單地說(shuō)狀態(tài)，定義為數(shù)據(jù) 的最小集合，這組數(shù)據(jù)足以唯一地描述系統(tǒng)的自然動(dòng)態(tài)行為。換句話(huà)說(shuō)，狀態(tài)由 預(yù)測(cè)系統(tǒng)未來(lái)特性時(shí)所素要的，與系統(tǒng)的過(guò)去行為有關(guān)的最少的數(shù)據(jù)組成。典型 地，比較有代表性的情況是，狀態(tài)兀)是未知的。為了估計(jì)它，我們使用一組觀 測(cè)數(shù)據(jù)，在途中用向量y(n)表示。y(/i)成為觀測(cè)向屋或者簡(jiǎn)稱(chēng)觀測(cè)值，并假設(shè)它 是N維的。在數(shù)學(xué)上，圖3.1表示的信號(hào)流圖隱含著一下兩個(gè)方程：(1) 過(guò)程方程x(n + 1) = F(n 4- l,n)x(n) + vt(n)(3.1) 式中，MX1向量巧(町表示

10、噪聲過(guò)程，可建模為零均值的白噪聲過(guò)程，且其 相關(guān)矩陣定義為£h(n)v?(fc) = (n)鳥(niǎo)：(2) 測(cè)量方程y(n) = c (n)x(n) 4- v2(n)(3.2) 其中“小是已知的N X M測(cè)最矩陣。N X 1向ftv2(n)稱(chēng)為測(cè)量噪聲，建模 為零均值的白噪聲過(guò)程，其相關(guān)矩陣為E”2(3她) = 腫)鳥(niǎo):(3.3) 測(cè)量方程(3. 2)確立了可測(cè)系統(tǒng)輸岀y(町與狀態(tài)x(n)之間的關(guān)系，如圖 3. 1所示。3.2新息過(guò)程為了求解卡爾曼濾波問(wèn)題，我們將應(yīng)用基于新息過(guò)程的方法。根據(jù)之前所述, 用向RyCnlyn-x)表示n = 1時(shí)刻到八-1時(shí)刻所有觀測(cè)數(shù)據(jù)過(guò)去值給定的情況下,

11、 你時(shí)刻觀測(cè)數(shù)據(jù)y(n)的最小均方估計(jì)。過(guò)去的值用觀測(cè)值y(l)(2)，(n- 1) 表示，他們張成的向量空間用表示。從而可以定義新息過(guò)程如下：ot(n) =y(n)-y(n|yn_i)(3.4) 其中M X 1向Sa(n)表示觀測(cè)數(shù)據(jù)y(n)的新息。3.3應(yīng)用新息過(guò)程進(jìn)行狀態(tài)估計(jì)下面，我們根據(jù)信息過(guò)程導(dǎo)出狀態(tài)“)的最小均方估計(jì)。根據(jù)推導(dǎo)，這個(gè)估 計(jì)可以表示成為新息過(guò)程a(l),a(2),-,a(n)序列的線(xiàn)性組合，即nx(i|y)=W«(k)k=l(3.5) 其中艮伙)忙“是一組待定的MXN矩陣。根據(jù)正交性原理，預(yù)測(cè)狀態(tài)誤差向量 與新息過(guò)程正交，即E£(ifn)aH(m)

12、 = Ex(i) - xiynaH(m) = O(3.6) 將式(3.5)代入式(3.6),并利用新息過(guò)程的正交性質(zhì)，即得(3.7) 因此，式(3.刀兩邊同時(shí)右乘逆矩陣R7(m),可得創(chuàng)(m)的表達(dá)式為Bj(m) = E x(0 aH(m) 1 (m)(3.8) 最后，將式(3.8)帶入式(3.5),可得最小軍方差估計(jì)n x(t|yn)=工 ExaHWR-k)aW k=ln-1 =乂 Ex(i)aH(QR7(/c)a(k) +Ejr(i)(rH(n)7r1(n)仇(n) k=l(3.9) 故對(duì)于i =71+1,有n-1x(n + l|yn)=工 Ex(n + 1)/伙)/T丄(k)a伙)k=l

13、+ Ex(n + l)aH(n)J?"1(n)a(n)(3.10) 然而，n + 1時(shí)刻的狀態(tài)X(7l + 1)與71時(shí)刻的狀態(tài)x(ti)的關(guān)系式由式可以推導(dǎo)出對(duì) 于0 < k < n,有Ex(n + l)aH(k) = EF(n + 1, n)x(n) + Vi(n)aH(k)=F(n + 1,砒引班砒卅仇)(3.11)其中a(k)只與觀測(cè)數(shù)據(jù)丁(1),歹(2),丁伙)有關(guān)。因此可知，vi(n)與a(k)彼此正 交(其中0 <k<n)o利用式(3.11)以及當(dāng)i = n時(shí)兒(i|yn)的計(jì)算公式,可將式(3.10)H-1乂 Ex(n + l)aH(k)K-x

14、(k)a(k)k=l右邊的求和項(xiàng)改寫(xiě)為n-1=F(n + l,n)工 Ex(n)aH(/<)Zri(k)a伙) k=l=F(n + 1, n) x(n|yn_i)(3.12)為了進(jìn)一步討論，引入如下基本定義。3.4卡爾曼增益定義MXN矩陣G(n) = Ex(n + l)aH(n) R“(k)(3.13)其中Ex(n + 1)土(町是狀態(tài)向量班71 + 1)和新息過(guò)程2(71)的互相關(guān)矩陣。利用 這一定義和式(3.12)的結(jié)果，可以將式(3.10)簡(jiǎn)單重寫(xiě)為x(n + l|yn) = F(n + l,n)x(n|yn_i)+ G(n)a(n)(3.11) 式(3.14)具有明確的物理意義。

15、它標(biāo)明：線(xiàn)性動(dòng)態(tài)系統(tǒng)狀態(tài)的最小均方估計(jì) x(n+ 1|%)可以由前一個(gè)估計(jì)免(八嘰J求得。為了表示對(duì)卡爾曼開(kāi)創(chuàng)性貢獻(xiàn)的 認(rèn)可，將矩陣G(n)稱(chēng)為卡爾曼增益?，F(xiàn)在剩下唯一要解決的問(wèn)題是，怎樣以一種便于計(jì)算的形式來(lái)表示卡爾曼增 益G(n)o為此，酋先將x(n 4-1)與仇H®)乘積的期望表示為Ex(n + l)aH (n) = F(n+ 1, n) Ex(n)£H(n, n l)CH(n)(3.12) 式中利用了狀態(tài)班n)與噪聲向®v2(n)互不相關(guān)這一事實(shí)。其次，由于預(yù)測(cè)狀態(tài) 誤差向一 1)與估計(jì)x(n|yn_i)正交，因比無(wú)仇嘰_»與占,” 一 1)乘機(jī)

16、的 期望為零。這樣，用預(yù)測(cè)狀態(tài)誤差向量£(n,n-l)代替相乘因子x(n),將不會(huì)引 起式(3.15)變化，故有Ex(n+ l)aH(n) = F(n + 1, n)Ee(n,n 一 1)eh(h,h- l)CH(n)(3.13) 由此，可將上式進(jìn)一步變化為Ex(n 4- l)aH(n) = F(n+ l,n)K(n,n- l)CH(n)(3.14) 現(xiàn)在我們重新定義卡爾曼增益。為此，將式(3.17)代入式(3.13)得G(n) = F(n + l,n)K(n,n - l)CH(n) Rk)(3.15) 現(xiàn)在我們已經(jīng)了解了卡爾曼濾波的整個(gè)過(guò)程利相應(yīng)的參數(shù)設(shè)置，為了能夠更為方 便利用計(jì)

17、算機(jī)仿真實(shí)現(xiàn)，特將其中參數(shù)變量進(jìn)行小結(jié)?？柭兞亢蛥?shù)小結(jié)變童定義維數(shù)x(n)n時(shí)刻狀態(tài)M x 1yW丹時(shí)刻狀態(tài)值iVx 1F(n + l,n)從71時(shí)刻到71 + 1時(shí)刻的轉(zhuǎn)移矩陣M xMC(n)n時(shí)刻的測(cè)量矩陣NxMQ心)過(guò)程噪聲“100的相關(guān)矩陣M xM過(guò)程噪聲巾00的相關(guān)矩陣NxN*(nlyn-i)給定觀測(cè)值y(l),y(2),y(n -M x 11)在“時(shí)刻狀態(tài)的預(yù)測(cè)佔(zhàn)計(jì)飢“l(fā)y給定觀測(cè)值也)(2),(町在Mxln時(shí)刻狀態(tài)的濾波估計(jì)G(n)n時(shí)刻卡爾曼增益矩陣M xNa(n)71時(shí)刻新息向fitNx 1R(n)新息向T.a(n)的相關(guān)矩陣NxNK(nt n 1)x(n|yn_x)&

18、#171;|誤基相關(guān)矩陣MxMx(n|yn)中課差相關(guān)矩陣M xM基于單步預(yù)測(cè)的卡爾曼濾波器的小結(jié)觀測(cè)<t=y(l),y(2), -,y(n- 1)轉(zhuǎn)移矩陣=F(n + l,n)測(cè)呈矩陣=C(n)過(guò)程噪聲的郴關(guān)矩陣=<?!(«)測(cè)呈噪聲的柑關(guān)矩陣=(?2(n)G(n) = F(n + l,n)K(n,n l)CH(n) C(n)K(n, n - l)Cw(n) + (?2(n)_1<r(n) = y(n) - C(n)5(n|yn_x)*(n + 1|%) = F(n + 1, n)免(n|yn_J + G(n)a(n)K(n) = K(n,n - 1) - F(n

19、,n + l)G(n)C(n)K(n,n - 1)K(n + l,n) = F(n + l,n)K(n)FM(n + l,n) + (?x(n)4 Matlab 仿真為了簡(jiǎn)化，這里只討論簡(jiǎn)單的一維單輸入一單輸出線(xiàn)性系統(tǒng)模型，其中加入白噪聲作為系統(tǒng)的擾動(dòng)，具體仿真結(jié)果可以獲得如下4.1維納最速下降法濾波器仿真結(jié)果以上為最速下降法中不同的遞歸步長(zhǎng)所導(dǎo)致的跟蹤效果變化，對(duì)F最速下降法中 的步長(zhǎng)是影響其算法穩(wěn)定的關(guān)鍵，最速下降算法穩(wěn)定的充分必要條件是條件步長(zhǎng) 因子為小于輸入自相關(guān)矩陣的最大特征值倒數(shù)的2倍。上面的字列分別從相關(guān)矩 陣的隨大特征之2倍的0.4倍開(kāi)始變化至其1倍，最后一幅圖象能夠看出其己經(jīng)

20、 不再收斂，下面是大于輸入相關(guān)矩陣的最大特征值2倍步長(zhǎng)時(shí)所表現(xiàn)的跟蹤結(jié)果可以看出其已經(jīng)明顯發(fā)散，不再是我們所期望的濾波算法。因此可以總結(jié)出，對(duì) 于最速下降法來(lái)說(shuō)，步長(zhǎng)的選取是很重要的，根據(jù)不冋條件的需求，選取正確的 步長(zhǎng)，能為算法的快速高效提供基礎(chǔ)。4.2卡爾曼濾波器仿真結(jié)果從圖中可以發(fā)現(xiàn)，卡爾曼濾波器能夠非常有效地在比較大的干擾下比較準(zhǔn)確 地反映真實(shí)值，如果觀測(cè)端加入干擾較大時(shí)，卡爾曼濾波器能夠較為有效地進(jìn)行 濾波，不過(guò)當(dāng)狀態(tài)端的干擾增大時(shí)，卡爾曼濾波器的濾波效果也會(huì)隨之下降。如 下圖，是加大了狀態(tài)端的干擾，所呈現(xiàn)的濾波效果。如上圖所示，狀態(tài)端的干擾導(dǎo)致?tīng)顟B(tài)不穩(wěn)定，卡爾曼濾波器的估計(jì)值也出現(xiàn)

21、 了比較大的波動(dòng)。如果將狀態(tài)端的干擾再增大，則會(huì)出現(xiàn)更為嚴(yán)峻的濾波考驗(yàn), 濾波效果如下。這是的狀態(tài)已經(jīng)很勉強(qiáng)了，所以，研究更為有效的多方法卡爾曼濾波器也顯 得十分必要了。4.3 一種不需初始化的卡爾曼濾波器仿真這種濾波器只是實(shí)現(xiàn)了無(wú)需對(duì)部分變量進(jìn)行初始化的設(shè)計(jì)，沒(méi)有特別意義上 的改進(jìn)經(jīng)典卡爾曼濾波器本身性能的特點(diǎn)。仿真圖如下4.4后聯(lián)平滑濾波的卡爾曼濾波器仿真只是在經(jīng)典卡爾曼濾波器后端聯(lián)接了平滑濾波器，對(duì)性能改進(jìn)的效果并不特別明顯，仿真圖如下V5 tin 4如圖中所表示，即使平滑過(guò)的估值與觀測(cè)值之間的差別也不是特別令人滿(mǎn)意, 所以，對(duì)于經(jīng)典卡爾曼濾波的研究還需要更深一步進(jìn)行，由于時(shí)間和能力有限

22、， 本次的作業(yè)對(duì)丁卡爾曼及其他濾波器的研究只能達(dá)到這種程度，希望在以后的學(xué) 習(xí)中，能發(fā)現(xiàn)更好的對(duì)經(jīng)典卡爾曼濾波器的改進(jìn)方法。5 Matlab源代碼（部分參考自互聯(lián)網(wǎng)）5.1經(jīng)典卡爾曼濾波器clearN=200;w(l)=O;x(l)=5;a=l；c=l；QI = randn(lzN)*l;% 過(guò)程噪聲Q2 = randn(lzN);% 測(cè)量噪聲for k=2:N;x(k)=a*x(k-l)+Ql(k-l); end%狀態(tài)矩陣for k=l:N;Y(k)=c*x(k)+Q2(k);endP(l)=10；s(l)=l；for t=2:N;Rwv/=cov(Ql(l:t);Rw=cov(Q2(l:t

23、);pl(t)=a.A2*p(t-l)+Rww;b(t)=c*pl(t)/(c.A2*pl(t)+Rvv);%kalman 增益s(t)=a*s(t-l)+b(t)*(Y(t)-a*c*s(t-l);p(t)=pl(t)-c*b(t)*pl(t);endt=l:N;plot(t,s,T,t,Y,g,t,x,b);%紅色卡爾曼，綠色觀測(cè)值，藍(lán)色狀態(tài)值 legend('kalman estimate'/ovservations'/truth')；5.2最速下降法clcclear allN=30;q=2.1;%q>l&&q<2/Ryx 最大

24、特征值hn=zeros(l,N);hn(:)=5;vg=O;Rxx=xcorr(l);Ryx=mi n( min(corrcoef(l, 1+rand n);echo offfor i=l:N-l;%vg=2*Rxx*hn(:,i)-2*Ryx;%hn(:,i+l)=hn(:,i)-l/2*q*vg;vg=2*Rxx*hn(i)-2*Ryx;hn(i+l)=hn(i)-l/2*q*vg;m(i)=l；endl=l:N-l;plot(t,hn(t),W,t,m(t),'b)；5.3后聯(lián)平滑濾波器的卡爾曼濾波器clearclc;N=300;CON = 5;x = zeros(l,N);x(l)=l；P = 10；Q = randn(lzN)#0.2;%ii 程噪聲協(xié)方差R = randn(lzN);%觀測(cè)噪聲協(xié)方差y=R + CON;%加過(guò)程噪聲的狀態(tài)輸出for k =