完整版卡爾曼濾波試驗報告

1、實驗：卡爾曼濾波實現(xiàn)實驗報告姓名：學號：日期：(以下內(nèi)容用五號字書寫，本頁空白不夠可續(xù)頁)一、實驗理論及程序卡爾曼濾波是一種遞推的在最小均方誤差下求得的一步預(yù)測估計，估計結(jié)果對真實值是收斂的，具體可分為濾波、平滑和預(yù)測三種。對一離散時間線性動態(tài)系統(tǒng)，若其狀態(tài)方程滿足：X(k+1)=(k)X(k)+r(k)u(k)+G(k)V(k)其中X(k)為n維向量，表示k時刻的狀態(tài)向量，(k)為k時亥Jn*n階的狀態(tài)轉(zhuǎn)移矩陣，u(k)為已知p維輸入或控制信號，F(xiàn)(k)為n*p階輸入矩陣，G(k)為n*n維過程噪聲分布矩陣，V(k)為n維過程噪聲，為高斯白噪聲，且滿足EV(k)=0,EV(k)V(k)T=Q

2、(k)dkj,其中dkj為狄利克雷函數(shù)。測量方程滿足：Z(k)=H(k)X(k)+W(k)其中Z(k)為m為向量，表示k時刻的測量向量，H(k)為m*n階測量矩陣，W(k)是m維測量噪聲，為高斯白噪聲，且EW(k)=0,EW(k)W(k)T=R(k)dkj。設(shè)k時刻的濾波值為：X*(k/k)=EX(k)/Zk,Zk=：Z(j),j=1,2，,k)對應(yīng)的協(xié)方差矩陣為：P(k/k)E,：X(k)-)?(k/k)X(k)-父(k/k)T/Zk)濾波過程可表示如下：(1) 求出狀態(tài)的一步預(yù)測和協(xié)方差的一步預(yù)測，分別為：寅(k1/k)=中(k)?(k/k)-(k)U(k)，_.P(k1/k)=p(k)P

3、(k/k)z(k)G(k)Q(k)G(k)(2) 求出量測的預(yù)測值與量測預(yù)測協(xié)方差：f(k1/k)=EZ(k1)/Zk=H(k1)火(k1/k)_.,.S(k1)=H(k1)P(k1/k)H(k1)R(k1)(3) 新增加量測Z(k+1),計算信息與增益：V(k1)=Z(k1)-N(k1/k)_1K(k1)=P(k1/k)H(k1)S-(k1)|(口)1K(k1)=P(k1/k1)H(k1)R-(k1)|Q2)(4) 則狀態(tài)更新方程為：父(k1/k1)=乂(k1/k)K(k1)V(k1)協(xié)方差更新方程為：P(k1/k1)=P(k1/k)-K(k1)H(k1)P(k1/k)Matlab程序如下：

4、clearclccloseallN=200;w=randn(2,N);x=zeros(2,N);x(:,1)=1;1;a=1.10;01.02;fork=2:N;x(:,k)=a*x(:,k-1)+w(k-1);endV=randn(2,N);Rvv=cov(V);Rww=cov(w);c=10;00.9;Y=c*x+V;p=zeros(2,2*N);s=zeros(2,N);b=zeros(2,2*N);x1=zeros(2,N);z1=x1;zp1=b;p(:,1:2)=00;00;s(:,1)=4;8;fort=2:N;x1(:,t-1)=a*s(:,t-1)+w(:,t-1);%X(k

5、/k-1)p1(:,t*2-1:t*2)=a*p(:,t*2-3:t*2-2)*a+Rww;%X(k|k-1)的協(xié)方差z1(:,t-1)=c*x1(:,t-1);%測量估計zp1(:,t*2-1:t*2)=c*p1(:,t*2-1:t*2)*c+Rvv;%測量估計協(xié)方差b(:,t*2-1:t*2)=p1(:,t*2-1:t*2)*c/zp1(:,t*2-1:t*2);%Kg卡爾曼增益s(:,t)=x1(:,t-1)+b(:,t*2-1:t*2)*(Y(:,t)-z1(:,t-1);%當前狀態(tài)的最優(yōu)結(jié)果p(:,t*2-1:t*2)=p1(:,t*2-1:t*2)-b(:,t*2-1:t*2)*c

6、*p1(:,t*2-1:t*2);%更新當前狀態(tài)估計值的協(xié)方差endt=1:N;plot(s(1,:),s(2,:),ro,Y(1,:),Y(2,:),g.,x(1,:),x(2,:),b*);xlabel(time);ylabel(Amplitude);title(KalmanFilter)legend(kalman濾波估計值,測量值,真實值,4)gridon二、實驗過程(1)分析卡爾曼濾波算法原理，總結(jié)卡爾曼濾波算法流程(2)在第一步的基礎(chǔ)上編寫matlab程序(3)設(shè)計狀態(tài)方程及測量方程，確定初值進行濾波。實驗過程中遇到的問題：(a)矩陣的維數(shù)沒有對應(yīng)好，matlab報錯，在搞清楚各個變

7、量的物理意義并進行公示推算后終于調(diào)整好了所有的矩陣及向量維數(shù)；(b)維數(shù)調(diào)整好后程序可以運行，但結(jié)果很不穩(wěn)定，濾波值也沒能很好地跟蹤真實值，期初以為是狀態(tài)轉(zhuǎn)移矩陣與測量矩陣選取的問題，后來發(fā)現(xiàn)是在計算協(xié)方差是少求了轉(zhuǎn)置；(c)應(yīng)注意狀態(tài)轉(zhuǎn)移矩陣與測量矩陣的選取，不然不容易觀察濾波結(jié)果。三、實驗分析KalmanFilter40003500300025002X200015001000500呢*事而*.4.杰tp*kalman濾波估計值測量值於真實值一rrr200040006000800010000120001400016000X1上圖中藍色星號代表狀態(tài)真實值，綠色圓點代表測量值，紅色圓圈代表卡爾曼濾波結(jié)果，卡爾曼濾波初值選擇的偏差很大，但從實驗結(jié)果可以看出隨后的濾波值可以很好地跟蹤了真實值。四、實驗結(jié)論線性卡