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

1）扩展卡尔曼的递推公式的程序 function [x_kk,p_kk]=KF(x_k1k1,p_k1k1,yk) TT=0.2; ztzy=[1 TT TT^2/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=[TT^2/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=[(x^2+y^2+h^2)^(1/2);atan2(y,x);atan2(h,sqrt(x^2+y^2))]; H_k=[1/(x^2+y^2+h^2)^(1/2)*x 0 0 1/(x^2+y^2+h^2)^(1/2)*y 0 0 1/(x^2+y^2+h^2)^(1/2)*h 0 0; -y/x^2/(1+y^2/x^2) 0 0 1/x/(1+y^2/x^2) 0 0 0 0 0; -h/(x^2+y^2)^(3/2)*x/(1+h^2/(x^2+y^2)) 0 0 -h/(x^2+y^2)^(3/2)*y/(1+h^2/(x^2+y^2)) 0 0 1/(x^2+y^2)^(1/2)/(1+h^2/(x^2+y^2)) 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; return 2）主函数的程序 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; end T=0.2; dx=(cpol(2,1)-cpol(1,1))/T;dxx=(cpol(3,1)+cpol(1,1)-2*cpol(2,1))/T^2; dy=(cpol(2,2)-cpol(1,2))/T;dyy=(cpol(3,2)+cpol(1,2)-2*cpol(2,2))/T^2; dh=(cpol(2,3)-cpol(1,3))/T;dhh=(cpol(3,3)+cpol(1,3)-2*cpol(2,3))/T^2; 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