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

1、卡爾曼濾波的MATLAB 實(shí)現(xiàn)一、實(shí)驗(yàn)內(nèi)容一個(gè)系統(tǒng)模型為 ( ( 1(, 1, 0, ( ( ( 1(22211k w k x k x k k w k x k x k x +=+=+=+同時(shí)有下列條件：（1） 初始條件已知且有T x 0, 0 0(=。（2） (k w 是一個(gè)標(biāo)量零均值白高斯序列，且自相關(guān)函數(shù)已知為jkk w j w E =( (。另外，我們有下列觀測模型，即 1( 1( 1(, 1, 0, 1( 1( 1(222111+=+=+=+k v k x k y k k v k x k y且有下列條件：（3） 1(1+k v 和 1(2+k v 是獨(dú)立的零均值白高斯序列，且有 , 2

2、, 1, 0, 2( (,( (2211=k k v j v E k v j v E jkjk（4） 對(duì)于所有的j 和k ， (k w 與觀測噪聲過程 1(1+k v 和 1(2+k v 是不相關(guān)的，即, 2, 1, 0, , 2, 1, 0, 0( (,0( (21=k j k v j w E k v j w E我們希望得到由觀測矢量 1(+k y ，即T k y k y k y 1(, 1( 1(21+=+估計(jì)狀態(tài)矢量T k x k x k x 1(, 1( 1(21+=+的卡爾曼濾波器的公式表示形式，并求解以下問題：（a ） 求出卡爾曼增益矩陣，并得出最優(yōu)估計(jì) 1(+k x 和觀測矢量1

3、(,., 2(, 1(+k y y y 之間的遞歸關(guān)系。（b ） 通過一個(gè)標(biāo)量框圖（不是矢量框圖）表示出狀態(tài)矢量 1(+k x 中元素1(1+k x 和 1(2+k x 估計(jì)值的計(jì)算過程。（c ） 用模擬數(shù)據(jù)確定狀態(tài)矢量 (k x 的估計(jì)值, 10,., 1, 0, (=k k k x 并畫出當(dāng)k 0，1，10時(shí) (1k k x 和 (2k k x 的圖。（d ） 通常，狀態(tài)矢量的真實(shí)值是得不到得。但為了用作圖來說明問題，表P8.1和P8.2給出來狀態(tài)矢量元素得值。對(duì)于k 0，1，10，在同一幅圖中畫出真實(shí)值和在（c ）中確定的 (1k x 的估計(jì)值。對(duì) (2k x 重復(fù)這樣過程。當(dāng)k 從1變

4、到10時(shí)，對(duì)每一個(gè)元素i 1，2，計(jì)算并畫出各自的誤差圖，即 ( (k k x k x i i -。（e ） 當(dāng)k 從1變到10時(shí)，通過用卡爾曼濾波器的狀態(tài)誤差協(xié)方差矩陣畫出(21k k E 和(22k k E ，而( ( (111k k x k x k k -=，( ( (222k k x k x k k -=。（f ） 討論一下（d ）中你計(jì)算的誤差與（e ）中方差之間的關(guān)系。表P8.1 題8.1到題8.3中的觀測值時(shí)間下標(biāo)k 觀測值 (1k y (2k y 觀測值1 23456789103.296919693.387365157.028306419.7121252111.42018315

5、15.9787058322.0693428528.3021278130.4468383138.758755952.101342940.475407973.176888982.498111402.919924246.173076165.425192743.053657415.980511414.51016361表P8.2 題8.1到題8.3中的由模擬得到的實(shí)際狀態(tài)值時(shí)間下標(biāo)k實(shí)際狀態(tài)值(1k x 實(shí)際狀態(tài)值(1k x 0123456789100.00000000001.654287143.503007025.9978529249.1504074012.5087391016.9219259421.

6、3448335225.8933514431.5413533036.936056700.0000000001.654287141.848719882.475522223.171878163.358331704.413186844.422907584.548517925.648001865.394470340二、實(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)和噪聲

7、是用狀態(tài)方程和測量方程來表示的。因此設(shè)計(jì)卡爾曼濾波器要求已知狀態(tài)方程和測量方程。它不需要知道全部過去的數(shù)據(jù)，采用遞推的方法計(jì)算，它既可以用于平穩(wěn)和不平穩(wěn)的隨機(jī)過程，同時(shí)也可以應(yīng)用解決非時(shí)變和時(shí)變系統(tǒng)，因而它比維納過濾有更廣泛的應(yīng)用。2、卡爾曼濾波的遞推公式(11-+=k k k k k k k k x A C y H x A x (11(-+''=k k k k k k k R C P C C P H (211-+='k k k k k Q A P A P (3k k k k P C H I P '-= (43、遞推過程的實(shí)現(xiàn) 如果初始狀態(tài)x 的統(tǒng)計(jì)特性0x E

8、 及var0x 已知，并令 000=x E x又 var (000000x x x x x E P =-=將P 代入式（3）可求得1P '，將1P '代入式（2）可求得1H ，將此1H 代入式（1）可求得在最小均方誤差條件下的1x ，同時(shí)將1P '代入式（4）又可求得1P ；由1P 又可求2P '，由2P '又可求得2H ，由2H 又可求得2x ，同時(shí)由2H 與2P '又可求得2P ；以此類推，這種遞推計(jì)算方法用計(jì)算機(jī)計(jì)算十分方便。三、MATLAB 程序%卡爾曼濾波實(shí)驗(yàn)程序 clc;y1=3.29691969,3.38736515,7.02830

9、641,9.71212521,11.42018315,15.97870583,22.06934285,28.30212781,30.44683831,38.75875595; %觀測值y1(ky2=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; %均方誤差陣賦初值 Ak=1,1;0,1; %轉(zhuǎn)移矩陣Qk=1,0;0,1; %系統(tǒng)噪聲矩陣 Ck=1,0;0,1; %量測矩陣

10、Rk=1,0;0,2; %測量噪聲矩陣 x0=0,0'xk=x0; %狀態(tài)矩陣賦初值 for k=1:10Pk=Ak*p*Ak'+Qk; %濾波方程3 Hk=Pk*Ck'*inv(Ck*Pk*Ck'+Rk; %濾波方程2 yk=y1(k;y2(k; %觀測值xk=Ak*xk+Hk*(yk-Ck*Ak*xk; %濾波方程1 x1(k=xk(1;x2(k=xk(2; %記錄估計(jì)值 p=(eye(2-Hk*Ck*Pk; %濾波方程4pk(:,k=p(1,1,p(2,2' %記錄狀態(tài)誤差協(xié)方差矩陣 endfigure %畫圖表示狀態(tài)矢量的估計(jì)值 subplot(

11、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.92192594,21.34

12、483352,25.89335144,31.54135330,36.93605670; %由模擬得到的實(shí)際狀態(tài)值X1(kX2=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(kfigure %在同一幅圖中畫出狀態(tài)矢量的估計(jì)值與真實(shí)值 subplot(2,1,1 i=1:10;plot(i,x1(i,'k',i,X1(i+1,'b'h=legend('

13、;x1(k的估計(jì)值','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'for i=1:10 %計(jì)算x(k的誤差 e1(i=X1(i+1-x1(i; e2(i=X2(i+1-x2(i; endfigure %畫

14、出誤差圖 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é)方差矩陣畫出E1(k/k2和E2(k/k2 i=1:10;subplot(2,1,1 pl

15、ot(i,pk(1,i,'r'h= legend('由狀態(tài)誤差協(xié)方差矩陣得到的E1(k/k2' set(h,'Interpreter','none' subplot(2,1,2 plot(i,pk(2,i,'r'h= legend('由狀態(tài)誤差協(xié)方差矩陣得到的E2(k/k2' set(h,'Interpreter','none'四、實(shí)驗(yàn)結(jié)果分析（a ）卡爾曼增益矩陣：k k k T T 1H P C (CP C R -=+ 估計(jì)值與觀測值之間的遞歸關(guān)系為：k k-1k-1k k k k k X A X H (YC A X =+-（b ）狀態(tài)矢量估計(jì)值的計(jì)算框圖： xk +1 yk +1 + xk +1