扩展卡尔曼滤波(EKF)仿真演示

扩展卡尔曼滤波（EKF）仿真演示（西工大严恭敏，2012-2-4）一、问题描述如图1所示，从空中水平抛射出的物体，初始水平速度)0(xv，初始位置坐标（)0(),0(yx）；受重力g和阻尼力影响，阻尼力与速度平方成正比，水平和垂直阻尼系数分别为yxkk,；还存在不确定的零均值白噪声干扰力xa和ya。在坐标原点处有一观测设备（不妨想象成雷达），可测得距离r（零均值白噪声误差r）、角度（零均值白噪声误差）。xy)0(xvgr)0,0(雷达物体图1雷达观测示意图二、建模系统方程：yyyyyxxxxxagvkvvyavkvvx22:f量测方程：)/tan(a:22yxryxrh选状态向量Tyxvyvxx,量测向量Trz系统Jacobian矩阵yyxxvkvk2000010000200010xf量测Jacobian矩阵0)/(1/0)/(1/101012222222yxyxyxyyxyxxh三、Matlab仿真functiontest_ekfkx=.01;ky=.05;%阻尼系数g=9.8;%重力t=10;%仿真时间Ts=0.1;%采样周期len=fix(t/Ts);%仿真步数%真实轨迹模拟dax=1.5;day=1.5;%系统噪声X=zeros(len,4);X(1,:)=[0,50,500,0];%状态模拟的初值fork=2:lenx=X(k-1,1);vx=X(k-1,2);y=X(k-1,3);vy=X(k-1,4);x=x+vx*Ts;vx=vx+(-kx*vx^2+dax*randn(1,1))*Ts;y=y+vy*Ts;vy=vy+(ky*vy^2-g+day*randn(1))*Ts;X(k,:)=[x,vx,y,vy];endfigure(1),holdoff,plot(X(:,1),X(:,3),'-b'),gridon%figure(2),plot(X(:,2:2:4))%构造量测量mrad=0.001;dr=10;dafa=10*mrad;%量测噪声fork=1:lenr=sqrt(X(k,1)^2+X(k,3)^2)+dr*randn(1,1);a=atan(X(k,1)/X(k,3))+dafa*randn(1,1);Z(k,:)=[r,a];endfigure(1),holdon,plot(Z(:,1).*sin(Z(:,2)),Z(:,1).*cos(Z(:,2)),'*')%ekf滤波Qk=diag([0;dax;0;day])^2;Rk=diag([dr;dafa])^2;Xk=zeros(4,1);Pk=100*eye(4);X_est=X;fork=1:lenFt=JacobianF(X(k,:),kx,ky,g);Hk=JacobianH(X(k,:));fX=fff(X(k,:),kx,ky,g,Ts);hfX=hhh(fX,Ts);[Xk,Pk,Kk]=ekf(eye(4)+Ft*Ts,Qk,fX,Pk,Hk,Rk,Z(k,:)'-hfX);X_est(k,:)=Xk';endfigure(1),plot(X_est(:,1),X_est(:,3),'+r')xlabel('X');ylabel('Y');title('ekfsimulation');legend('real','measurement','ekfestimated');%%%%%%%%%%%%%%%%%%%%子程序%%%%%%%%%%%%%%%%%%%functionF=JacobianF(X,kx,ky,g)%系统状态雅可比函数vx=X(2);vy=X(4);F=zeros(4,4);F(1,2)=1;F(2,2)=-2*kx*vx;F(3,4)=1;F(4,4)=2*ky*vy;functionH=JacobianH(X)%量测雅可比函数x=X(1);y=X(3);H=zeros(2,4);r=sqrt(x^2+y^2);H(1,1)=1/r;H(1,3)=1/r;xy2=1+(x/y)^2;H(2,1)=1/xy2*1/y;H(2,3)=1/xy2*x*(-1/y^2);functionfX=fff(X,kx,ky,g,Ts)%系统状态非线性函数x=X(1);vx=X(2);y=X(3);vy=X(4);x1=x+vx*Ts;vx1=vx+(-kx*vx^2)*Ts;y1=y+vy*Ts;vy1=vy+(ky*vy^2-g)*Ts;fX=[x1;vx1;y1;vy1];functionhfX=hhh(fX,Ts)%量测非线性函数x=fX(1);y=fX(3);r=sqrt(x^2+y^2);a=atan(x/y);hfX=[r;a];function[Xk,Pk,Kk]=ekf(Phikk_1,Qk,fXk_1,Pk_1,Hk,Rk,Zk_hfX)%ekf滤波函数Pkk_1=Phikk_1*Pk_1*Phikk_1'+Qk;Pxz=Pkk_1*Hk';Pzz=Hk*Pxz+Rk;Kk=Pxz*Pzz^-1;Xk=fXk_1+Kk*Zk_hfX;Pk=Pkk_1-Kk*Pzz*Kk';020406080100120140160180200360380400420440460480500520ekfsimulationXYrealmeasurementekfestimated图2仿真结果