拓展卡爾曼濾波

1、精選優(yōu)質(zhì)文檔-傾情為你奉上精選優(yōu)質(zhì)文檔-傾情為你奉上專(zhuān)心-專(zhuān)注-專(zhuān)業(yè)專(zhuān)心-專(zhuān)注-專(zhuān)業(yè)精選優(yōu)質(zhì)文檔-傾情為你奉上專(zhuān)心-專(zhuān)注-專(zhuān)業(yè) 南京航空航天大學(xué) 隨機(jī)信號(hào)小論文題 目擴(kuò)展卡爾曼濾波學(xué)生姓名梅晟學(xué) 號(hào)SX學(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)于線(xiàn)性濾波與預(yù)測(cè)問(wèn)題的新方法），這讓卡爾曼濾波成為了時(shí)域內(nèi)有效的濾波方法，從此各種基于卡爾曼濾波的方法橫空出世，在目標(biāo)跟蹤、故

2、障診斷、計(jì)量經(jīng)濟(jì)學(xué)、慣導(dǎo)系統(tǒng)等方面得到了長(zhǎng)足的發(fā)展。二、卡爾曼濾波器卡爾曼濾波是一種高效率的遞歸濾波器(自回歸濾波器), 它能夠從一系列的不完全及包含噪聲的中，估計(jì)的狀態(tài)?？柭鼮V波的一個(gè)典型實(shí)例是從一組有限的，包含噪聲的，對(duì)物體位置的觀(guān)察序列（可能有偏差）預(yù)測(cè)出物體的位置的及?？柭贜ASA研究中心訪(fǎng)問(wèn)時(shí)，發(fā)現(xiàn)他的方法對(duì)于解決的軌道預(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ā)的平方根濾波器的變種。也許最

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

4、噪聲和觀(guān)測(cè)噪聲。它們?yōu)橄嗷オ?dú)立，p(w) N(0, Q),p(v) N(0, R).實(shí)際系統(tǒng)中，過(guò)程激勵(lì)噪聲協(xié)方差矩陣 Q 和觀(guān)測(cè)噪聲協(xié)方差矩陣 R 可能會(huì)隨每次迭代計(jì)算而變化。但在這我們假設(shè)它們是常數(shù)。差分方程式(1.1)中的非線(xiàn)性函數(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ù)。 非線(xiàn)性函數(shù) h 反映了狀態(tài)變量 xk 和觀(guān)測(cè)變量 zk 的關(guān)系。實(shí)際中我們顯然不知道每一時(shí)刻噪聲 wk xk=fxk-1,uk-1和zk=h(xk,0)其中，xk 是過(guò)程相對(duì)前一時(shí)刻 k 有一點(diǎn)非常重要，那就是擴(kuò)展卡爾曼濾波

5、器的一個(gè)基本缺陷：離散隨機(jī)變量的分布（或連續(xù)隨機(jī)變量的密度）在經(jīng)過(guò)非線(xiàn)性系統(tǒng)轉(zhuǎn)化后不再是正態(tài)的了。擴(kuò)展卡爾曼濾波器其實(shí)就是一個(gè)通過(guò)線(xiàn)性化而達(dá)到漸進(jìn)最優(yōu)貝葉斯決策的特殊狀態(tài)估計(jì)器。3.2 濾波器的計(jì)算原型為了估計(jì)一個(gè)具有非線(xiàn)性差分和量測(cè)關(guān)系的過(guò)程， 我們先給出式(1.3)和式(1.4)的一個(gè)新的線(xiàn)性化表示：xkxk+Azkzk+Hx其中， xk和 zk是狀態(tài)向量和觀(guān)測(cè)向量的真值， xk和 zk來(lái)自1.3式和1.4式，是狀態(tài)向量和觀(guān)測(cè)向量的近似值， xk是 k 時(shí)刻狀態(tài)向量的后驗(yàn)估計(jì)， 隨機(jī)變量 wk 和 vk表示過(guò)程激勵(lì)噪聲和觀(guān)測(cè)噪聲。 A 是A W 是 f 對(duì) w 的偏導(dǎo)的雅可比矩陣：W H

6、是 h 對(duì) x 的偏導(dǎo)的雅可比矩陣：H V 是 h 對(duì) v 的偏導(dǎo)的雅可比矩陣：V現(xiàn)在我們定義一個(gè)新的預(yù)測(cè)誤差的表達(dá)式：exkxk-x和觀(guān)測(cè)變量的殘余，ezkzk-z但實(shí)際中無(wú)法獲得(1.7)式中的xk ,它是狀態(tài)向量的真值，也就是要估計(jì)的對(duì)象。同樣， (1.8)式中的zk也是無(wú)法獲取的，它是用來(lái)估計(jì)xk的觀(guān)測(cè)向量的真值。 由(1.7)式和(exkAxk-1-ezkHexk 和 k 代表具有零均值和協(xié)方差矩陣 WQWT 和 VRVT 的獨(dú)立隨機(jī)變量， Q 和 R 分別為過(guò)程激勵(lì)噪聲協(xié)方差矩陣和觀(guān)測(cè)噪聲協(xié)方差矩陣。在此我們利用(1.8)式中的觀(guān)測(cè)殘余真值ezk去估計(jì)(1.9)式中的預(yù)測(cè)誤差exk

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

8、和狀態(tài)變量通過(guò) 拓展卡爾曼濾波器基本運(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) = 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

9、 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(ORIGINAL SIGNAL);subplot(212); plot(YY); title(ESTIMATED SIGNAL);figure(2); plot(tt,XX,tt,YY); title(Combined plot);

10、 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)-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

11、: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) = XKk(ii)2 + UUk(ii); % UPPER EQUATIONS.Zk(ii,1) = cos(XUPK(ii) + VVk(ii); % UPPER EQUATIONS.endfor ii = 1:1:length(XKk)XBAR

12、k(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.endfor in = 1:1:length(XU