卡爾曼濾波試驗(yàn)及matlab實(shí)現(xiàn)

1、實(shí)驗(yàn)一卡爾曼濾波、實(shí)驗(yàn)?zāi)康?、了解卡爾曼濾波的準(zhǔn)則和信號(hào)模型，以及卡爾曼濾波的應(yīng)用。2、熟練掌握卡爾曼濾波的遞推過程，提高對(duì)信號(hào)進(jìn)行處理的能力3、分析討論實(shí)際狀態(tài)值和估計(jì)值的誤差。、實(shí)驗(yàn)原理1、卡爾曼濾波簡介卡爾曼濾波是解決以均方誤差最小為準(zhǔn)則的最佳線性濾波問題，它根據(jù)前一個(gè)估計(jì)值和最近一個(gè)觀察數(shù)據(jù)來估計(jì)信號(hào)的當(dāng)前值。它是用狀態(tài)方程和遞推方法進(jìn)行估計(jì)的，而它的解是以估計(jì)值(常常是狀態(tài)變量的估計(jì)值)的形式給出其信號(hào)模型是從狀態(tài)方程和量測方程得到的?？柭^濾中信號(hào)和噪聲是用狀態(tài)方程和測量方程來表示的。因此設(shè)計(jì)卡爾曼濾波器要求已知狀態(tài)方程和測量方程。它不需要知道全部過去的數(shù)據(jù)，采用遞推的方法計(jì)算，

2、它既可以用于平穩(wěn)和不平穩(wěn)的隨機(jī)過程，同時(shí)也可以應(yīng)用解決非時(shí)變和時(shí)變系統(tǒng)，因而它比維納過濾有更廣泛的應(yīng)用。2、卡爾曼濾波的遞推公式A.AAxk-AkxkJ.Hk(yk_CkAkxkJ).Pk=AkPkAkQkPk=(I-HkCk)Pk(4)3、遞推過程的實(shí)現(xiàn)如果初始狀態(tài)Xo的統(tǒng)計(jì)特性 Ex。及 varx。已知，并A令Xo=EXo二0ci 八、,好，一，又Po=E(Xo-Xo)(XoXo)=varXo將Po代入式(3)可求得P將 P；代入式(2)可求得Hi,將此Hi代入式A(1)可求得在最小均方誤差條件下的 Xi,同時(shí)將代入式(4)又可求得 P;由P又可求 Pl,由P2又可求得H2,由H2又可求得

3、X2,同時(shí)由H2與 P；又可求得巳；以此類推，這種遞推計(jì)算方法用計(jì)算機(jī)計(jì)算十分方便。三、實(shí)驗(yàn)器材1、計(jì)算機(jī)一臺(tái)Hk=PkCk(CkPkCkRJ(2)2、MATLA 歆件一套四、實(shí)驗(yàn)內(nèi)容一個(gè)系統(tǒng)模型為x1(k1)=x1(k)x2(k)w(k),k=0,1,x2(k1)=x2(k)w(k)同時(shí)有下列條件：(1)初始條件已知且有 x(0)=0,0T。(2)w(k)是一個(gè)標(biāo)量零均值白高斯序列，且自相關(guān)函數(shù)已知為Ew(j)w(k)=%。另外，我們有下列觀測模型，即y1(k1)=x(k1)v1(k1),k=0,1,y2(k1)=x2(k1)v2(k1)且有下列條件：(3)v1(k+1)和 V2(k+1)是

4、獨(dú)立的零均值白高斯序列，且有Ev(jM(k)=cjk,EV2(j)V2(k)=2、jk,k=0,1,2(4)對(duì)于所有的 j 和 k,w(k)與觀測噪聲過程 v1(k 十 1)和 vz(k+1)是不相關(guān)的，即Ew(j)v1(k)=0,Ew(j)v2(k)=0,j=0,1,2,k=0,1,2,我們希望得到由觀測矢量y(k+1),即 y(k+1)=%*+1),丫2+1)估計(jì)狀態(tài)矢量 x(k+1)=x(k+1),x2(k+1)T的卡爾曼濾波器的公式表示形式，并求解以下問題：(a)求出卡爾曼增益矩陣，并得出最優(yōu)估計(jì)x(k+1)和觀測矢量y(1),y(2)，y(k+1)之間的遞歸關(guān)系。(b)通過一個(gè)標(biāo)量框

5、圖(不是矢量框圖)表示出狀態(tài)矢量x(k+1)中元素x1(k+1)和 x2(k+1)估計(jì)值的計(jì)算過程。(c)用模擬數(shù)據(jù)確定狀態(tài)矢量x(k)的估計(jì)值x(k/k),k=0,1，,10，并畫出當(dāng)k=0,1,，10時(shí)xi(k/k)和x2(k/k)的圖(d)通常，狀態(tài)矢量的真實(shí)值是得不到得。但為了用作圖來說明問題，表 P8.1 和 P8.2給出來狀態(tài)矢量元素得值。對(duì)于 k=0,1,，10,在同一幅圖中畫出真實(shí)值和在(c)中確定的 x(k)的估計(jì)值。對(duì) X2(k)重復(fù)這樣過程。當(dāng) k 從 1變到 10 時(shí)，對(duì)每一個(gè)元素 i=1,2,計(jì)算并畫出各自的誤差圖，即xi(k)_xk/k)。(e)當(dāng) k 從 1 變到

6、 10 時(shí)，通過用卡爾曼濾波器的狀態(tài)誤差協(xié)方差矩陣畫出22一E當(dāng)(k/k)和 ES2(k/k),而(k/k)=x1(k)一x(k/k),2(k/k)=x2(k)-x2(k/k)。(f)討論一下(d)中你計(jì)算的誤差與(e)中方差之間的關(guān)系。表 P8.1P8.1 題 8.18.1 到題 8.38.3 中的觀測值時(shí)間下標(biāo)k觀測值y1(k)觀測值y2(k)13.296919692.1013429423.387365150.4754079737.028306413.1768889849.712125212.49811140511.420183152.91992424615.978705836.17307

7、616722.069342855.42519274828.302127813.05365741930.446838315.980511411038.758755954.51016361表P8.2題8.1到題8.3中的由模擬得到的實(shí)際狀態(tài)值時(shí)間下標(biāo)k實(shí)際狀態(tài)值x1(k)實(shí)際狀態(tài)值x1(k)00.00000000000.00000000011.654287141.6542871423.503007021.8487198835.9978529242.4755222249.150407403508739103.35833170616.921925944.41318684721

8、.344833524.42290758825.893351444.54851792931.541353305.648001861036.936056705.394470340A.A.A.估計(jì)值與觀測值之間的遞歸關(guān)系為：Xk=AjXk-i+HkX(Yk_CkMAkMXk-i)(b)狀態(tài)矢量估計(jì)值的計(jì)算框圖：xk.1(c)xi(k/k)和X2(k/k)的圖:五、實(shí)驗(yàn)結(jié)果分析(a)卡爾曼增益矩陣:Hk=RMCTM(CMPkaCT+R)(d)真實(shí)值與估計(jì)值的比較圖:各自的誤差圖:(e)通過用卡爾曼濾波器的狀態(tài)誤差協(xié)方差矩陣畫出的 E軸2(Hk)和E42(k/k):附程序：%卡爾曼濾波實(shí)驗(yàn)程序clc;y

9、1=3.29691969,3.38736515,7.02830641,9.71212521,11.42018315,15.97870583,22.06934285,28.30212781,30.44683831,38.75875595;%觀測值 y1(k)y2=2.10134294,0.47540797,3.17688898,2.49811140,2.91992424,6.17307616,5.42519274,3.05365741,5.98051141,4.51016361;%觀測值 y2(k)p0=1,0;0,1;p=p0;%均方誤差陣賦初值A(chǔ)k=1,1;0,1;%轉(zhuǎn)移矩陣Qk=1,0;0

10、,1;%系統(tǒng)噪聲矩陣Ck=1,0;0,1;%量測矩陣Rk=1,0;0,2;%測量噪聲矩陣x0=0,0;xk=x0;%狀態(tài)矩陣賦初值fork=1:10Pk=Ak*p*Ak+Qk;%濾波萬程 3Hk=Pk*Ck*inv(Ck*Pk*Ck+Rk);%濾波方程 2yk=y1(k);y2(k);%觀測值xk=Ak*xk+Hk*(yk-Ck*Ak*xk);%濾波方程 1x1(k)=xk(1);x2(k)=xk(2);”36（f）分析：（e）中的方差是（d）中的誤差平方后取均值，是均方誤差。誤差直接由真實(shí)值減去估計(jì)值，有正有負(fù)，而均方誤差沒有這個(gè)缺陷，更能綜合的表示濾波的效果。%記錄估計(jì)值%濾波方程 4%記

11、錄狀態(tài)誤差協(xié)方差矩陣p=(eye(2)-Hk*Ck)*Pk;pk(:,k)=p(1,1),p(2,2);endfigure%畫圖表示狀態(tài)矢量的估計(jì)值subplot(2,1,1)i=1:10;plot(i,x1(i),k)h=legend(x1(k)的估計(jì)值)set(h,interpreter,none)subplot(2,1,2)i=1:10;plot(i,x2(i),k)h=legend(x2(k)的估計(jì)值)set(h,interpreter,none)X1=0,1.65428714,3.50300702,5.997852924,9.15040740,12.50873910,16.92192

12、594,21.34483352,25.89335144,31.54135330,36.93605670;%由模擬得到的實(shí)際狀態(tài)值X1(k)X2=0,1.65428714,1.84871988,2.47552222,3.17187816,3.35833170,4.41318684,4.42290758,4.54851792,5.64800186,5.394470340;%由模擬得至 U 的實(shí)際狀態(tài)值 X2(k)figure%在同一幅圖中畫出狀態(tài)矢量的估計(jì)值與真實(shí)值subplot(2,1,1)i=1:10;plot(i,x1(i),k,i,X1(i+1),b)h=legend(x1(k)的估計(jì)值,

13、x1(k)的真實(shí)值)set(h,interpreter,none)subplot(2,1,2)i=1:10;plot(i,x2(i),k,i,X2(i+1),b)h=legend(x2(k)的估計(jì)值,x2(k)的真實(shí)值)set(h,interpreter,none)fori=1:10%計(jì)算 x(k)的誤差e1(i)=X1(i+1)-x1(i);e2(i)=X2(i+1)-x2(i);endfigure%畫出誤差圖subplot(2,1,1)i=1:10;plot(i,e1(i),r)h=legend(x1(k)的誤差)set(h,interpreter,none)subplot(2,1,2)i=1:10;plot(i,e2(i),r)h=legend(x2(k)的誤差)set(h,interpreter,none)figure%通過用卡爾曼濾波器的狀態(tài)誤差協(xié)方差矩陣畫出