卡爾曼濾波器及其簡matlab仿真

1、卡爾曼濾波器及其簡matlab仿真一、卡爾曼濾波的起源談到信號的分析與處理，就離不開濾波兩個字。通常，信號的頻譜處于有限的頻率范圍內，而噪聲的頻譜則散布在很廣的頻率范圍內，為了消除噪聲，可以進行頻域濾波。但在許多應用場合，需要直接進行時域濾波，從帶噪聲的信號中提取有用信號。雖然這樣的過程其實也算是對信號的濾波，但其所依據的理論，即針對隨機信號的估計理論，是自成體系的。人們對于隨機信號干擾下的有用信號不能“確知”，只能“估計”。為了“估計”，要事先確定某種準則以評定估計的好壞程度。I960年卡爾曼發(fā)表了用遞歸方法解決離散數據線性濾波問題的論文ANewApproachtoLinearFilteri

2、ngandPredictionProblems（線性濾波與預測問題的新方法），在這篇文章里一種克服了維納濾波缺點的新方法被提出來，這就是我們今天稱之為卡爾曼濾波的方法。卡爾曼濾波應用廣泛且功能強大，它可以估計信號的過去和當前狀態(tài)甚至能估計將來的狀態(tài)即使并不知道模型的確切性質。其基本思想是以最小均方誤差為最佳估計準則，采用信號與噪聲的狀態(tài)空間模型利用前一時刻的估計值和當前時刻的觀測值來更新對狀態(tài)變量的估計，求出當前時刻的估計值。算法根據建立的系統(tǒng)方程和觀測方程對需要處理的信號做出滿足最小均方誤差的估計。對于解決很大部分的問題，它是最優(yōu)，效率最高甚至是最有用的。它的廣泛應用已經超過30年，包括機器

3、人導航，控制，傳感器數據融合甚至在軍事方面的雷達系統(tǒng)以及導彈追蹤等等。近年來更被應用于計算機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等?？柭鼮V波不要求保存過去的測量數據，當新的數據到來時，根據新的數據和前一時刻的儲值的估計，借助于系統(tǒng)本身的狀態(tài)轉移方程，按照一套遞推公式，即可算出新的估值?？柭f推算法大大減少了濾波裝置的存儲量和計算量，并且突破了平穩(wěn)隨機過程的限制，使卡爾曼濾波器適用于對時變信號的實時處理?？柭鼮V波的原理卡爾曼濾波思想的來源是在海圖作業(yè)中，航海長通常以前一時刻的船位為基準，根據航向、船速和海流等一系列因素推算下一個船位，但是他并不輕易認為船位就一定在推算船位上，還

4、要選擇適當的方法，通過儀器得到另一個推算船位。觀測和推算這兩個船位一般不重合，航海長需要通過分析和判斷選擇一個可靠的船位，作為船艦當前的位置。就是以現(xiàn)時刻的最佳估計為在前一時刻的最佳估計的基礎上根據現(xiàn)時刻的觀測值作線性修正。卡爾曼濾波在數學上是一種線性最小方差統(tǒng)計估算方法，它是通過處理一系列帶有誤差的實際測量數據而得到物理參數的最佳估算。其實質要解決的問題是八要尋找在最小均方誤差下Xk的估計值Xko它的特點是可以用遞推的方法計算Xk,其所需數據存儲量較小，便于進行實時處理。具體來說，卡爾曼濾波就是要用預測方程和測量方程對系統(tǒng)狀態(tài)進行估計。設動態(tài)系統(tǒng)的狀態(tài)方程和測量方程分別為：XK="

5、)K,KXK1-K,KWk1ZK-HKXKVK上兩式子中，Xk是k時刻的系統(tǒng)狀態(tài)，Gk,k和K'K是k-1時刻到k時刻的狀態(tài)轉移矩陣，ZK是k時刻的測量值，Hk是測量系統(tǒng)的參數，Wk和VK分別表示過程和測量的噪聲，他們被假設成高斯白噪聲。如果被估計狀態(tài)和觀測量是滿足上述第一式，系統(tǒng)過程噪聲和觀測噪聲滿足第二式的假設，k時刻的觀測Xk的估計X可按下述方程求解。狀態(tài)的一步預測：丸/k-1=k(1k-19k-1均方誤差進一步預測：PL=熊,k*ph怙u+kQkJL濾波增益矩陣：K=Pk/k4-HT_HkP3LHTRk"濾波估計方程：乂卜二_1-KIZk-(4)k?k/均方誤差更新矩

6、陣(K時刻的最優(yōu)均方誤差)：Pk=_I-K_他卜P?”上述就是卡爾曼濾波器的5條基本公式，只有給定初值Xo和R,根據k時刻的觀測值ZK,就可以遞推計算得k時刻的狀態(tài)估計Xk卜面論述卡爾曼五個公式的推導過程:設系統(tǒng)(1)Zk = %Xk vkk>=1(2)Xk:k,kXk,-kJ-k其中，動態(tài)噪聲%與量測噪聲vk是互不相關的零均值白噪聲序列，對任意k,j其基本統(tǒng)計性質為：Ek=0Cov(k,j)=EkT=Q'kjEvk=0Cov(vk,vj)=EvkvjT=RkkjCov(k,vj)=EkvT=0其中均是克羅內克6函數，即:又設初始狀態(tài)的統(tǒng)計特征為Exo=:VarXo=E（Xo-：

7、）（xo-：）T=Po且Xo與M,vk都不相關，即Cov（Xo,k）=oCov（Xo,vk）=o在量測（k-1）次之后，已經有一個"k=的估計值，要推測k狀態(tài)的狀態(tài)值，因為E°k-1=o,可定義功/J為由k-1次量測值所估計信心的一步預測合理數值，即眼/k=6k,k/Xk（3）同樣，考慮到Evk=o,因而量測的期望值為Hk*k也是合適的?？紤]到這兩點以后利用第k次的量測數據zk來估計眼的遞推形式，其應該為:必k=xk/k=xk/kI'Kk(zk-Hk)?k/kJ)(4)這里的是一個待定的增益矩陣，其應使誤差矩陣極小。接下來推導誤差方差公式。定義Xk/k-*k/kJ-

8、xk(5)冷/k=格=xk/k-xk(6)表示誤差，式（5）表示先驗（沒有測量值）的誤差，式（6）表示后驗的誤差（經測量值校正）。則，根據式（6）,將式（4）,式（5）帶入，得k=xk-xk=xk/k'Kk(Hkxkvk-Hk?k/k_1)-xk-xk/k_1xkKkHkxxKkvk-KkHkxk/k-KkHkxk-xk=(I-KkHk)/kKkvk估計誤差矩陣Pk=EKkXTKkVkT/k(I二印I一KkHk)Xk/jxT/k(I-KkHk)TvTKT-KkHk)TvkKT（7）定義Pk/k=EkT./kxk/k_1J(8)這就是一步預測誤差矩陣。由模型的統(tǒng)計性質可知EvkvT=R(

9、9)而且，預測誤差與測量噪聲不相關，即:Etxk/k_1vk=EtvkKk/k=0(10)將式（8）,式（9）,式（10）帶入式（7）,得Pk=(I-KkHk)Pk/k_1(I-KkHk)TKkRkKT(11)現(xiàn)尋找一個使式（11）最小。將式（11）右端展開后，加減同一項Pk/kHT(HkPk/kHTR)HkPk/k再把有關&的項歸在平方項里，即Pk=Pk/kj-Pk/kHT(HkPk/kHTR),HkPk/j&-Pk/k,HT(HkPk/kHTRk)(HkPk/jHkRJKk-Pk/k,H：(HkPk/kHTRQ(12)在式（12）中，欲使R極小，則Kk=Pk/kH：(HkP

10、k/kHTR)，(13)此時Pk=Pk/k-Pk/kH：(HkPk/kHT=I-KkHkPk/kjR)HkPk/j(14)這就是誤差迭代公式。由此可見，卡爾曼濾波的遞推公式即可得到濾波的估計值，又可得到誤差的方差陣。由式（3）兩邊同時減去xk得:xV/k-Xk=k,k_1xk-Xk(15)將式（1）,式（5）帶入式（15）Xk/k_J=：Jk,k-1k-1一k-1'k因此(20)Pk/k=k,kk,k'-k_1Q-k_1式（20）即一步預測估計誤差矩陣。到此，卡爾曼濾波公式推導完畢。三、卡爾曼濾波的實例一一以溫度檢測濾波為例假設我們要研究一個房間的溫度，這個房間的真實溫度是25

11、度，以一分鐘為時間單位。根據我們的經驗判斷，這個房間的溫度是恒定的（A=1）,但是對我們的經驗不是完全相信，可能存在上下幾度的偏差，我們把該偏差看做是高斯白噪聲（系統(tǒng)噪聲W）。另外，我們在房間里放一個溫度計（H=1）,溫度計也不準確,測量值會與實際值存在偏差，我們也把這偏差看做是高斯白噪聲（測量噪聲V）現(xiàn)在我們用卡爾曼濾波，過濾掉噪聲，估算出房間的實際溫度。系統(tǒng)參數名稱解釋如下：xk系統(tǒng)狀態(tài)實際溫度系統(tǒng)矩陣溫度不變，為1B、uk狀態(tài)的控制量無控制量，為0Zk觀測值溫度計讀數H觀測矩陣直接讀出，為1wk過程噪聲溫度艾化偏差，常量1e-1vk測量噪聲讀數誤差，常量1e-6假如我們要估算2時刻房間的

12、實際溫度值。首先你要根據1時刻溫度的估計值（就假設為1）,來算出2時刻溫度的估計值，即：X=4印?,-,公式1）,然后由給出的1時刻的均方差P1（假設為10）進一步更新均方差，有P2,1=如P1”歸101009屋公式2）。有方差P2,1之后，根1據卡爾曼增益方程計算出增益：K=P州HT1H州P0aHT+Rvv（*公式3）。現(xiàn)在，我們可以通過增益和測量值（Z=26.676,溫度計有測量誤差）來估計2時刻的溫度了，&=&1+KZ=-H*X2,1124.763（*公式4）。得到了2時刻的估計溫度，下一步就是對3時刻的溫度值進行最優(yōu)估算，需要得到K時刻的最優(yōu)溫度（24.56）的偏差，算

13、法如下：P2=E0.751*H】*邑1公式5）就這樣，再進入3時刻的濾波循環(huán)，卡爾曼濾波器就不斷的把均方誤差遞歸，從而估算出最優(yōu)的溫度值，運行速度快。以下是matlab程序:clearall;clc;closeall;%系統(tǒng)方程X(k)=AX(k-1)+BU(k)+W(k)%系統(tǒng)測量值Z(k)=HX(k)+V(k)N=200;w=0.1*randn(1,N);%產生隨機高斯分布x(1)=0;%賦值與否無所謂a=1;%系統(tǒng)狀態(tài)矩陣V=randn(1,N);%產生隨機高斯分布q1=std(V);Rvv=q1A2;%Rq2=std(w);Qww=q2.A2;%Qh=1;%觀測矩陣Y=25+V;%測量

14、誤差值p(1)=10;X(1)=0;%kalmanfilter%x(k)=x(k|k-1),X(k)=x(k|k),p1(k)=p(k|k-1),p(k)=p(k|k)fork=2:N;x(k)=a*X(k-1);%X(k|k-1)=AX(k-1|k-1)+BU(k),(1)p1(k)=a*p(k-1)*a'+Qww;%P(k|k-1)=AP(k-1|k-1)A+Q,(2)Kg(k)=p1(k)*h'/(h*p1(k)*h'+Rvv);%Kg(k)=P(k|k-1)H/(HP(k|k-1)H+R),(4)% X(k|k)=X(k|k-1)+Kg(k)% P(k|k)= ( I-Kg(k) H )X(k)=x(k)+Kg(k)*(Y(k)-h*x(k);(Z(k)-HX(k|k-1),(3)p(k)=p1(k)-Kg(k)*h*p1(k);P(k|k-1),(5)endfori=1:NexpValue(i)=25;endk=1:N;plot(k,X,'r',k,Y,'g',k,expValue,'b');legend('真實值','估計值','測量值');axis(0N1030)xlabel('時間');ylabel