threebdyna.m

一个关于卫星导航方面的ekf滤波程序

function ydot = threebdyna(t,state)
%   earth-moon restricted dynamics
%   t is a time vector with certain time step.
%   state is a 6 elements vector: x, y, z, vx, vy, vz.
%   Reference: RJMCMC
%   Copyright (c) Shi Heng (2006)

miue=3.986005e5;
mium= miue/81.5;
J2=1.08263e-3;
Re=6371.004;
epher;
xm=moon(52,1);
ym=moon(52,2);
zm=moon(52,3);
x=state(1);
y=state(2);
z=state(3);
r=sqrt(x^2+y^2+z^2);
rm=sqrt(xm^2+ym^2+zm^2);
rsm=sqrt((x-xm)^2+(y-ym)^2+(z-zm)^2);

rj=(1-J2*Re*Re/r/r*(7.5*z*z/r/r-1.5));
mcx=-mium*((x-xm)/rsm/rsm/rsm+xm/rm/rm/rm);
mcy=-mium*((y-ym)/rsm/rsm/rsm+ym/rm/rm/rm);
mcz=-mium*((z-zm)/rsm/rsm/rsm+zm/rm/rm/rm);
ydot = [state(4); state(5); state(6); -miue*x/r/r/r*rj+mcx;  -miue*y/r/r/r*rj+mcy; -miue*z/r/r/r*rj+mcz];