用matlab编写卫星高斯摄动方程函数

以下是一个简单的 Matlab 函数，用于计算卫星在高斯摄动下的运动状态：

function [r, v] = gauss_perturbation(mu, r0, v0, t)
% mu: 引力常数
% r0: 初始位置矢量
% v0: 初始速度矢量
% t: 时间间隔

% 计算初始轨道参数
a = norm(r0);
e = norm(v0)^2/2 - mu/a;
h = cross(r0,v0);
i = acos(h(3)/norm(h));
N = cross([0 0 1],h);
N = N/norm(N);
if N(2) < 0
    RAAN = 2*pi - acos(N(1));
else
    RAAN = acos(N(1));
end
if e < 1e-10
    w = 0;
else
    E = acos((a*(1-e^2)/norm(r0)-1)/e);
    if dot(r0,v0) < 0
        E = 2*pi - E;
    end
    w = acos(dot(r0,E)-a*(1-e^2)/norm(r0)/e);
    if r0(3) < 0
        w = 2*pi - w;
    end
end
M = E - e*sin(E);

% 计算摄动项
n = sqrt(mu/a^3);
J2 = 1.0826e-3;
R = 6378.137;
p = a*(1-e^2);
Omega = 7292115.8553e-11;
omega_dot = -1.5*J2*n*R^2/p*(1-5/2*sin(i)^2)*cos(i);
Omega_dot = 1.5*J2*n*R^2/p*(1-5/2*sin(i)^2)*cos(i)*cos(i)*(1-3*sin(RAAN)^2);
M_dot = n + 1.5*J2*n*(R/p)^2*(1-7/2*sin(i)^2)*(1-3*sin(RAAN)^2)*cos(w);

% 计算时间间隔内的轨道参数变化量
dM = M_dot*t;
M = M + dM;
domega = omega_dot*t;
w = w + domega;
dRAAN = Omega_dot*t;
RAAN = RAAN + dRAAN;

% 计算新的位置和速度矢量
E = M;
for i = 1:100
    E_old = E;
    E = M + e*sin(E);
    if abs(E-E_old) < 1e-10
        break
    end
end
f = 2*atan(sqrt((1+e)/(1-e))*tan(E/2));
r = p/(1+e*cos(f));
x = r*cos(f);
y = r*sin(f);
z = 0;
r = [x y z]';
v = sqrt(mu/p)*[-sin(f) e+cos(f) 0]';

R3 = [cos(-RAAN) sin(-RAAN) 0; -sin(-RAAN) cos(-RAAN) 0; 0 0 1];
R1 = [1 0 0; 0 cos(-i) sin(-i); 0 -sin(-i) cos(-i)];
R3_w = [cos(-w) sin(-w) 0; -sin(-w) cos(-w) 0; 0 0 1];
Q = R3*R1*R3_w;
r = Q*r;
v = Q*v + cross([0 0 Omega]',r);
end

这个函数接受四个参数：引力常数 mu、初始位置矢量 r0、初始速度矢量 v0，以及时间间隔 t。它将返回一个长度为 3 的列向量 r，表示卫星在时间间隔后的位置矢量，以及一个长度为 3 的列向量 v，表示卫星在时间间隔后的速度矢量。