funy.m

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

function f = funy(X)
% the function of the equation

g=3.9860305e14             ;%coefficient of G-force 
J2=1.0823e-3;%coefficient of the core of earth
Re=6378137.00;%radius of earth

r=sqrt(X(1)^2+X(2)^2+X(3)^2);
f=[X(4) X(5) X(6) -g*X(1)/r^3*(1+3*J2/2*(Re/r)^2*(1-5*(X(3)/r)^2)) -g*X(2)/r^3*(1+3*J2/2*(Re/r)^2*(1-5*(X(3)/r)^2)) -g*X(3)/r^3*(1+3*J2/2*(Re/r)^2*(3-5*(X(3)/r)^2))]';