卡爾曼濾波的MATLAB實(shí)現(xiàn)

1、卡爾曼濾波的 MATLA 實(shí)現(xiàn)一、實(shí)驗(yàn)內(nèi)容一個系統(tǒng)模型為xi(k 1) xi(k) X2(k)w(k), k 0,1,x2(k 1) x2(k) w(k)同時有下列條件：(1)初始條件已知且有x(0) 0, 0T。(2)w(k)是一個標(biāo)量零均值白高斯序列，且自相關(guān)函數(shù)已知為Ew(j)w(k)jk。另外，我們有下列觀測模型，即%(k1) xdk1)V1(k 1),k 0,1,y2(k1)X2(k 1)V2(k 1)且有下列條件：(3)v,k 1)和V2(k 1)是獨(dú)立的零均值白高斯序列，且有Ev1(j)v1(k)jk,Ev2(j)v2(k)2jk, k 0,1, 2,(4)對于所有的 j 和 k

2、，w(k)與觀測噪聲過程w(k 1)和v2(k 1)是不相關(guān)的， 即Ew(j)w(k)0,Ew(j)v2(k) 0, j 0,1,2, k 0, 1, 2,我們希望得到由觀測矢量y(k 1)，即y(k 1) y1(k 1), y2(k 1)T估計狀態(tài)矢量x(k 1) xk 1),X2(k 1)T的卡爾曼濾波器的公式表示形式，并求解以下問 題：(a)求出卡爾曼增益矩陣，并得出最優(yōu)估計x(k 1)和觀測矢量y(1), y(2),，y(k 1)之間的遞歸關(guān)系。(b)通過一個標(biāo)量框圖(不是矢量框圖)表示出狀態(tài)矢量x(k 1)中元素x1(k 1)和x2(k 1)估計值的計算過程。(c)用模擬數(shù)據(jù)確定狀態(tài)

3、矢量x(k)的估計值x(k k), k 0,1,.,10,并畫出當(dāng) k=0, 1,，10 時xi(k k)和X2(k k)的圖。(d)通常， 狀態(tài)矢量的真實(shí)值是得不到得。 但為了用作圖來說明問題， 表 P8.1 和 P8.2給出來狀態(tài)矢量元素得值。對于 k = 0, 1,，10,在同一幅圖 中畫出真實(shí)值和在(c)中確定的x1(k)的估計值。對X2(k)重復(fù)這樣過程。當(dāng) k 從 1 變到 10 時，對每一個元素 i 二 1, 2,計算并畫出各自的誤差圖，即xi(k) xi(k k)。(e)當(dāng) k 從 1 變到 10 時，通過用卡爾曼濾波器的狀態(tài)誤差協(xié)方差矩陣畫出2 2E1(k. k)和E2(k

4、k), 而jk k) x,k) X1(k k),2(kk) x2(k) X2(k k)。(f)討論一下(d)中你計算的誤差與(e)中方差之間的關(guān)系。表 P8.1P8.1 題 8.18.1 到題 8.38.3 中的觀測值時間下標(biāo) k觀測值y/k)觀測值y2(k)13.296919692.1013429423.387365150.4754079737.028306413.1768889849.712125212.49811140511.420183152.91992424615.978705836069342855.42519274828.302127813.053657

5、41930.446838315.980511411 038.758755954.51016361表 P8.2 題 8.1 到題 8.3 中的由模擬得到的實(shí)際狀態(tài)值時間下標(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.344833524.42290758825.893351444.54851

6、792931.541353305.648001861 036.936056705.394470340、實(shí)驗(yàn)原理1、卡爾曼濾波簡介 卡爾曼濾波是解決以均方誤差最小為準(zhǔn)則的最佳線性濾波問題，它根據(jù)前一 個估計值和最近一個觀察數(shù)據(jù)來估計信號的當(dāng)前值。 它是用狀態(tài)方程和遞推方法 進(jìn)行估計的， 而它的解是以估計值 （常常是狀態(tài)變量的估計值） 的形式給出其信號模型是從狀態(tài)方程和量測方程得到的?？柭^濾中信號和噪聲是用狀態(tài)方程和測量方程來表示的。 因此設(shè)計卡爾 曼濾波器要求已知狀態(tài)方程和測量方程。 它不需要知道全部過去的數(shù)據(jù)， 采用遞 推的方法計算， 它既可以用于平穩(wěn)和不平穩(wěn)的隨機(jī)過程， 同時也可以應(yīng)用解

7、決非 時變和時變系統(tǒng)，因而它比維納過濾有更廣泛的應(yīng)用。2、卡爾曼濾波的遞推公式xkAkxk 1Hk(ykCkAkxk 1)如果初始狀態(tài)X。的統(tǒng)計特性Ex。及varxo已知，并x0Ex0P0E(x0 x0)(x0 x0) varx0將Po代入式（3）可求得pi，將pi代入式（2）可求得Hi，將此Hi代入式（1）可求得在最小均方誤差條件下的x1，同時將P1代入式（ 4）又可求得P1； 由P又可求P2，由p2又可求得H2，由H2又可求得x2，同時由H2與p2又可求 得P2；以此類推，這種遞推計算方法用計算機(jī)計算十分方便。三、MATLA程序%卡爾曼濾波實(shí)驗(yàn)程序clc;yi=3.2969i969,3.3

8、87365i5,7.0283064i,9.7i2i252i,ii.420i83i5,i5.97870583PkCk(CkPkCkRk)(2)PkAkPk 1AkQk 1(3)Pk（I HOPk .3、遞推過程的實(shí)現(xiàn)(4)(1),22.06934285,28.302i278i,30.4468383i,38.75875595; %觀測值 yi(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

9、,0;0,1;p=p0;%均方誤差陣賦初值A(chǔ)k=1,1;0,1;%轉(zhuǎn)移矩陣Qk=1,0;0,1;%系統(tǒng)噪聲矩陣Ck=1,0;0,1;%量測矩陣Rk=1,0;0,2;%測量噪聲矩陣x0=0,0;xk=x0;%狀態(tài)矩陣賦初值for k=1:10Pk=Ak*p*Ak+Qk;%濾波方程 3Hk=Pk*Ck*inv(Ck*Pk*Ck+Rk); %濾波方程 2 yk=y1(k);y2(k); %觀測值xk=Ak*xk+Hk*(yk-Ck*Ak*xk);%濾波方程 1x1(k)=xk(1);x2(k)=xk(2);p=(eye(2)-Hk*Ck)*Pk;pk(:,k)=p(1,1),p(2,2);endfi

10、gure%畫圖表示狀態(tài)矢量的估計值subplot(2,1,1)i=1:10;plot(i,x1(i),k)h=legend(x1(k)的估計值) set(h,interpreter,none)subplot(2,1,2) i=1:10;plot(i,x2(i),k)h=legend(x2(k)的估計值) set(h,interpreter,none)X1=0,1.65428714,3.50300702,5.997852924,9.15040740,12.50873910,16.92192594,21.34483352,25.89335144,31.54135330,36.93605670; %

11、由模擬得到的 實(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;%由模擬得到的實(shí)際狀態(tài)值 X2(k)figure%在同一幅圖中畫出狀態(tài)矢量的估計值與真實(shí)值subplot(2,1,1)i=1:10;plot(i,x1(i),k,i,X1(i+1),b)h=legend(x1(k)的估計值,x1(k)的真實(shí)值)set(h,interpreter,none)subplot(2,1,2)i=1:10;

12、%記錄估計值%濾波方程 4%記錄狀態(tài)誤差協(xié)方差矩陣plot(i,x2(i),k,i,X2(i+1),b)h=legend(x2(k)的估計值：x2(k)的真實(shí)值)set(h,interpreter,none)for i=1:10%計算 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=lege nd(x1(k)的誤差)set(h,interpreter,none)subplot(2,1,2)i=1:10;plot(i,e2(i),r)h=lege

13、 nd(x2(k)的誤差)set(h,interpreter,none)figure%通過用卡爾曼濾波器的狀態(tài)誤差協(xié)方差矩陣畫出E1(k/k)A2jP E2(k/k)A2i=1:10;subplot(2,1,1)plot(i,pk(1,i),r)h= lege nd(由狀態(tài)誤差協(xié)方差矩陣得到的 E & 1(k/k)A2)set(h,Interpreter,none) subplot(2,1,2) plot(i,pk(2,i),r)h= legend(由狀態(tài)誤差協(xié)方差矩陣得到的E & 2(k/k)A2)set(h,Interpreter,none)四、實(shí)驗(yàn)結(jié)果分析(a)卡爾曼增益矩陣：HkPkCT(C PkCTR)1估計值與觀測值之