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

卡爾曼濾波器及其簡matlab仿真一、卡爾曼濾波的起源談到信號的分析與處理，就離不開濾波兩個字。通常，信號的頻譜處于有限的頻率范圍內(nèi)，而噪聲的頻譜則散布在很廣的頻率范圍內(nèi)，為了消除噪聲，可以進行頻域濾波。但在許多應(yīng)用場合，需要直接進行時域濾波，從帶噪聲的信號中提取有用信號。雖然這樣的過程其實也算是對信號的濾波，但其所依據(jù)的理論，即針對隨機信號的估計理論，是自成體系的。人們對于隨機信號干擾下的有用信號不能“確知”，只能“估計”。為了“估計”，要事先確定某種準則以評定估計的好壞程度。1960年卡爾曼發(fā)表了用遞歸方法解決離散數(shù)據(jù)線性濾波問題的論文ANewApproachtoLinearFilteringandPredictionProblem9線性濾波與預(yù)測問題的新方法），在這篇文章里一種克服了維納濾波缺點的新方法被提出來，這就是我們今天稱之為卡爾曼濾波的方法。卡爾曼濾波應(yīng)用廣泛且功能強大，它可以估計信號的過去和當前狀態(tài)甚至能估計將來的狀態(tài)即使并不知道模型的確切性質(zhì)。其基本思想是以最小均方誤差為最佳估計準則，采用信號與噪聲的狀態(tài)空間模型利用前一時刻的估計值和當前時刻的觀測值來更新對狀態(tài)變量的估計，求出當前時刻的估計值。算法根據(jù)建立的系統(tǒng)方程和觀測方程對需要處理的信號做出滿足最小均方誤差的估計。對于解決很大部分的問題，它是最優(yōu)，效率最高甚至是最有用的。它的廣泛應(yīng)用已經(jīng)超過30年，包括機器人導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達系統(tǒng)以及導(dǎo)彈追蹤等等。近年來更被應(yīng)用于計算機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等?？柭鼮V波不要求保存過去的測量數(shù)據(jù)，當新的數(shù)據(jù)到來時，根據(jù)新的數(shù)據(jù)和前一時刻的儲值的估計，借助于系統(tǒng)本身的狀態(tài)轉(zhuǎn)移方程，按照一套遞推公式，即可算出新的估值?？柭f推算法大大減少了濾波裝置的存儲量和計算量，并且突破了平穩(wěn)隨機過程的限制，使卡爾曼濾波器適用于對時變信號的實時處理。二、卡爾曼濾波的原理卡爾曼濾波思想的來源是在海圖作業(yè)中，航海長通常以前一時刻的船位為基準，根據(jù)航向、船速和海流等一系列因素推算下一個船位，但是他并不輕易認為船位就一定在推算船位上，還要選擇適當?shù)姆椒ǎㄟ^儀器得到另一個推算船位。觀測和推算這兩個船位一般不重合，航海長需要通過分析和判斷選擇一個可靠的船位，作為船艦當前的位置。就是以現(xiàn)時刻的最佳估計為在前一時刻的最佳估計的基礎(chǔ)上根據(jù)現(xiàn)時刻的觀測值作線性修正。卡爾曼濾波在數(shù)學(xué)上是一種線性最小方差統(tǒng)計估算方法，它是通過處理一系列帶有誤差的實際測量數(shù)據(jù)而得到物理參數(shù)的最佳估算。其實質(zhì)要解決的問題是要尋找在最小均方誤差下X的估計值父。它的特點是可以用遞推的方法計算KKX，其所需數(shù)據(jù)存儲量較小，便于進行實時處理。具體來說，卡爾曼濾波就K是要用預(yù)測方程和測量方程對系統(tǒng)狀態(tài)進行估計。設(shè)動態(tài)系統(tǒng)的狀態(tài)方程和測量方程分別為：x:①x+rwK K,K-1K-1K,K-1K-1Z=HX+VKKKK上兩式子中，X是k時刻的系統(tǒng)狀態(tài)，①和r 是k-1時刻到k時刻K K,K-1K,K-1的狀態(tài)轉(zhuǎn)移矩陣，z是k時刻的測量值，h是測量系統(tǒng)的參數(shù)，w和V分別K K KK表示過程和測量的噪聲，他們被假設(shè)成高斯白噪聲。如果被估計狀態(tài)和觀測量是滿足上述第一式，系統(tǒng)過程噪聲和觀測噪聲滿足第二式的假設(shè)k時刻的觀測XKTOC\o"1-5"\h\z的估計X可按下述方程求解。 ?人 人 人x=Q*x狀態(tài)的一步預(yù)測： k/k-1 k,k-1 k-1 （1）/X均方誤差進一步預(yù)測： p=°*p*°T+rQrT （2）k/k-1 k,k-1k-1k,k-1 k-1k-1k-1濾波增益矩陣： K=P*HTrH*p*HT+R]-1 ⑶k/k-1 kL.-kk/k-1 kk一濾波估計方程： x=x+kLz-H*x」 （4）k k/k-1 kkk/k-1均方誤差更新矩陣（K時刻的最優(yōu)均方誤差）P=[I-K*H]*P （5）k kk/k-1上述就是卡爾曼濾波器掙條基本公式，只有給定初值X和P,根據(jù)k時刻00的觀測值Z,就可以遞推計算得k時刻的狀態(tài)估計父。KK

下面論述卡爾曼五個公式的推導(dǎo)過程：設(shè)系統(tǒng)（1）（2）x=①x+「3k k,k-1k-1 k-1k-（1）（2）z=Hx+vkkkk k>=1其中，動態(tài)噪聲{3目與量測噪聲{vk}是互不相關(guān)的零均值白噪聲序列，對任意k,j其基本統(tǒng)計性質(zhì)為：E{3k}=0Cov(3k,?)=E[3k3T]=Qk5kjE{vk}=0 Cov(Vk,Vj)=E[VkVT]=Rk5kjCov(3k,vj)=E[3kvjT]=0其中5口是克羅內(nèi)克5函數(shù)，即：二上打JVif—，，又設(shè)初始狀態(tài)的統(tǒng)計特征為E[xJ*OVarx0=E{(x0—乂。)(x0-2u)t}=P0且乂0與{33，{,}都不相關(guān)，即Cov(x0,3k)=0Cov(x0,vk)=0在量測(k-1)次之后，已經(jīng)有一個xk一1二xk一1/k一1的估計值，要推測k狀態(tài)的狀態(tài)值，因為E{3k-J=0,可定義xk/k一1為由k-1次量測值所估計值\.1的一步預(yù)測合理數(shù)值，即X_=①_X_k/k—1 k,k—1k—1 (3)同樣，考慮到E{vk}=0,因而量測的期望值為Hkxk.1也是合適的?？紤]到這兩點以后利用第k次的量測數(shù)據(jù)zk來估計Xk的遞推形式，其應(yīng)

該為：（4）x=x=x+K（z—Hx）（4）k k/k k/k—1kkkk/k—1這里的Kk是一個待定的增益矩陣，其應(yīng)使誤差矩陣極小。接下來推導(dǎo)誤差方差公式。定義（5）x二x一x（5）k/k一1k/k一1kk/k二x二x一xk k/kk（6）其中“?”表示誤差，式（5）表示先驗（沒有測量值）的誤差，式（6）k/k二x二x一xk k/kk（6）其中“?”表示誤差，式（5）表示先驗（沒有測量值）的誤差，式（6）表示后驗的誤差（經(jīng)測量值校正）。則，根據(jù)式（6），將式（4），式（5）帶入，得x—x一x—x+K（Hx+v—Hx）一xkkk k/k一1 kkkkkk/k一1k—5~ +x+KHx+Kv—KH5（ —KHx—xk/k一1 kkkxkkkkk/k一1kkkk—（I一KH）x+Kvkkk/k一1kk估計誤差矩陣P— E[x xt]—E{（I 一KH ）x [xt （I -KH ）t +vtKt]+k kk kkk/k_1k/k一1 kk kkKv[xt （I一KH）t+vKt]}kkk/k一1 kk kk（7）定義P—E[xx xxT]k/k一1 k/k一1k/k一1這就是一步預(yù)測誤差矩陣。由模型的統(tǒng)計性質(zhì)可知（8）E[vkvkT]=Rk （9）而且，預(yù)測誤差與測量噪聲不相關(guān)，即：E[x vT]—E[vxxT ]—0k/k一1k kk/k一1（10）將式（8），式（9），式（10）帶入式（7），得P—（I一KH）P （I一KH）t+KRKtk kkk/k一1 kk kkk（11）現(xiàn)尋找一個Kk使式（11）最小。將式（11）右端展開后，加減同一項PHt（HPHt+R）―1HPk/k一1kkk/k一1kkkk/k一1再把有關(guān)Kk的項歸在平方項里，即P=P—PHt(HP Ht+R)-iHP+k k/k-1 k/k-1k kk k/k-1 k/k-1k k[K-P HT(HP HTk k/k-1 k kk/k-1 k[K-P HT(HP HTk k/k-1 k kk/k-1 k(12)+R)-i](HPH+R)k kk/k-1kk+R)-1]kK=PHt(HPK=PHt(HPHt+R)-1k k/k-1kkk/k-1kk此時P二P-PHt(HPHtkk/k-1k/k-1kkk/k-1k二[I-KH]Pkkk/k-1這就是誤差迭代公式。由此可見，的估計值，又可得到誤差的方差陣。(13)+R)-1HPkkk/k-1(14)卡爾曼濾波的遞推公式即可得到濾波(15)(20)(15)(20)由式(3)兩邊同時減去xk得：x-x=①x-xk/k-1 k k,k-1k-1 k將式(1)，式(5)帶入式(15)戈二①義-r3k/k-1 k,k-1k-1k-1k-1因此p=①p①t +rQ0k/k-1 k,k-1k-1k,k-1 k-1k-1k-1式(20)即一步預(yù)測估計誤差矩陣。到此，卡爾曼濾波公式推導(dǎo)完畢。三、卡爾曼濾波的實例——以溫度檢測濾波為例假設(shè)我們要研究一個房間的溫度，這個房間的真實溫度是25度，以一分鐘為時間單位。根據(jù)我們的經(jīng)驗判斷，這個房間的溫度是恒定的(A=1),但是對我們的經(jīng)驗不是完全相信，可能存在上下幾度的偏差，我們把該偏差看做是高斯白噪

聲（系統(tǒng)噪聲W）。另外，我們在房間里放一個溫度計（H=1）,溫度計也不準確，測量值會與實際值存在偏差，我們也把這偏差看做是高斯白噪聲（測量噪聲V）?，F(xiàn)在我們用卡爾曼濾波，過濾掉噪聲，估算出房間的實際溫度。系統(tǒng)參數(shù)名稱解釋如下：xkG系統(tǒng)狀態(tài)系統(tǒng)矩陣xkG系統(tǒng)狀態(tài)系統(tǒng)矩陣B、uk 狀態(tài)的控制量Zk 觀測值H 觀測矩陣wk 過程噪聲vk 測量噪聲實際溫度溫度不變，為1無控制量，為0溫度計讀數(shù)直接讀出，為1溫度變化偏差，常量1e-1讀數(shù)誤差，常量1e-6假如我們要估算2時刻房間的實際溫度值。首先你要根據(jù)1時刻溫度的估TOC\o"1-5"\h\z八 xk 八計值（就假設(shè)為1），來算出2時刻溫度的估計值，即：X=9*X=12,1 1（*公式1），然后由給出的1時刻的均方差P1（假設(shè)為10）進一步更新均方差，有P=9*p*9t+rqrt=10.009（*公式2）。有方差P之后，根2,1 1 k-1 2,1據(jù)卡爾曼增益方程計算出增益：K=P*HtH*P*Ht+R-1=0.925（*2L2,1 vv一公式3）?，F(xiàn)在，我們可以通過增益和測量值（Z=26.676,溫度計有測量誤差）來估計2時刻的溫度了，X=X+K「Z-H*X] =24.763（*\o"CurrentDocument"2 2,1 2,1公式4）。得到了2時刻的估計溫度，下一步就是對3時刻的溫度值進行最優(yōu)估算，需要得到K時刻的最優(yōu)溫度（24.56）的偏差，算法如下：P=昆-K*H]*P =0.751（*公式5）2 2,1就這樣，再進入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);%產(chǎn)生隨機高斯分布x(1)=0;%賦值與否無所謂a=1; %系統(tǒng)狀態(tài)矩陣V=randn(1,N); %產(chǎn)生隨機高斯分布q1=std(V);Rvv=q1「2; %Rq2=std(w);Qww=q2「2; %Qh=1; %觀測矩陣Y=25+V; %測量誤差值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’/(H

Kg(k)=p1(k)*h'/(h*p1(k)*h'+Rvv);%Kg(k)=P(k|k-1)H’/(HP(k|k-1)H’+P(k|k-1)H’+R)(4)%X(k|k)=X(k|k-1)+Kg(k)%X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))………(3)p(k)=p1(k)-Kg(k)*h*p1(k); %P(k|k)=(I-Kg