拓展卡爾曼濾波

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

2、器(自回歸濾波器), 它能夠從一系列的不完全及包含噪聲的中，估計的狀態(tài)?？柭鼮V波的一個典型實例是從一組有限的，包含噪聲的，對物體位置的觀察序列（可能有偏差）預測出物體的位置的及。卡爾曼在NASA研究中心訪問時，發(fā)現(xiàn)他的方法對于解決的軌道預測很有用，后來阿波羅飛船的導航電腦便使用了這種濾波器。目前,卡爾曼濾波已經(jīng)有很多不同的實現(xiàn)。卡爾曼最初提出的形式現(xiàn)在一般稱為簡單卡爾曼濾波器。除此以外，還有施密特擴展濾波器、信息濾波器以及很多Bierman, Thornton 開發(fā)的平方根濾波器的變種。也許最常見的卡爾曼濾波器是，它在收音機、計算機和幾乎任何視頻或通訊設備中廣泛存在。三、擴展卡爾曼濾波器3.

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

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

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

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

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

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

9、通過改變卡爾曼增益從而使得殘余 zk-h(xk-,0)中真正作用于狀態(tài)變量的部分被加權。 當然， 如果整個觀測中觀測變量 zk 和狀態(tài)變量通過 h 都沒有一個一一對應的關系， 那么濾波器很快就會發(fā)散。 這種情況下過程是不可觀測的。拓展卡爾曼濾波器基本運行流程圖如下：四、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) = 0; H(ii) = -s

10、in(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); title('ORIG

11、INAL 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

12、,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( (C*PPk*C'

13、) + (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 - Kk*C)*PPk; % F

14、IFTH 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.endfor