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

2、ear Filtering and Prediction Problems線 性濾波與預測問題的新方法,在這篇文章里一種克服了維納濾波缺點的新方法 被提出來,這就是我們今天稱之為卡爾曼濾波的方法.卡爾曼濾波應用廣泛且功能強大,它可以估計信號的過去和當前狀態甚至能估計將來的狀態即使并不知道 模型確實切性質.其根本思想是以最小均方誤差為最正確估計準那么,采用信號與噪聲的狀態空 間模型利用前一時刻的估計值和當前時刻的觀測值來更新對狀態變量的估計,求出當前時刻的估計值.算法根據建立的系統方程和觀測方程對需要處理的信號做 出滿足最小均方誤差的估計.對于解決很大局部的問題,它是最優,效率最高甚至是最有用的

3、.它的廣泛 應用已經超過30年,包括機器人導航,限制,傳感器數據融合甚至在軍事方面 的雷達系統以及導彈追蹤等等.近年來更被應用于電腦圖像處理,例如頭臉識別, 圖像分割,圖像邊緣檢測等等.卡爾曼濾波不要求保存過去的測量數據, 當新的數據到來時,根據新的數據 和前一時刻的儲值的估計,借助于系統本身的狀態轉移方程,根據一套遞推公式, 即可算出新的估值.卡爾曼遞推算法大大減少了濾波裝置的存儲量和計算量,并且突破了平穩隨機過程的限制,使卡爾曼濾波器適用于對時變信號的實時處理.卡爾曼濾波的原理卡爾曼濾波思想的來源是在海圖作業中,航海長通常以前一時刻的船位為基 準,根據航向、船速和海流等一系列因素推算下一個

4、船位, 但是他并不輕易認為 船位就一定在推算船位上,還要選擇適當的方法,通過儀器得到另一個推算船位. 觀測和推算這兩個船位一般不重合,航海長需要通過分析和判斷選擇一個可靠的 船位,作為船艦當前的位置.就是以現時刻的最正確估計為在前一時刻的最正確 估計的根底上根據現時刻的觀測值作線性修正.卡爾曼濾波在數學上是一種線性最小方差統計估算方法,它是通過處理一系 列帶有誤差的實際測量數據而得到物理參數的最正確估算.其實質要解決的問題是要尋找在最小均方誤差下Xk的估計值Xk.它的特點是可以用遞推的方法計算 Xk,其所需數據存儲量較小,便于進行實時處理.具體來說,卡爾曼濾波就是要用預測方程和測量方程對系統狀

5、態進行估計.設動態系統的狀態方程和測量方程分別為：X KK,K 1XK 1 K ,K 1WK 1Zk HkXk Vk上兩式子中,Xk是k時刻的系統狀態,k,k 1和k,k 1是k-1時刻到k時亥IJ的狀態轉移矩陣,Zk是k時刻的測量值,Hk是測量系統的參數,Wk和Vk分別 表示過程和測量的噪聲,他們被假設成高斯白噪聲.如果被估計狀態和觀測量是 滿足上述第一式,系統過程噪聲和觀測噪聲滿足第二式的假設,k時刻的觀測Xk 的估計X可按下述方程求解.狀態的一步預測：丸k / k 1k(1k 1 女k 1均方誤差進一步預測：Pk/k 1 k,k 1 Pk 1(2/1 k 1Qk 1 k 11濾波增益矩陣

6、：KPk/k1 H T H k故1 hT Rk濾波估計方程：Xk?k/k 1 K Z k帆k/k 1均方誤差更新矩陣K時刻的最優均方誤差：PkI K (5)F?/k 1上述就是卡爾曼濾波器的5條根本公式,只有給定初值Xo和R,根據k時刻的觀測值Zk ,就可以遞推計算得k時刻的狀態估計Xk.卜面論述卡爾曼五個公式的推導過程:設系統x kk,k 1xk 1 k 1 k 11zkHkxkvk k>=12其中,動態噪聲 k與量測噪聲Vk是互不相關的零均值白噪聲序列, 對任意k,j其根本統計性質為：kjE k = 0 Cov( k, j)=E k j=QEVk = 0Cov( Vk, Vj)=EV

7、kVj = Rk kjCov( k,Vj)=E kVT=0其中kj是克羅內克函數,即：,Jo if 31 it Z = J,又設初始狀態的統計特征為Exo=同 Var xo = E( xo-曲)(xo/o)T=Po且凡與 k, Vk都不相關,即Cov(x0, k)=0Cov(x0, Vk)=0在量測k-1次之后,已經有一個x?k 1 x?k 1/k 1的估計值,要推測k狀 態的狀態值,由于E k-1 = 0,可定義溫/k 1為由k-1次量測值所估計值x?k 1的 一步預測合理數值,即xk / k 1k,k 1xk 13同樣,考慮到EVk = 0 , 因而量測的期望值為1也是適宜的.考慮到這兩點

8、以后利用第k次的量測數據zk來估計xk的遞推形式,其應該為:Xk琳 / k刈 / k 1Kk(zkHkXk/k 1)這里的&是一個待定的增益矩陣,其應使誤差矩陣極小.接下來推導誤差方差公式.定義k / k 1X?k/ k 1 Xk6其中“表示誤差,式5表示先驗沒有測量值的誤差,式6表示后驗的誤差經測量值校正o那么,根據式6,將式4,式5帶入,得k 溫 XkXk(I/ k 1 Xkxk / k 1KkHkXxKk(HkXkVkKkVkKM4HkXk/ k 1 ) XkKkHkXkXkKkHk/k 1KkVkk / k4 踩 / k X k估計誤差矩陣Pk曰&犬：KUM/k 1(I

9、印 IKkHk)&/k 1M/kKkHk)TvkKT kkkk1(IKkHk )TkkT |/T 1Vk Kk 7定義Pk/k 1Ek/kXk / k 18這就是一步預測誤差矩陣.由模型的統計性質可知EVkvT= Rk而且,預測誤差與測量噪聲不相關,即:日X“k MTEVk£/k 1010將式8,式9,式10帶入式7,得11Pk(IKk%)Pk/k1(IKk%)TKkRkKT現尋找一個&使式11最小.將式11右端展開后,加減同一項Pk/k1HT(HkPk/k1HTRk)1HkPk/k 1再把有關人的項歸在平方項里,即PkPk/kiPk/k iHT(%Pk/k 1HT

10、R)1HkPk/ki&Pk/kiHT(HkPk/k iHTRk)1(HkPk/k iHk R)KkPk/kiH：(HkPk/k iHTRk)i在式M2中,欲使R極小,那么Kk Pk/k iHT(HkPk/k iHT R) i此時PkPk/k iPk/k iHT(HkPk/k iHTRk) iHkPk/ki4這就是誤差迭代公式.由此可見,卡爾曼濾波的遞推公式即可得到濾波IKk%Pk/k i的估計值,又可得到誤差的方差陣.由式3兩邊同時減去Xk得:xk / k i - xkk,k ixk i xk(i5)帶入式i5“k / k ik, k i*k因此Pk/k ik , k iPk ik,k

11、 i k iQT ki20式20即一步預測估計誤差矩陣.到此,卡爾曼濾波公式推導完畢三、 卡爾曼濾波的實例一一以溫度檢測濾波為例假設我們要研究一個房間的溫度,這個房間的真實溫度是 25度,以一分鐘為 時間單位.根據我們的經驗判斷,這個房間的溫度是恒定的 A=i,但是對我們 的經驗不是完全相信,可能存在上下幾度的偏差,我們把該偏差看做是高斯白噪聲系統噪聲 W.另外,我們在房間里放一個溫度計H=1,溫度計也不準確, 測量值會與實際值存在偏差,我們也把這偏差看做是高斯白噪聲測量噪聲V現在我們用卡爾曼濾波,過濾掉噪聲,估算出房間的實際溫度.系統參數名稱解釋如下：B、ukZkH系統狀態系統矩陣狀態的限制

12、量觀測值觀測矩陣過程噪聲測量噪聲實際溫度溫度不變,為1無限制量,為0溫度計讀數直接讀出,為1溫度變化偏差,常量1e-1讀數誤差,常量1e-6假設我們要估算2時刻房間的實際溫度值.首先你要根據1時刻溫度的估計值就假設為1,來算出2時刻溫度的估計值,即： 父 =1兄 ,*公式1,然后由給出的1時刻的均方差P1假設為10進一步更新均方差,有,P1T Q T 1 *公式2.有方差之加2,恨據卡爾曼1增益方程計算出增益：K P2 HT H 斡HT R*公式3.現在,我們可以通過增益和測量值Z,溫度計有測量誤差來估計 2時刻的溫度了,/&1 K Z H 孩1 *公式4.得到了 2時刻的估計溫度,下

13、一步就是對3時刻的溫度值進行最優估算,需要得到K時刻的最優溫度24.56的偏差,算法如下：P2E K H 包* 公式 5,就這樣,再進入3時刻的濾波循環,卡爾曼濾波器就不斷的把均方誤差遞歸,從而估算出最優的溫度值,運行速度快.以下是matlab程序:clear all;clc;close all;粽統方程 X(k)=AX(k-1)+BU(k)+W(k) 粽統測量值Z(k)=HX(k)+V(k)N=200;w=0.1*randn(1,N);峻生隨機高斯分布x(1)=0; %賦值與否無所謂a=1; %系統狀態矩陣V=randn(1,N);%產生隨機高斯分布q1=std(V);Rvv=q1A2;%R

14、q2=std(w);Qww=q2 A2; %Qh=1;%觀測矩陣Y=25+V;% 測量誤差值p(1)=10;X(1)=0;% kalman filter %x(k)=x(k|k-1),X(k)=x(k|k),p1(k)=p(k|k-1),p(k)=p(k|k)for k=2:N;x(k)=a*X(k-1);% X(k|k-1)=A X(k-1|k-1)+B U(k) (1)p1(k)=a*p(k-1)*a'+Qww;% P(k|k-1)=A P(k-1|k-1) A ' +Q (2)Kg(k)=p1(k)*h'/(h*p1(k)*h'+Rvv);% Kg(k)=

15、 P(k|k- 1) H'/ (HP(k|k- 1) H ' + R) (4)X(k)=x(k)+Kg(k)*(Y(k)-h*x(k);% X(k|k)=X(k|k-1)+Kg(k)(Z(k)-H X(k|k- 1)p(k)=p1(k)-Kg(k)*h*p1(k);% P(k|k)= I-Kg(k) HP(k|k-1) (5)endfor i = 1: NexpValue(i) = 25;endk=1:N;plot(k,X,'r',k,Y,'g',k,expValue,'b');legend('真實值','估計值',測量值)；axis