拓展卡爾曼濾波

1、 南京航空航天大學(xué) 隨機(jī)信號小論文題 目擴(kuò)展卡爾曼濾波學(xué)生姓名梅晟學(xué) 號SX1504059學(xué) 院電子信息工程學(xué)院專 業(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ù)測問題的新方法），這讓卡爾曼濾波成為了時(shí)域內(nèi)有效的濾波方法，從此各種基于卡爾曼濾波的方法橫空出世，在目標(biāo)跟蹤、故障診斷、計(jì)量經(jīng)濟(jì)學(xué)、慣導(dǎo)系統(tǒng)等方面得到了長足的發(fā)展。二、卡爾曼濾波器卡爾曼濾波是一種高效率的遞歸濾波器(自回歸濾

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

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

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

5、向量為：xk=fxk-1,uk-1,0 (1.3)和zk=h(xk,0) (1.4)其中，xk 是過程相對前一時(shí)刻 k 的后驗(yàn)估計(jì)。有一點(diǎn)非常重要，那就是擴(kuò)展卡爾曼濾波器的一個(gè)基本缺陷：離散隨機(jī)變量的分布（或連續(xù)隨機(jī)變量的密度）在經(jīng)過非線性系統(tǒng)轉(zhuǎn)化后不再是正態(tài)的了。擴(kuò)展卡爾曼濾波器其實(shí)就是一個(gè)通過線性化而達(dá)到漸進(jìn)最優(yōu)貝葉斯決策的特殊狀態(tài)估計(jì)器。3.2 濾波器的計(jì)算原型為了估計(jì)一個(gè)具有非線性差分和量測關(guān)系的過程， 我們先給出式(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、和觀測向量的真值， xk和 zk來自1.3式和1.4式，是狀態(tài)向量和觀測向量的近似值， xk是 k 時(shí)刻狀態(tài)向量的后驗(yàn)估計(jì)， 隨機(jī)變量 wk 和 vk表示過程激勵(lì)噪聲和觀測噪聲。 A 是 f 對 x 的偏導(dǎo)的雅可比矩陣：Ai,j=fixjxk-1,uk-1,0 W 是 f 對 w 的偏導(dǎo)的雅可比矩陣：Wi,j=fiwjxk-1,uk-1,0 H 是 h 對 x 的偏導(dǎo)的雅可比矩陣：Hi,j=hixjxk,0 V 是 h 對 v 的偏導(dǎo)的雅可比矩陣：Vi,j=hivjxk,0現(xiàn)在我們定義一個(gè)新的預(yù)測誤差的表達(dá)式：exkxk-xk (1.7)和觀測變量的殘余，ezkzk-zk (1.8)但實(shí)際中無

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

9、的關(guān)系， 雅可比矩陣 Hk 便通過改變卡爾曼增益從而使得殘余 zk-h(xk-,0)中真正作用于狀態(tài)變量的部分被加權(quán)。 當(dāng)然， 如果整個(gè)觀測中觀測變量 zk 和狀態(tài)變量通過 h 都沒有一個(gè)一一對應(yīng)的關(guān)系， 那么濾波器很快就會(huì)發(fā)散。 這種情況下過程是不可觀測的。拓展卡爾曼濾波器基本運(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,

12、Uk,Qu,Vk,Qv,C,n,Wk,W,V);Ap(2,:) = 0;for ii = 1:1:length(Ap)-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

13、( (C*PPk*C') + (V*Qv*V') ); % THIRD STEPfor ii = 1:1:length(Xint_v)XUPK(ii,1) = 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 -

14、 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 EQUATION