飞行弹道仿真

clear;
clf;
global x_d H_d
global m V alpha theta omegaz x y H Theta deltaz n
global m_now V_now alpha_now theta_now omegaz_now x_now y_now H_now Theta_now deltaz_now
global t_step stepi step_all t
%% const or cal parameter
global m0 V0 x0 y0 H0
global ms q Sref Lref T0 Th P Cx Cy X Y Jz Mz mz
global rho_air0 rho_air rho_air_now
global k_phi k_phidiff

t_step = 0.001;
k_phi = -10;
k_phidiff = -5;
g = 9.8;
%% 方案弹道
x_d = 0: 1: 24000;
length_point = length(x_d);
H_d = zeros(1,length_point);
x_end = find(x_d < 9100);
x_end = x_end(end);
H_d(1:x_end) = 2000.*cos(0.000314.*1.1.*x_d(1:x_end))+5000;
H_d(x_end:end) = H_d(x_end);
plot(x_d,H_d);
axis([0,24000, 0,7000]);

step_all = 50 ./ t_step;

ms = 0.46;

rho_air0 = 1.2495;
T0 = 288.15;            
Jz = 315;               
P = 2500;
m0 = 320;               m_now = 320;            m       = zeros(1, step_all);
V0 = 250;               V_now = 250;            V       = zeros(1, step_all);
                        alpha_now = 0;          alpha   = zeros(1, step_all);
                        theta_now = 0;          theta   = zeros(1, step_all);
                        Theta_now = 0;          Theta   = zeros(1, step_all);
                        deltaz_now = 0;         deltaz  = zeros(1, step_all);
                        omegaz_now = 0;         omegaz  = zeros(1, step_all);
x0 = 0;                 x_now = 0;              x       = zeros(1, step_all);
H0 = 7000;              H_now = 7000;           H       = zeros(1, step_all);
Sref = 0.45;            Lref  = 2.5;

n = zeros(1, step_all);
%%  求解
stepi = 0;
while (m_now > 0) && (x_now < 24000) && (V_now > 0)
    if x_now > 9100
        P = 2500;
    else
        P = 0;
    end
    stepi = stepi + 1;
    Th  = T0 - 0.0065 * H_now;
    rho_air_now = rho_air0 * (Th ./ T0)^4.25588;
    q  = 0.5 * rho_air_now * V_now^2;
    Cy = 0.25 * alpha_now + 0.05 * deltaz_now;
    Y  = Cy * q * Sref;