拓展卡爾曼濾波

1、 南京航空航天大學(xué) 隨機(jī)信號(hào)小論文題 目擴(kuò)展卡爾曼濾波學(xué)生姓名梅晟學(xué) 號(hào)SX1504059學(xué) 院電子信息工程學(xué)院專(zhuān) 業(yè)通信與信息系統(tǒng)擴(kuò)展卡爾曼濾波一、引言20世紀(jì)60年代，在航空航天工程突飛猛進(jìn)而電子計(jì)算機(jī)又方興未艾之時(shí)，卡爾曼發(fā)表了論文A New Approach to Linear Filtering and Prediction Problems（一種關(guān)于線性濾波與預(yù)測(cè)問(wèn)題的新方法），這讓卡爾曼濾波成為了時(shí)域內(nèi)有效的濾波方法，從此各種基于卡爾曼濾波的方法橫空出世，在目標(biāo)跟蹤、故障診斷、計(jì)量經(jīng)濟(jì)學(xué)、慣導(dǎo)系統(tǒng)等方面得到了長(zhǎng)足的發(fā)展。二、卡爾曼濾波器卡爾曼濾波是一種高效率的遞歸濾波器(自回歸濾

2、波器), 它能夠從一系列的不完全及包含噪聲的測(cè)量中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)。卡爾曼濾波的一個(gè)典型實(shí)例是從一組有限的，包含噪聲的，對(duì)物體位置的觀察序列（可能有偏差）預(yù)測(cè)出物體的位置的坐標(biāo)及速度。卡爾曼在NASA埃姆斯研究中心訪問(wèn)時(shí)，發(fā)現(xiàn)他的方法對(duì)于解決阿波羅計(jì)劃的軌道預(yù)測(cè)很有用，后來(lái)阿波羅飛船的導(dǎo)航電腦便使用了這種濾波器。目前,卡爾曼濾波已經(jīng)有很多不同的實(shí)現(xiàn)?？柭畛跆岢龅男问浆F(xiàn)在一般稱(chēng)為簡(jiǎn)單卡爾曼濾波器。除此以外，還有施密特?cái)U(kuò)展濾波器、信息濾波器以及很多Bierman, Thornton 開(kāi)發(fā)的平方根濾波器的變種。也許最常見(jiàn)的卡爾曼濾波器是鎖相環(huán)，它在收音機(jī)、計(jì)算機(jī)和幾乎任何視頻或通訊設(shè)備中廣泛

3、存在。三、擴(kuò)展卡爾曼濾波器3.1 被估計(jì)的過(guò)程信號(hào)卡爾曼最初提出的濾波理論只適用于線性系統(tǒng),Bucy，Sunahara等人提出并研究了擴(kuò)展卡爾曼濾波(Extended Kalman Filter，簡(jiǎn)稱(chēng)EKF)，將卡爾曼濾波理論進(jìn)一步應(yīng)用到非線性領(lǐng)域。EKF的基本思想是將非線性系統(tǒng)線性化，然后進(jìn)行卡爾曼濾波，因此EKF是一種次優(yōu)濾波。同泰勒級(jí)數(shù)類(lèi)似，面對(duì)非線性關(guān)系時(shí)，我們可以通過(guò)求過(guò)程和量測(cè)方程的偏導(dǎo)來(lái)線性化并計(jì)算當(dāng)前估計(jì)。假設(shè)過(guò)程具有狀態(tài)向量xRn， 其狀態(tài)方程為非線性隨機(jī)差分方程的形式。xk=fxk-1,uk-1,wk-1 (1.1)觀測(cè)變量zRm為：zk=h(xk,vk) (1.2)隨機(jī)變

4、量wk和vk代表過(guò)程激勵(lì)噪聲和觀測(cè)噪聲。它們?yōu)橄嗷オ?dú)立，服從正態(tài)分布的白色噪聲：p(w) N(0, Q),p(v) N(0, R).實(shí)際系統(tǒng)中，過(guò)程激勵(lì)噪聲協(xié)方差矩陣 Q 和觀測(cè)噪聲協(xié)方差矩陣 R 可能會(huì)隨每次迭代計(jì)算而變化。但在這我們假設(shè)它們是常數(shù)。差分方程式(1.1)中的非線性函數(shù) f 將過(guò)去 k 1 時(shí)刻狀態(tài)與現(xiàn)在 k 時(shí)刻狀態(tài)聯(lián)系起來(lái)。 量測(cè)方程(1.2)中的驅(qū)動(dòng)函數(shù) uk 和零均值過(guò)程噪聲 wk 是它的參數(shù)。 非線性函數(shù) h 反映了狀態(tài)變量 xk 和觀測(cè)變量 zk 的關(guān)系。實(shí)際中我們顯然不知道每一時(shí)刻噪聲 wk 和 vk 各自的值。但是，我們可以將它們假設(shè)為零，從而估計(jì)狀態(tài)向量和觀測(cè)

5、向量為：xk=fxk-1,uk-1,0 (1.3)和zk=h(xk,0) (1.4)其中，xk 是過(guò)程相對(duì)前一時(shí)刻 k 的后驗(yàn)估計(jì)。有一點(diǎn)非常重要，那就是擴(kuò)展卡爾曼濾波器的一個(gè)基本缺陷：離散隨機(jī)變量的分布（或連續(xù)隨機(jī)變量的密度）在經(jīng)過(guò)非線性系統(tǒng)轉(zhuǎn)化后不再是正態(tài)的了。擴(kuò)展卡爾曼濾波器其實(shí)就是一個(gè)通過(guò)線性化而達(dá)到漸進(jìn)最優(yōu)貝葉斯決策的特殊狀態(tài)估計(jì)器。3.2 濾波器的計(jì)算原型為了估計(jì)一個(gè)具有非線性差分和量測(cè)關(guān)系的過(guò)程， 我們先給出式(1.3)和式(1.4)的一個(gè)新的線性化表示：xkxk+Axk-1-xk-1+Wwk-1 (1.5)zkzk+Hxk-xk+Vvk (1.6)其中， xk和 zk是狀態(tài)向量

6、和觀測(cè)向量的真值， xk和 zk來(lái)自1.3式和1.4式，是狀態(tài)向量和觀測(cè)向量的近似值， xk是 k 時(shí)刻狀態(tài)向量的后驗(yàn)估計(jì)， 隨機(jī)變量 wk 和 vk表示過(guò)程激勵(lì)噪聲和觀測(cè)噪聲。 A 是 f 對(duì) x 的偏導(dǎo)的雅可比矩陣：Ai,j=fixjxk-1,uk-1,0 W 是 f 對(duì) w 的偏導(dǎo)的雅可比矩陣：Wi,j=fiwjxk-1,uk-1,0 H 是 h 對(duì) x 的偏導(dǎo)的雅可比矩陣：Hi,j=hixjxk,0 V 是 h 對(duì) v 的偏導(dǎo)的雅可比矩陣：Vi,j=hivjxk,0現(xiàn)在我們定義一個(gè)新的預(yù)測(cè)誤差的表達(dá)式：exkxk-xk (1.7)和觀測(cè)變量的殘余，ezkzk-zk (1.8)但實(shí)際中無(wú)

7、法獲得(1.7)式中的xk ,它是狀態(tài)向量的真值，也就是要估計(jì)的對(duì)象。同樣， (1.8)式中的zk也是無(wú)法獲取的，它是用來(lái)估計(jì)xk的觀測(cè)向量的真值。 由(1.7)式和(1.8)式我們可以寫(xiě)出誤差過(guò)程的表達(dá)式：exkAxk-1-xk-1+k (1.9)ezkHexk+k (1.10)k 和 k 代表具有零均值和協(xié)方差矩陣 WQWT 和 VRVT 的獨(dú)立隨機(jī)變量， Q 和 R 分別為過(guò)程激勵(lì)噪聲協(xié)方差矩陣和觀測(cè)噪聲協(xié)方差矩陣。在此我們利用(1.8)式中的觀測(cè)殘余真值ezk去估計(jì)(1.9)式中的預(yù)測(cè)誤差exk，估計(jì)結(jié)果記為ek，結(jié)合(1.7)式可以獲得初始非線性過(guò)程的后驗(yàn)狀態(tài)估計(jì)：xk=xk+ek

8、(1.11)(1.9)式和(1.10)式中的隨機(jī)變量具有如下概率分布:p(exk)N(0,EexkexkT)p(k)N(0,WQkWT)p(k)N(0,VRkVT)令ek的估計(jì)值為零，由以上近似，可以寫(xiě)出估計(jì)ek 的卡爾曼濾波器表達(dá)式：ek=Kkezk (1.12)將(1.8)式和(1.12)式代入(1.11)式，得到：xk=xk+Kkezk=xk+Kk(zk-zk) (1.13)3.3 拓展卡爾曼濾波總結(jié)擴(kuò)展卡爾曼濾波器的一個(gè)重要特性是卡爾曼增益 Kk 的表達(dá)式中的雅可比矩陣 Hk 能夠正確地傳遞或“加權(quán)” 觀測(cè)信息中的有用部分。 例如， 如果通過(guò) h 觀測(cè)變量 zk 和狀態(tài)變量沒(méi)有一一對(duì)應(yīng)

9、的關(guān)系， 雅可比矩陣 Hk 便通過(guò)改變卡爾曼增益從而使得殘余 zk-h(xk-,0)中真正作用于狀態(tài)變量的部分被加權(quán)。 當(dāng)然， 如果整個(gè)觀測(cè)中觀測(cè)變量 zk 和狀態(tài)變量通過(guò) h 都沒(méi)有一個(gè)一一對(duì)應(yīng)的關(guān)系， 那么濾波器很快就會(huì)發(fā)散。 這種情況下過(guò)程是不可觀測(cè)的。拓展卡爾曼濾波器基本運(yùn)行流程圖如下：四、Matlab仿真附程序如下:clc; close all; clear all;Xint_v = 1; 0; 0; 0; 0;wk = 1 0 0 0 0;vk = 1 0 0 0 0;for ii = 1:1:length(Xint_v) Ap(ii) = Xint_v(ii)*2; W(ii)

10、= 0; H(ii) = -sin(Xint_v(ii); V(ii) = 0; Wk(ii) = 0;endUk = randn(1,200);Qu = cov(Uk);Vk = randn(1,200);Qv = cov(Vk);C = 1 0 0 0 0;n = 100; YY XX = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);for it = 1:1:length(XX) MSE(it) = YY(it) - XX(it);endtt = 1:1:length(XX);figure(1); subplot(211); plot(XX);

11、title(ORIGINAL SIGNAL);subplot(212); plot(YY); title(ESTIMATED SIGNAL);figure(2); plot(tt,XX,tt,YY); title(Combined plot); legend(original,estimated);figure(3); plot(MSE.2); title(Mean square error);%-%-function YY,XX = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);Ap(2,:) = 0;for ii = 1:1:length(Ap)-

12、1 Ap(ii+1,ii) = 1;endinx = 1;UUk = Uk(inx); 0; 0; 0; 0;PPk = (Xint_v*Xint_v);VVk = Vk(inx); 0; 0; 0; 0;Qv = V*V;for ii = 1:1:length(Xint_v)XKk(ii,1) = Xint_v(ii)2; %FIRST STEPendPPk = Ap*PPk*Ap; % SECOND STEPKk = PPk*C*inv( (C*PPk*C) + (V*Qv*V) ); % THIRD STEPfor ii = 1:1:length(Xint_v)XUPK(ii,1) =

13、XKk(ii)2 + UUk(ii); % UPPER EQUATIONS.Zk(ii,1) = cos(XUPK(ii) + VVk(ii); % UPPER EQUATIONS.endfor ii = 1:1:length(XKk)XBARk(ii,1)=XKk(ii)+ Kk(ii)*(Zk(ii) - (cos(XKk(ii) ; % FOURTH STEPendII = eye(5,5);Pk = ( II - Kk*C)*PPk; % FIFTH STEP %-%-for ii = 1:1:nUUk = Uk(ii+1); 0; 0; 0; 0;PPk = XBARk*XBARk;VVk = Vk(ii+1); 0; 0; 0; 0;XKk = exp(-XBARk); % FIRST STEPPPkM = Ap*PPk*Ap; % SECOND STEPKk = PPkM*C*inv( (C*PPkM*C) + (V*Qv*V) ); % THIRD STEPfor nn = 1:1:length(XBARk)XUPK(nn) = exp(-XKk(nn) + UUk(nn); % UPPER EQUATIONS.Zk(nn) = cos(XUPK(nn) + VVk(nn); % UPPER EQUATIONS.endf