擴(kuò)展卡爾曼濾波(EKF)仿真演示

擴(kuò)展卡爾曼濾波（EKF）仿真演示（西工大嚴(yán)恭敏，2012-2-4）一、問(wèn)題描述如圖1所示，從空中水平拋射出的物體，初始水平速度v（0）,初始位置坐標(biāo)x（x（0）,y（0））；受重力g和阻尼力影響，阻尼力與速度平方成正比，水平和垂直

阻尼系數(shù)分別為k,k;還存在不確定的零均值白噪聲干擾力5a和5a。在坐標(biāo)xyxy原點(diǎn)處有一觀測(cè)設(shè)備（不妨想象成雷達(dá)），可測(cè)得距離r（零均值白噪聲誤差5r）、角度a（零均值白噪聲誤差5a）。建模系統(tǒng)方程：f建模系統(tǒng)方程：f:量測(cè)方程：h:選狀態(tài)向量X二Lx=vxv--kv2+5axxxxy-vyv-kv2-g+5ayyyyr-px2+y2+5ra-atan(x/y)+5avyv］，量測(cè)向量z-「xy「0100系統(tǒng)Jacobian矩陣f-d系統(tǒng)Jacobian矩陣f-dx-2kvxx0001量測(cè)Jacobian矩陣?yán)?<x2+y2dx1/y1+(x/y)200102kvyy1I耳x2+y2一x/y21+(x/y)2三、Matlab仿真functiontest_ekfkx=.01;ky=.05;%阻尼系數(shù)g=9.8;%重力t=10;%仿真時(shí)間Ts=0.1;%采樣周期len=fix(t/Ts);%仿真步數(shù)%真實(shí)軌跡模擬dax=1.5;day=1.5;%系統(tǒng)噪聲X=zeros(len,4);X(1,:)=[0,50,500,0];%狀態(tài)模擬的初值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(l,l))*Ts;y=y+vy*Ts;vy=vy+(ky*vyA2-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))%構(gòu)造量測(cè)量mrad=0.001;dr=10;dafa=10*mrad;%量測(cè)噪聲fork=1:lenr=sqrt(X(k,1)A2+X(k,3)A2)+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])A2;Rk=diag([dr;dafa])A2;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)%系統(tǒng)狀態(tài)雅可比函數(shù)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)%量測(cè)雅可比函數(shù)x=X(1);y=X(3);H=zeros(2,4);r=sqrt(xA2+yA2);H(1,1)=1/r;H(1,3)=1/r;xy2=1+(x/y)A2;H(2,1)=1/xy2*1/y;H(2,3)=1/xy2*x*(-1/yA2);functionfX=fff(X,kx,ky,g,Ts)%系統(tǒng)狀態(tài)非線(xiàn)性函數(shù)x=X(1);vx=X(2);y=X(3);vy=X(4);x1=x+vx*Ts;vxl=vx+(-kx*vx^2)*Ts;y1=y+vy*Ts;vyl=vy+(ky*vyA2-g)*Ts;fX=[xl;vxl;yl;vyl];functionhfX=hhh(fX,Ts)%量測(cè)非線(xiàn)性函數(shù)x=fX(l);y=fX(3);r=sqrt(xA2+yA2);a=atan(x/y);hfX=[r;a];function[Xk,Pk,Kk]=ekf(Phikk_1,Qk,fXk_1,Pk_1,Hk,Rk,Zk_hfX)%ekf濾波函數(shù)Pkk_l=Phikk_l*Pk_l*Phi