function ydot = hel_eqm (t, y)
% heliocentric equations of motion with
% planetary, lunar and srp perturbations
% Battin's f(q) formulation
% input
% t = current simulation time (days)
% output
% ydot = first order equations of motion
% Orbital Mechanics with MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global aunit xmu mmu csrp_hel jdtdb_wrk
asun = zeros(3, 1);
amoon = zeros(3, 1);
accp = zeros(3, 1);
asrp = zeros(3, 1);
% current julian date
jdate = jdtdb_wrk + t;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% point-mass gravity of the sun
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
rs2scm = norm(y(1:3));
asun = -xmu(1) * y(1:3) / rs2scm^3;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% planetary point-mass perturbations
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i = 1:1:9
svplanet = jplephem(jdate, i, 11);
for k = 1:1:3
rplanet(k) = svplanet(k);
end
for j = 1:1:3
rp(i, j) = rplanet(j);
end
end
% compute planet-centered position vector of spacecraft
for i = 1:1:9
rp2sc(i, 1) = y(1) - rp(i, 1);
rp2sc(i, 2) = y(2) - rp(i, 2);
rp2sc(i, 3) = y(3) - rp(i, 3);
end
% compute f(q) functions for each planet
for k = 1:1:9
q(k) = dot(y(1:3), y(1:3) - 2.0 * rp(k, :)') / dot(rp(k, :), rp(k, :));
f(k) = q(k) * ((3.0 + 3.0 * q(k) + q(k) * q(k)) / (1.0 + (1.0 + q(k))^1.5));
d3(k) = norm(rp2sc(k, :)) * norm(rp2sc(k, :)) * norm(rp2sc(k, :));
end
% compute planetary perturbations
for j = 1:1:3
accp(j) = 0.0;
for k = 1:1:9
accp(j) = accp(j) - xmu(k + 1) * (y(j) + f(k) * rp(k, j)) / d3(k);
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% lunar point-mass perturbation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
svmoon = jplephem(jdate, 10, 11);
% compute selenocentric position vector of the spacecraft
for i = 1:1:3
%rm2sc = y(1:3) - svmoon(1:3);
rm2sc(i) = y(i) - svmoon(i);
end
% f(q) formulation
vtmp = y(1:3) - 2.0d0 * svmoon(1:3);
dot1 = dot(y(1:3), vtmp);
dot2 = dot(svmoon(1:3), svmoon(1:3));
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
muwrk = mmu * 86400.0d0^2 / aunit^3;
amoon = -muwrk * (y(1:3) + fmoon * svmoon(1:3)) / d3moon;
if (csrp_hel > 0.0d0)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solar radiation pressure perturbation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
asrp = csrp_hel * y(1:3) / rs2scm^3;
end
% compute integration vector
ydot = [ y(4)
y(5)
y(6)
asun(1) + amoon(1) + accp(1) + asrp(1)
asun(2) + amoon(2) + accp(2) + asrp(2)
asun(3) + amoon(3) + accp(3) + asrp(3)];