ceqm_mice (t, y)

function ydot = ceqm_mice (t, y)

% first order form of Cowell's
% equations of orbital motion

% includes perturbations due to

%   non-spherical earth gravity
%   point-mass sun and moon gravity

% input

%  t = current simulation time
%  y = current eci state vector

% output

%  yddot = eci acceleration vector

% Orbital Mechanics with Matlab

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global etime0 smu mmu

asun = zeros(3, 1);

amoon = zeros(3, 1);

ydot(1) = y(4);
ydot(2) = y(5);
ydot(3) = y(6);

% compute geopotential gravity vector

agrav = gravity_mice(t, y);

% solar perturbations

etime = etime0 + t;

ptarg = mice_spkpos('Sun', etime, 'J2000', 'NONE', 'Earth');

rsun = ptarg.pos(1:3);

% compute heliocentric position vector of the spacecraft

for i = 1:3
    
    rs2sc(i) = y(i) - rsun(i);
    
end

% f(q) formulation

for i = 1:1:3
    
    vtmp(i) = y(i) - 2.0d0 * rsun(i);
    
end

dot1 = dot(y(1:3), vtmp);

dot2 = dot(rsun, rsun);

qsun = dot1 / dot2;

fsun = qsun * ((3.0d0 + 3.0d0 * qsun + qsun * qsun) ...
    / (1.0d0 + (1.0d0 + qsun)^1.5d0));

d3sun = norm(rs2sc)^3;

% point-mass gravity of the sun

for i = 1:1:3
    
    asun(i) = -smu * (y(i) + fsun * rsun(i)) / d3sun;
    
end

% lunar perturbations

ptarg = mice_spkpos('Moon', etime, 'J2000', 'NONE', 'Earth');

rmoon = ptarg.pos(1:3);

% compute selenocentric position vector of the spacecraft

for i = 1:1:3
    
    rm2sc(i) = y(i) - rmoon(i);
    
end

% f(q) formulation

for i = 1:1:3
    
    vtmp(i) = y(i) - 2.0d0 * rmoon(i);
    
end

dot1 = dot(y(1:3), vtmp);

dot2 = dot(rmoon, rmoon);

qmoon = dot1 / dot2;

fmoon = qmoon * ((3.0d0 + 3.0d0 * qmoon + qmoon * qmoon) ...
    / (1.0d0 + (1.0d0 + qmoon)^1.5d0));

d3moon = norm(rm2sc)^3;

% point-mass gravity of the moon

for i = 1:1:3
    
    amoon(i) = -mmu * (y(i) + fmoon * rmoon(i)) / d3moon;
    
end

% total acceleration vector

for i = 1:1:3
    
    ydot(i + 3) = agrav(i) + asun(i) + amoon(i);
    
end