资源描述：
《扩展卡尔曼滤波ekf仿真演示》由会员上传分享，免费在线阅读，更多相关内容在学术论文-天天文库。
1、扩展卡尔曼滤波（EKF）仿真演示一、问题描述如图1所示，从空中水平抛射出的物体，初始水平速度，初始位置坐标（）；受重力和阻尼力影响，阻尼力与速度平方成正比，水平和垂直阻尼系数分别为；还存在不确定的零均值白噪声干扰力和。在坐标原点处有一观测设备（不妨想象成雷达），可测得距离（零均值白噪声误差）、角度（零均值白噪声误差）。图1雷达观测示意图二、建模系统方程：量测方程：选状态向量,量测向量系统Jacobian矩阵量测Jacobian矩阵三、Matlab仿真functiontest_ekfkx=.01;ky=.05;%阻尼系数g=9.8;%重力t
2、=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,v
3、y];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(:
4、,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(
5、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)=
6、-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+(-k
7、x*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;
8、Kk=Pxz*Pzz^-1;Xk=fXk_1+Kk*Zk_hfX;Pk=Pkk_1-Kk*Pzz*Kk';图2仿真结果
