(完整word版)卡爾曼濾波實(shí)驗(yàn)報(bào)告

1、實(shí)驗(yàn)：卡爾曼濾波實(shí)現(xiàn)實(shí)驗(yàn)報(bào)告姓名:學(xué)號：日期：-(以下內(nèi)容用五號字書寫，本頁空白不夠可續(xù)頁)一、實(shí)驗(yàn)理論及程序卡爾曼濾波是一種遞推的在最小均方誤差下求得的一步預(yù)測估計(jì)，估計(jì)結(jié)果對真實(shí)值是收斂的，具體可分為濾波、平滑和預(yù)測三種。對一離散時(shí)間線性動態(tài)系統(tǒng)，若其狀態(tài)方程滿足：X(k+1)=(k)X(k)+r(k)u(k)+G(k)V(k)其中X(k)為n維向量，表示k時(shí)刻的狀態(tài)向量，(k)為k時(shí)刻n*n階的狀態(tài)轉(zhuǎn)移矩陣，u(k)為已知p維輸入或控制信號，r(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時(shí)刻的測量向量，H(k)為m*n階測量矩陣，W(k)是m維測量噪聲，為高斯白噪聲，且EW(k)=O,EW(k)W(k)T=R(k)dkj。設(shè)k時(shí)刻的濾波值為：X(k/k)二EX(k)/Zk,Zk=z(j),j二1,2,.,k對應(yīng)的協(xié)方差矩陣為：P(k/k)=EX(k)-X(k/k)X(k)-X(k/k)T/Zk濾波過程可表示如下：求出狀態(tài)的一步預(yù)測和協(xié)方差的一步預(yù)測，分別為：X(k+1/k)=(k)X(k/k)+r(k)U(k)P(k+1/k)=(k)P(k/k)O(k)+G

3、(k)Q(k)G(k)求出量測的預(yù)測值與量測預(yù)測協(xié)方差：為(k+1/k)二EZ(k+1)/Zk=H(k+1)X(k+1/k)S(k+1)=H(k+1)P(k+1/k)H(k+1)+R(k+1)新增加量測Z(k+1),計(jì)算信息與增益：V(k+1)二Z(k+1)-為(k+1/k)K(k+1)=P(k+1/k)H(k+1)S-1(k+1)(1)K(k+1)=P(k+1/k+1)H(k+1)R-1(k+1)(2)則狀態(tài)更新方程為：X(k+1/k+1)=X(k+1/k)+K(k+1)V(k+1)協(xié)方差更新方程為：P(k+1/k+1)=P(k+1/k)K(k+1)H(k+1)P(k+1/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);

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

6、c*p1(:,t*2T:t*2);%更新當(dāng)前狀態(tài)估計(jì)值的協(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濾波估計(jì)值,測量值,真實(shí)值,4)gridon二、實(shí)驗(yàn)過程分析卡爾曼濾波算法原理，總結(jié)卡爾曼濾波算法流程在第一步的基礎(chǔ)上編寫matlab程序設(shè)計(jì)狀態(tài)方程及測量方程，確定初值進(jìn)行濾波。實(shí)驗(yàn)過程中遇到的問題：(a)矩陣的維數(shù)沒有對應(yīng)好，matlab報(bào)錯(cuò)，在搞清楚各個(gè)變量的物理意義并進(jìn)行公示推算后終于調(diào)整好了所有的矩陣及向量維數(shù)；(b)維數(shù)調(diào)整好后程序可以運(yùn)行，但結(jié)果很不穩(wěn)定，濾波值也沒能很好地跟蹤真實(shí)值，期初以為是狀態(tài)轉(zhuǎn)移矩陣與測量矩陣選取的問題，后來發(fā)現(xiàn)是在計(jì)算協(xié)方差是少求了轉(zhuǎn)置；(c)應(yīng)注意狀態(tài)轉(zhuǎn)移矩陣與測量矩陣的選取，不然不容易觀察濾波結(jié)果。三、實(shí)驗(yàn)分析KalmanFilter上圖中藍(lán)色星號代表狀態(tài)真實(shí)值，綠色圓點(diǎn)代表測量值，紅色圓圈代表卡爾曼濾波結(jié)果，卡爾曼濾波