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
