卡爾曼濾波器及其簡(jiǎn)matlab仿真

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

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

該為：（4）x=x=x+K（z—Hx）（4）k k/k k/k—1kkkk/k—1這里的Kk是一個(gè)待定的增益矩陣，其應(yīng)使誤差矩陣極小。接下來(lái)推導(dǎo)誤差方差公式。定義（5）x二x一x（5）k/k一1k/k一1kk/k二x二x一xk k/kk（6）其中“?”表示誤差，式（5）表示先驗(yàn)（沒(méi)有測(cè)量值）的誤差，式（6）k/k二x二x一xk k/kk（6）其中“?”表示誤差，式（5）表示先驗(yàn)（沒(méi)有測(cè)量值）的誤差，式（6）表示后驗(yàn)的誤差（經(jīng)測(cè)量值校正）。則，根據(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估計(jì)誤差矩陣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ù)測(cè)誤差矩陣。由模型的統(tǒng)計(jì)性質(zhì)可知（8）E[vkvkT]=Rk （9）而且，預(yù)測(cè)誤差與測(cè)量噪聲不相關(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)尋找一個(gè)Kk使式（11）最小。將式（11）右端展開(kāi)后，加減同一項(xiàng)PHt（HPHt+R）―1HPk/k一1kkk/k一1kkkk/k一1再把有關(guān)Kk的項(xiàng)歸在平方項(xiàng)里，即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此時(shí)P二P-PHt(HPHtkk/k-1k/k-1kkk/k-1k二[I-KH]Pkkk/k-1這就是誤差迭代公式。由此可見(jiàn)，的估計(jì)值，又可得到誤差的方差陣。(13)+R)-1HPkkk/k-1(14)卡爾曼濾波的遞推公式即可得到濾波(15)(20)(15)(20)由式(3)兩邊同時(shí)減去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ù)測(cè)估計(jì)誤差矩陣。到此，卡爾曼濾波公式推導(dǎo)完畢。三、卡爾曼濾波的實(shí)例——以溫度檢測(cè)濾波為例假設(shè)我們要研究一個(gè)房間的溫度，這個(gè)房間的真實(shí)溫度是25度，以一分鐘為時(shí)間單位。根據(jù)我們的經(jīng)驗(yàn)判斷，這個(gè)房間的溫度是恒定的(A=1),但是對(duì)我們的經(jīng)驗(yàn)不是完全相信，可能存在上下幾度的偏差，我們把該偏差看做是高斯白噪

聲（系統(tǒng)噪聲W）。另外，我們?cè)诜块g里放一個(gè)溫度計(jì)（H=1）,溫度計(jì)也不準(zhǔn)確，測(cè)量值會(huì)與實(shí)際值存在偏差，我們也把這偏差看做是高斯白噪聲（測(cè)量噪聲V）?，F(xiàn)在我們用卡爾曼濾波，過(guò)濾掉噪聲，估算出房間的實(shí)際溫度。系統(tǒng)參數(shù)名稱解釋如下：xkG系統(tǒng)狀態(tài)系統(tǒng)矩陣xkG系統(tǒng)狀態(tài)系統(tǒng)矩陣B、uk 狀態(tài)的控制量Zk 觀測(cè)值H 觀測(cè)矩陣wk 過(guò)程噪聲vk 測(cè)量噪聲實(shí)際溫度溫度不變，為1無(wú)控制量，為0溫度計(jì)讀數(shù)直接讀出，為1溫度變化偏差，常量1e-1讀數(shù)誤差，常量1e-6假如我們要估算2時(shí)刻房間的實(shí)際溫度值。首先你要根據(jù)1時(shí)刻溫度的估TOC\o"1-5"\h\z八 xk 八計(jì)值（就假設(shè)為1），來(lái)算出2時(shí)刻溫度的估計(jì)值，即：X=9*X=12,1 1（*公式1），然后由給出的1時(shí)刻的均方差P1（假設(shè)為10）進(jìn)一步更新均方差，有P=9*p*9t+rqrt=10.009（*公式2）。有方差P之后，根2,1 1 k-1 2,1據(jù)卡爾曼增益方程計(jì)算出增益：K=P*HtH*P*Ht+R-1=0.925（*2L2,1 vv一公式3）。現(xiàn)在，我們可以通過(guò)增益和測(cè)量值（Z=26.676,溫度計(jì)有測(cè)量誤差）來(lái)估計(jì)2時(shí)刻的溫度了，X=X+K「Z-H*X] =24.763（*\o"CurrentDocument"2 2,1 2,1公式4）。得到了2時(shí)刻的估計(jì)溫度，下一步就是對(duì)3時(shí)刻的溫度值進(jìn)行最優(yōu)估算，需要得到K時(shí)刻的最優(yōu)溫度（24.56）的偏差，算法如下：P=昆-K*H]*P =0.751（*公式5）2 2,1就這樣，再進(jìn)入3時(shí)刻的濾波循環(huán)，卡爾曼濾波器就不斷的把均方誤差遞歸，從而估算出最優(yōu)的溫度值，運(yùn)行速度快。以下是matlab程序:

clearall;clc;closeall;%系統(tǒng)方程X(k)=AX(k-1)+BU(k)+W(k)%系統(tǒng)測(cè)量值Z(k)=HX(k)+V(k)N=200;w=0.1*randn(1,N);%產(chǎn)生隨機(jī)高斯分布x(1)=0;%賦值與否無(wú)所謂a=1; %系統(tǒng)狀態(tài)矩陣V=randn(1,N); %產(chǎn)生隨機(jī)高斯分布q1=std(V);Rvv=q1「2; %Rq2=std(w);Qww=q2「2; %Qh=1; %觀測(cè)矩陣Y=25+V; %測(cè)量誤差值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