orbcalc7.m

小推力航天器转移轨道程序

function [dX] = OrbCalc7(t,X,F,Isp,go,mu_earth,mu_sun,mu_mars,SOI_E,SOI_M,a_mars,R_mars)
global z

M = X(1);
R_sc = X(2:3);
V_sc = X(4:5);
R_E = X(6:7);
V_E = X(8:9);
R_M = X(10:11);
V_M = X(12:13);

A_sc_grav = -mu_sun*R_sc/norm(R_sc)^3 + ...   %Acceleration due to sun
            mu_earth*((R_E-R_sc)/norm(R_E-R_sc)^3 - R_E/norm(R_E)^3)...  %Perturbation due to Earth
            + mu_mars*((R_M-R_sc)/norm(R_M-R_sc)^3 - R_M/norm(R_M)^3); %Perturbation due to Mars

zeta = norm(V_sc)^2/2 - mu_sun/norm(R_sc);
h = cross([R_sc;0],[V_sc;0]);
e_vec = ((norm(V_sc)^2  - mu_sun/norm(R_sc))*R_sc - dot(R_sc,V_sc)*V_sc)/mu_sun;
ecc = norm(e_vec);
Ra = -mu_sun/(2*zeta)*(1+ecc);
VatMars = (2*(mu_sun/a_mars + zeta))^.5;
nu_mars = acos(dot(e_vec,R_sc)/(ecc*norm(R_sc)));  %True anomaly in radians
if dot(R_sc,V_sc) < 0  %Checking for quadrant
    nu_mars = 2*pi - nu_mars;
end
E_mars = atan2(sin(nu_mars)*(1-ecc^2)^.5,ecc+cos(nu_mars));
fpa_mars = atan2(ecc*sin(E_mars),(1-ecc^2)^.5);  %This is really the current fpa, not fpa at mars

if norm(R_sc-R_E) < SOI_E && Ra < a_mars
    A_sc_F = F/M*10^-3*(V_sc-V_E)/norm(V_sc-V_E);  %Accel due to thrust
    mdot = -F/Isp/go;
    zeta_E = norm(V_sc-V_E)^2/2 - mu_earth/norm(R_sc-R_E);
elseif norm(R_sc-R_M) < 8*SOI_M
    zeta_M = norm(V_sc-V_M)^2/2 - mu_mars/norm(R_sc-R_M);
    V_sc_M = V_sc - V_M;
    R_sc_M = R_sc - R_M;
    h_M = cross([R_sc_M;0],[V_sc_M;0]);
    e_vec_M = ((norm(V_sc_M)^2  - mu_mars/norm(R_sc_M))*R_sc_M - dot(R_sc_M,V_sc_M)*V_sc_M)/mu_mars;
    ecc_M = norm(e_vec_M);
    Rp_M = -mu_mars/(2*zeta_M)*(1-ecc_M);
    Ra_M = -mu_mars/(2*zeta_M)*(1+ecc_M);
    nu_M = acos(dot(e_vec_M,R_sc_M)/(ecc_M*norm(R_sc_M)));  %True anomaly in radians
    if dot(R_sc_M,V_sc_M) < 0  %Checking for quadrant
        nu_M = 2*pi - nu_M;
    end
    E_M = atan2(sin(nu_M)*(1-ecc_M^2),ecc_M+cos(nu_M)); %rad
    M_M = zeroTo360(E_M-ecc_M*sin(E_M),1); %rad
	fpa_M = atan2(ecc_M*sin(E_M),(1-ecc_M^2)^.5);  %This is really the current fpa, not fpa at mars

    if Rp_M < R_mars+400
        r = norm(R_sc_M);
        D1 = R3(-pi/2)*[R_sc_M;0]/r;
        A_sc_F = F/M*10^-3*D1(1:2);  %Accel due to thrust
        mdot = -F/Isp/go;
    elseif zeta_M >= 0
        A_sc_F = -F/M*10^-3*(V_sc_M)/norm(V_sc_M);  %Accel due to thrust
        mdot = -F/Isp/go;
    elseif zeta_M > -.1 && M_M < 5*pi/4 && M_M > 3*pi/4
        A_sc_F = F/M*10^-3*(V_sc_M)/norm(V_sc_M);  %Accel due to thrust
        mdot = -F/Isp/go;
    elseif Ra_M > R_mars + 300 && M_M > 7*pi/4 && M_M < pi/4
        A_sc_F = -F/M*10^-3*(V_sc_M)/norm(V_sc_M);  %Accel due to thrust
        mdot = -F/Isp/go;
    elseif Rp_M > R_mars + 300
        A_sc_F = -F/M*10^-3*(V_sc_M)/norm(V_sc_M);  %Accel due to thrust
        mdot = -F/Isp/go;
    else
        A_sc_F = [0;0];
        mdot = 0;
    end
elseif Ra < a_mars
    r = norm(R_sc);
    D1 = R3(-pi/2)*[R_sc;0]/r;
    A_sc_F = F/M*10^-3*D1(1:2);  %Accel due to thrust
    mdot = -F/Isp/go;
else
    A_sc_F = [0;0];
    mdot = 0;
end

A_sc = A_sc_grav + A_sc_F;

A_E = -mu_sun*R_E/norm(R_E)^3;
A_M = -mu_sun*R_M/norm(R_M)^3;

dX = [mdot; V_sc; A_sc; V_E; A_E; V_M; A_M];

if fix(z/10000)*10000 == z
    t
end
z = z + 1;