吴超 冯浩明-有色噪声下的kalman滤波.docx

1、有色噪声下的kalman滤波汇报人：冯浩明，吴超在处理有色噪声的卡尔曼滤波问题中有以下几种情况：1. 系统噪声是有色噪声，测量噪声是白噪声；2. 系统噪声是白噪声，测量噪声是有色噪声；3.两者都是有色噪声；同时，在卡尔曼滤波处理有色噪声情况的几种常用的方法：扩充状态变量法；改变观测方程；一步相关卡尔曼滤波(OCKF)；多步相关卡尔曼滤波(MCKF)等本文介绍比较基础的扩充状态变量法：设系统状态和观测方程为x(k+1)=人化+1,3(局+伏+1,局岫)皆+1)=H(A+1*)w(k)+fj(k)这里为与W(0)不相关的高斯白噪声。且1) W(A+l,A),AzO；为已知；2) 引(人)=0,。(

2、蜀=var/(A)为已知；3) (*)"20与岫),屋0和HO)互不相关。对于这种情况，一般采用扩充状态变量法，为此，定义新的符号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)这样将其转化为观测噪声和系统噪声均为白噪声情况下的传统卡尔曼滤波了，带入到下面的常用的5个重要方程，同时也得到了扩充后方程与原方程之间对应关系，从而可以解决。一辆小车力速前进，在原点处有

3、一测量装置以每隔一秒的时间对其位移进行测量，在有外界噪声干扰的情况下，估计它的位移量。假设其系统噪声为有色噪声，观测噪声为白噪声，初始速度为5米/秒，位直为10米，假设时间为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、化的卡尔曼滤波的五项方程。进行仿真得到: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