geta.m

来自「这是个扩展kalman滤波的历程」· M 代码 · 共 16 行

function A = GetA(X)
% according to the given state equation to get the matirx A
g=3.9860305e14;%coefficient of G-force 
J2=1.0823e-3;%coefficient of the core of earth
Re=6378137;%radius of earth

r = sqrt(X(1)^2+X(2)^2+X(3)^2);

A = [0 0 0 1 0 0
     0 0 0 0 1 0
     0 0 0 0 0 1
     -g*(r^(-3)-3*X(1)^2*r^(-5))*(1+3/2*J2*(Re/r)^2*(1-5*(X(3)/r)^2))-g*X(1)/r^3*(-3*X(1)*J2*Re^2/r^4*(1-5*(X(3)/r)^2)+3/2*J2*(Re/r)^2*10*X(1)*X(3)^2/r^4) 3*g*X(1)*X(2)*r^(-5)*(1+3/2*J2*(Re/r)^2*(1-5*(X(3)/r)^2))-g*X(1)/r^3*(-3*X(2)*J2*Re^2/r^4*(1-5*(X(3)/r)^2)+3/2*J2*(Re/r)^2*10*X(2)*X(3)^2/r^4) 3*g*X(1)*X(3)*r^(-5)*(1+3/2*J2*(Re/r)^2*(1-5*(X(3)/r)^2))-g*X(1)/r^3*(-3*X(3)*J2*Re^2/r^4*(1-5*(X(3)/r)^2)+3/2*J2*(Re/r)^2*(-10)*X(3)*(X(1)^2+X(2)^2)/r^4) 0 0 0
     3*g*X(1)*X(2)*r^(-5)*(1+3/2*J2*(Re/r)^2*(1-5*(X(3)/r)^2))-g*X(2)/r^3*(-3*X(1)*J2*Re^2/r^4*(1-5*(X(3)/r)^2)+3/2*J2*(Re/r)^2*10*X(1)*X(3)^2/r^4) -g*(r^(-3)-3*X(2)^2*r^(-5))*(1+3/2*J2*(Re/r)^2*(1-5*(X(3)/r)^2))-g*X(2)/r^3*(-3*X(2)*J2*Re^2/r^4*(1-5*(X(3)/r)^2)+3/2*J2*(Re/r)^2*10*X(2)*X(3)^2/r^4) 3*g*X(2)*X(3)*r^(-5)*(1+3/2*J2*(Re/r)^2*(1-5*(X(3)/r)^2))-g*X(2)/r^3*(-3*X(3)*J2*Re^2/r^4*(1-5*(X(3)/r)^2)+3/2*J2*(Re/r)^2*(-10)*X(3)*(X(1)^2+X(2)^2)/r^4) 0 0 0
     3*g*X(1)*X(3)*r^(-5)*(1+3/2*J2*(Re/r)^2*(3-5*(X(3)/r)^2))-g*X(3)/r^3*(-3*X(1)*J2*Re^2/r^4*(3-5*(X(3)/r)^2)+3/2*J2*(Re/r)^2*10*X(1)*X(3)^2/r^4) 3*g*X(2)*X(3)*r^(-5)*(1+3/2*J2*(Re/r)^2*(3-5*(X(3)/r)^2))-g*X(3)/r^3*(-3*X(2)*J2*Re^2/r^4*(3-5*(X(3)/r)^2)+3/2*J2*(Re/r)^2*10*X(2)*X(3)^2/r^4) -g*(r^(-3)-3*X(3)^2*r^(-5))*(1+3/2*J2*(Re/r)^2*(3-5*(X(3)/r)^2))-g*X(3)/r^3*(-3*X(3)*J2*Re^2/r^4*(3-5*(X(3)/r)^2)+3/2*J2*(Re/r)^2*(-10)*X(3)*(X(1)^2+X(2)^2)/r^4) 0 0 0];