sol_rock.m

来自「导弹控制系统的鲁棒控制设计实例」· M 代码 · 共 42 行

% Solution of rocket differential equations
L           = 2.10;        % m       rocket length
S           = 0.081;       % m^2     reference surface area
x_R         = 0.3;         % m       rocket fins coordinate
%
t                          % s       time
V           = 300;         % m/s     initial speed
%H           =  1000;       % m       initial altitude
H           =  5000;       % m       initial altitude
%H           = 10000;       % m       initial altitude
Theta       = 0;           % rad
%
f           = [V; H; Theta];
[t_r,f]     = ode23tb(@dif_rock,[0 t],f);
[nr,nc]     = size(f);
V           = f(nr,1)
H           = f(nr,2)
Theta       = f(nr,3);

%
% Fly parameters
[p,ro,M,Tm] = air_data(V,H);
q           = ro*V^2/2;
%
% Rocket parameters
[m,G,P,J_x,J_y,J_z,x_G,x_C] ...   
            = prm_rock(t,V,H);
%
% Aerodynamic coefficients
c_x         = cx_fct(M);
c_x_delta_z = 0.032;
c_y_alpha   = cy_fct(M);
c_y_delta_z = 0.27;
m_z_alpha   = (c_x + c_y_alpha)*(x_G - x_C)/L;
m_z_delta_z = c_y_delta_z*(x_G - x_R)/L;
%
% Set the pitch angle
theta       = theta_rock(t);
alpha       = theta - Theta;
%
% Control action
delta       = -(m_z_alpha/m_z_delta_z)*alpha;