getf.m

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

function f = Getf(X)
%

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);
   f=[X(4);X(5);X(6);
         -g*X(1)*(1-J2*(Re/r)^2*(7.5*X(3)^2/r^2-1.5))/r^3;
         -g*X(2)*(1-J2*(Re/r)^2*(7.5*X(3)^2/r^2-1.5))/r^3;
         -g*X(3)*(1-J2*(Re/r)^2*(7.5*X(3)^2/r^2-4.5))/r^3];