吳超 馮浩明-有色噪聲下的kalman濾波.docx

1、有色噪聲下的kalman濾波匯報人：馮浩明，吳超在處理有色噪聲的卡爾曼濾波問題中有以下幾種情況：1. 系統(tǒng)噪聲是有色噪聲，測量噪聲是白噪聲；2. 系統(tǒng)噪聲是白噪聲，測量噪聲是有色噪聲；3.兩者都是有色噪聲；同時，在卡爾曼濾波處理有色噪聲情況的幾種常用的方法：擴(kuò)充狀態(tài)變量法；改變觀測方程；一步相關(guān)卡爾曼濾波(OCKF)；多步相關(guān)卡爾曼濾波(MCKF)等本文介紹比較基礎(chǔ)的擴(kuò)充狀態(tài)變量法：設(shè)系統(tǒng)狀態(tài)和觀測方程為x(k+1)=人化+1,3(局+伏+1,局岫)皆+1)=H(A+1*)w(k)+fj(k)這里為與W(0)不相關(guān)的高斯白噪聲。且1) W(A+l,A),AzO；為已知；2) 引(人)=0,。(

2、蜀=var/(A)為已知；3) (*)"20與岫),屋0和HO)互不相關(guān)。對于這種情況，一般采用擴(kuò)充狀態(tài)變量法，為此，定義新的符號x(A)H0)x(A)H0),A(k+1,4)=A(k+l,k)r(k+,k)0H(k+i.k)C,(A+1)C,(A+1)Cf(k+lj0x"(k+l)=A'(k+1,A：)F(A：)+"(A+1M)(局z(A)=C*(A)x*(k)+v(k)這樣將其轉(zhuǎn)化為觀測噪聲和系統(tǒng)噪聲均為白噪聲情況下的傳統(tǒng)卡爾曼濾波了，帶入到下面的常用的5個重要方程，同時也得到了擴(kuò)充后方程與原方程之間對應(yīng)關(guān)系，從而可以解決。一輛小車力速前進(jìn)，在原點(diǎn)處有

3、一測量裝置以每隔一秒的時間對其位移進(jìn)行測量，在有外界噪聲干擾的情況下，估計它的位移量。假設(shè)其系統(tǒng)噪聲為有色噪聲，觀測噪聲為白噪聲，初始速度為5米/秒，位直為10米，假設(shè)時間為100秒，估計這期間的位移量。（11）0*（"）=01杪T）+奐）z（幻=尤（化）+口（幻z（幻=尤（化）+口（幻z(A)=lOx(=+v(。)Ow(f)=0即)1110A=011H=0()000.8x(t)=Ax(t)+w(t)z(r)=Hxt)+v(0離散化：X(k+l,k)=(D(k+l,k)X(k)+日(k)Z(k)=HX(k)+V(k)£。+1)=中伙+1,幻£(幻+n(A)帶入離散

4、化的卡爾曼濾波的五項方程。進(jìn)行仿真得到:T=1;N=100/T;X=zeros(5,N);X(:,l)=10,5,0,2,2;Z=zeros(2,N);Z(:,l)=X(1,1),X(3,1)；¥=00001*;Q=0.8;R=100;%Ai2aOeEu-2iIa0£-.KalmanAE2F=1,T,0,0,0.05;0,1,0,0,0.1;0,0,1,T,0.5;0,0,0,1,1；0,0,0,0,0.8;a。跆006H=1,0,0,0,0;0,0,1,0,0;%1U2a0O6fori=2:NX(:zi)=F*X(:,i-1)+Y*sqrtm(Q)*randn(l,l);

5、%A£±e0aeE|ji»OAZ(:,i)=H*X(:,i)+l,0;0,0.5*sqrtm(R)*randn(2,1);%102aI»OAendX;Z;%KalmanAE2"Xkf=zeros(5,N);Xkf(:,1)=X(:,l);%AE2"x,i-,3dE»_P0=100*eye(5);fork=2:NXn=F*Xkf(:,k-1);%0o2aP=F*PO*F'+Y*Q*Y*;%Io2iD-h2iK=P*H,*inv(H*P*H,+R*eye(2);%AE2"0ob$Xkf(:,k)=Xn+K*(Z

6、(:,k)-H*Xn);%x'，誑)民PO=(eye(5)-K*H)*P;%I621D-end%to2i-Otofori=l:NErr_Obsevation(i)=RMS(X(:,i),Z(:,i);Err_KF(i)=RMS(X(:,i),Xkf(:,i);endErr_before=mse(Err_Obsevation)Err_after=mse(Err_KF)%»-f蝴figure(1)holdon;boxon;plot(X(l,:),X(3,:),;holdon;plot(Z(l,:),Z(2,:),*-b.');plot(Xkf(lz:),Xkf(3,:),

7、'-r+');legend(10»Epi»OA',*102ai»OA*,*KalmanAE2"I»OA1);xlabel('X/m');figure(2)plot(Err_Obsevationz'-g*);holdon;plot(Err_KF,'-r+');legend('AE2'(；0130162i','AE2"°6laSIOi62i');axis(080-1030);title('i»dAaAeiaOi62i');figure(3)plot(Xkf(5,:),*-r');holdonplot(X(5,:),'-g');legend('KFlddeupAIu3y1,'O-OeEu');titleCOeEuSIOfE1);%holdon%plot(Z(5,:),*-b');functiondi