dif_rock.m

导弹控制系统的鲁棒控制设计实例

function dfdt = difeq(t,f)
%
% Rocket differential equations
%
L           = 2.1;         % m       rocket length
S           = 0.081;       % m^2     reference surface area
x_R         = 0.3;         % m       rocket fins coordinate
%
V           = f(1);
H           = f(2);
Theta       = f(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;
%
% Determine forces
Q_a         = c_x*q*S;
Q_c         = c_x_delta_z*abs(delta)*q*S;
Y_a         = c_y_alpha*alpha*q*S;
Y_c         = c_y_delta_z*delta*q*S;
Q           = Q_a + Q_c;
Y           = Y_a + Y_c;
%
dfdt(1) = (P*cos(alpha) - Q - G*sin(Theta))/m;
dfdt(2) = V*sin(Theta);
dfdt(3) = (P*sin(alpha) + Y - G*cos(Theta))/(m*V);
%
dfdt = [dfdt(1); dfdt(2); dfdt(3)];