利用扩展卡尔曼滤波算法进行目标状态估计.doc

1）扩展卡尔曼的递推公式的程序function x_kk,p_kk=KF(x_k1k1,p_k1k1,yk) TT=0.2; ztzy=1 TT TT2/2;0 1 TT;0 0 1; F=ztzy zeros(3,3) zeros(3,3);zeros(3,3) ztzy zeros(3,3);zeros(3,3) zeros(3,3) ztzy; gr=TT2/2 TT 1; tou=gr zeros(3,1) zeros(3,1);zeros(3,1) gr zeros(3,1);zeros(3,1) zeros(3,1) gr; R=20 0 0;0 5*2*pi/6000 0;0 0 5*2*pi/6000; x_kk1=F*x_k1k1; x=x_kk1(1,1); y=x_kk1(4,1); h=x_kk1(7,1); h_k=(x2+y2+h2)(1/2);atan2(y,x);atan2(h,sqrt(x2+y2); H_k=1/(x2+y2+h2)(1/2)*x 0 0 1/(x2+y2+h2)(1/2)*y 0 0 1/(x2+y2+h2)(1/2)*h 0 0; -y/x2/(1+y2/x2) 0 0 1/x/(1+y2/x2) 0 0 0 0 0;-h/(x2+y2)(3/2)*x/(1+h2/(x2+y2) 0 0 -h/(x2+y2)(3/2)*y/(1+h2/(x2+y2) 0 0 1/(x2+y2)(1/2)/(1+h2/(x2+y2) 0 0; p_kk1=F*p_k1k1*F+0.1*tou*tou; k_k=p_kk1*H_k*inv(H_k*p_kk1*H_k+R); x_kk=x_kk1+k_k*(yk-h_k); p_kk=(eye(9)-k_k*H_k)*p_kk1; return2）主函数的程序cd D:MATLAB;load(polreal2.dat);load(polcl2.dat);for i=1:150; cpol(i,1)=polcl2(i,1)*cos(polcl2(i,3)*2*pi/6000)*cos(polcl2(i,2)*2*pi/6000); cpol(i,2)=polcl2(i,1)*cos(polcl2(i,3)*2*pi/6000)*sin(polcl2(i,2)*2*pi/6000); cpol(i,3)=polcl2(i,1)*sin(polcl2(i,3)*2*pi/6000);end %cpol直角坐标下的for i=1:150 cpolreal(i,1)=polreal2(i,1)*cos(polreal2(i,2)/6000*2*pi)*cos(polreal2(i,3)/6000*2*pi); cpolreal(i,2)=polreal2(i,1)*cos(polreal2(i,3)/6000*2*pi)*sin(polreal2(i,2)/6000*2*pi); cpolreal(i,3)=polreal2(i,1)*sin(polreal2(i,3)/6000*2*pi);end %cpolreal直角坐标下的（单位弧度）for i=1:150 ppol(i,1)=polcl2(i,1); ppol(i,2)=polcl2(i,2)*2*pi/6000; ppol(i,3)=polcl2(i,3)*2*pi/6000;endT=0.2;dx=(cpol(2,1)-cpol(1,1)/T;dxx=(cpol(3,1)+cpol(1,1)-2*cpol(2,1)/T2;dy=(cpol(2,2)-cpol(1,2)/T;dyy=(cpol(3,2)+cpol(1,2)-2*cpol(2,2)/T2;dh=(cpol(2,3)-cpol(1,3)/T;dhh=(cpol(3,3)+cpol(1,3)-2*cpol(2,3)/T2;xk(1,1)=cpol(1,1);xk(1,2)=dx;xk(1,3)=dxx;xk(1,4)=cpol(1,2);xk(1,5)=dy;xk(1,6)=dyy;xk(1,7)=cpol(1,3);xk(1,8)=dh;xk(1,9)=dhh;x0=xk(1,:);p0=1000*eye(9);x_k1k1=x0;p_k1k1=p0;for k=2:150; yk=ppol(k,:); x_kk,p_kk=KF(x_k1k1,p_k1k1,yk); xk(k,:)=x_kk; x_k1k1=x_kk; p_k1k1=p_kk;endfor i=1:150;error(i,1)=xk(i,1)-cpolreal(i,1);error(i,2)=xk(i,4)-cpolreal(i,2);error(i,3)=xk(i,7)-cpolreal(i,3);endt=0:0.2:149*0.2;figure(1);hold on;title(x-y曲线);xlabel(Position x);ylabel(Position y);legend(xk,cpolreal,cpol);plot(xk(:,1),xk(:,4),r-v);hold on;plot(cpolreal(:,1),cpolreal(:,2),k-o);hold on;plot(cpol(:,1),cpol(:,2),b-x);t=0:0.2:149*0.2;figure(