用matlab写一个多级火箭从发射到入轨考虑推力空气阻力含有俯仰角和偏航角的弹道方程

考虑推力、空气阻力、重力和惯性力等因素，多级火箭从发射到入轨的弹道方程可以表示为如下的微分方程组：
$$
\left\{
\begin{aligned}
\dot{v}_x &= \frac{T_x-D_x}{m}-g\sin\gamma \\
\dot{v}_y &= \frac{T_y-D_y}{m}+g\cos\gamma\sin\phi \\
\dot{v}_z &= \frac{T_z-D_z}{m}+g\cos\gamma\cos\phi \\
\dot{x} &= v_x \\
\dot{y} &= v_y \\
\dot{z} &= v_z \\
\dot{m} &= -\frac{F}{v_e} \\
\dot{\gamma} &= \frac{v_z\cos\phi-v_y\sin\phi}{v} \\
\dot{\phi} &= \frac{v_xv_z\cos\gamma-v_yv_z\sin\gamma}{v^2\cos\gamma}
\end{aligned}
\right.
$$
其中 $m$ 是火箭质量，$v_e$ 是喷气速度，$T_x,D_x,T_y,D_y,T_z,D_z$ 分别表示火箭在 $x,y,z$ 方向上的推力和空气阻力，$\gamma,\phi$ 分别为俯仰角和偏航角，$g$ 表示重力加速度，$F$ 表示火箭推力。

为了求解这个微分方程组，我们可以使用 MATLAB 中的 ode45 函数。代码如下：

function [t,x,y,z,vx,vy,vz,gamma,phi,m] = multi_stage_rocket()
% 多级火箭弹道计算

% 火箭参数
m0 = 2.5e4;             % 初始质量
m1 = 1.5e4;             % 第1级质量
m2 = 1.0e4;             % 第2级质量
m3 = 0.5e4;             % 第3级质量
t1 = 50;                % 第1级工作时间
t2 = 60;                % 第2级工作时间
t3 = 70;                % 第3级工作时间
v_e = 2.5e3;            % 喷气速度
F1 = 1.0e6;             % 第1级推力
F2 = 0.5e6;             % 第2级推力
F3 = 0.2e6;             % 第3级推力
A = pi*(2.5/2)^2;       % 火箭横截面积
Cd = 0.3;               % 火箭阻力系数
g = 9.8;                % 重力加速度

% 初始状态
x0 = 0;                 % 初始位置
y0 = 0;
z0 = 0;
vx0 = 0;                % 初始速度
vy0 = 0;
vz0 = 0;
gamma0 = 90*pi/180;     % 初始俯仰角
phi0 = 0;               % 初始偏航角
m0 = m0 + m1 + m2 + m3;

% 求解微分方程组
tspan = [0, 500];       % 计算时间范围
y0 = [vx0, vy0, vz0, x0, y0, z0, gamma0, phi0, m0];
options = odeset('RelTol', 1e-4, 'AbsTol', 1e-5);
[t, y] = ode45(@rocket_eq, tspan, y0, options);

% 获取结果
vx = y(:, 1);
vy = y(:, 2);
vz = y(:, 3);
x = y(:, 4);
y = y(:, 5);
z = y(:, 6);
gamma = y(:, 7);
phi = y(:, 8);
m = y(:, 9);
end

function dydt = rocket_eq(t, y)
% 火箭微分方程
vx = y(1);
vy = y(2);
vz = y(3);
x = y(4);
y = y(5);
z = y(6);
gamma = y(7);
phi = y(8);
m = y(9);

% 推力和阻力
F1 = 0; F2 = 0; F3 = 0; D1 = 0; D2 = 0; D3 = 0;
if t < t1
    F1 = F1_calc(t);
    D1 = D_calc(vx, vy, vz, gamma, phi, m1);
else
    m = m - m1;
end
if t >= t1 && t < t1+t2
    F2 = F2_calc(t-t1);
    D2 = D_calc(vx, vy, vz, gamma, phi, m2);
else
    m = m - m2;
end
if t >= t1+t2 && t < t1+t2+t3
    F3 = F3_calc(t-t1-t2);
    D3 = D_calc(vx, vy, vz, gamma, phi, m3);
else
    m = m - m3;
end
T_x = F1*cos(gamma)*cos(phi) + F2*cos(gamma)*cos(phi) + F3*cos(gamma)*cos(phi);
T_y = F1*cos(gamma)*sin(phi) + F2*cos(gamma)*sin(phi) + F3*cos(gamma)*sin(phi);
T_z = F1*sin(gamma)        + F2*sin(gamma)        + F3*sin(gamma);
D_x = D1*cos(gamma)*cos(phi) + D2*cos(gamma)*cos(phi) + D3*cos(gamma)*cos(phi);
D_y = D1*cos(gamma)*sin(phi) + D2*cos(gamma)*sin(phi) + D3*cos(gamma)*sin(phi);
D_z = D1*sin(gamma)        + D2*sin(gamma)        + D3*sin(gamma);

% 计算微分方程
dydt = zeros(9,1);
dydt(1) = (T_x-D_x)/m-g*sin(gamma);
dydt(2) = (T_y-D_y)/m+g*cos(gamma)*sin(phi);
dydt(3) = (T_z-D_z)/m+g*cos(gamma)*cos(phi);
dydt(4) = vx;
dydt(5) = vy;
dydt(6) = vz;
dydt(7) = (vz*cos(phi)-vy*sin(phi))/sqrt(vx^2+vy^2+vz^2);
dydt(8) = (vx*vz*cos(gamma)-vy*vz*sin(gamma))/(vx^2+vz^2);
dydt(9) = -F1/v_e*(t < t1) - F2/v_e*(t >= t1 && t < t1+t2) - F3/v_e*(t >= t1+t2 && t < t1+t2+t3);
end

function F = F1_calc(t)
% 第1级推力函数
F = 1.0e6;
end

function F = F2_calc(t)
% 第2级推力函数
F = 0.5e6;
end

function F = F3_calc(t)
% 第3级推力函数
F = 0.2e6;
end

function D = D_calc(vx, vy, vz, gamma, phi, m)
% 阻力函数
v = sqrt(vx^2+vy^2+vz^2);
rho = get_density(z);
D = 0.5*rho*v^2*Cd*A/m;
end

function rho = get_density(z)
% 获取高度为z处的大气密度
% 这里简化了大气模型，仅考虑了常数
rho0 = 1.2;
H = 8.5e3;
rho = rho0*exp(-z/H);
end

在上述代码中，我们定义了火箭参数、初始状态和微分方程，并使用 ode45 函数求解微分方程组。其中，函数 F1_calc、F2_calc 和 F3_calc 分别为第1、2、3级推力函数，D_calc 为阻力函数，get_density 函数获取高度为 z 处的大气密度。

注意，上述代码中的大气密度模型是一个简化模型，仅考虑了高度的影响，而没有考虑大气温度、湿度等其他影响因素。因此，如果需要更加准确的结果，需要使用更加复杂的大气模型。