s_rock.m

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

function [sys,x0,str,ts] = s_rock(t,x,w,flag) 
%
% S-function for nonlinear simulation of the 3D motion of a rocket 
%
% Inputs:  t    - time in secs.
%          x    - rocket state.
%          w    - input signals:
%                 w(1) contains delta_x
%                 w(2) contains delta_y
%                 w(3) contains delta_z
%          flag - an integer value that indicates the task
%                 to be performed by the S-function:
%                 flag = 0 - initialize the state vector
%                 flag = 1 - calculate the state derivatives
%                 flag = 3 - calculate outputs 
%
% Outputs: sys  - a generic return argument whose values depend
%                 on the flag value.
%          x0   - the initial state values. x0 is ignored, except
%                 when flag = 0.
%          str  - argument reserved for future use.
%          ts   - a two column matrix containing the sample times
%                 and offsets of the blocks. For continuous time
%                 systems ts = [0 0].
%
%
% Dispatch the flag
%
switch flag,
    
case 0
%
%   Initialization
%
%   Call function simsizes to create the sizes structure.    
    sizes = simsizes;
%   Load the sizes structure with the initialization information.    
    sizes.NumContStates  = 10;
    sizes.NumDiscStates  = 0;
    sizes.NumOutputs     = 13;
    sizes.NumInputs      = 3;
    sizes.DirFeedthrough = 1;
    sizes.NumSampleTimes = 1;
%   Load the sys vector with the sizes information.    
    sys = simsizes(sizes);
% 
%   Initialize the state vector
%
    x0 = inc_rock;
%
%   str is an empty matrix.
%
    str = [];
%
    ts = [0 0];
    
case 1
%
%   Calculate derivatives
%
%   Fins deflections
    delta_x = w(1);
    delta_y = w(2);
    delta_z = w(3);
%
%   x(1) = V
%   x(2) = H
%   x(3) = Theta
%   x(4) = Psi
%   x(5) = omega_x
%   x(6) = omega_y
%   x(7) = omega_z
%   x(8) = psi
%   x(9) = theta
%   x(10) = gamma 
%
    V       = x(1);
    H       = x(2);
    Theta   = x(3);
    Psi     = x(4);
    omega_x = x(5);
    omega_y = x(6);
    omega_z = x(7);
    psi     = x(8);
    theta   = x(9);
    gamma   = x(10);
%
%   Determine angles
    beta    = asin((sin(theta)*sin(gamma)*cos(Psi-psi) - cos(gamma)*sin(Psi-psi))*cos(Theta) - ...
                    cos(theta)*sin(gamma)*sin(Theta));
    alpha   = asin((sin(theta)*cos(gamma)*cos(Psi-psi) + sin(gamma)*sin(Psi-psi))*cos(Theta) - ...
                    cos(theta)*cos(gamma)*sin(Theta))/cos(beta); 
    gamma_c = asin((cos(alpha)*sin(beta)*sin(theta) - (sin(alpha)*sin(beta)*cos(gamma) - ...
                    cos(beta)*sin(gamma))*cos(theta))/cos(Theta)); 
%
%   Determine forces, moments and parameters
    [m,G,P,J_x,J_y,J_z,x_G,x_C] ...
                  = prm_rock(t,V,H);                                      % rocket parameters
    [Q,Y,Z]       = frc_rock(t,V,H,alpha,beta,delta_x,delta_y,delta_z);   % N  forces
    [M_x,M_y,M_z] = mmn_rock(t,V,H,alpha,beta,omega_x,omega_y,omega_z, ...% Nm moments
                             delta_x,delta_y,delta_z);
%
%   Compute derivatives
%
%   Mass center motion
    dxdt(1) = (P*cos(alpha)*cos(beta) - Q - G*sin(Theta))/m;
    dxdt(2) =  V*sin(Theta);
    dxdt(3) = (P*(sin(alpha)*cos(gamma_c) + cos(alpha)*sin(beta)*sin(gamma_c)) + ...
               Y*cos(gamma_c) - Z*sin(gamma_c) - G*cos(Theta))/(m*V);
    dxdt(4) = (P*(sin(alpha)*sin(gamma_c) - cos(alpha)*sin(beta)*cos(gamma_c)) + ...
               Y*sin(gamma_c) + Z*cos(gamma_c))/(-m*V*cos(Theta));
%
%   Angular motion
    dxdt(5)  = (M_x - (J_z - J_y)*omega_y*omega_z)/J_x;
    dxdt(6)  = (M_y - (J_x - J_z)*omega_z*omega_x)/J_y;
    dxdt(7)  = (M_z - (J_y - J_x)*omega_x*omega_y)/J_z;
    dxdt(8)  = (omega_y*cos(gamma) - omega_z*sin(gamma))/cos(theta);
    dxdt(9)  = omega_y*sin(gamma) + omega_z*cos(gamma);
    dxdt(10) = omega_x - tan(theta)*(omega_y*cos(gamma) - omega_z*sin(gamma));
%
    sys = [dxdt(1) dxdt(2) dxdt(3) dxdt(4) dxdt(5) dxdt(6) dxdt(7) dxdt(8) ...
           dxdt(9) dxdt(10)]';
    
case 3
%
%   Calculate outputs
%
    delta_x = w(1);
    delta_y = w(2);
    delta_z = w(3);
%    
    V       = x(1);
    H       = x(2);
    Theta   = x(3);
    Psi     = x(4);
    omega_x = x(5);
    omega_y = x(6);
    omega_z = x(7);
    psi     = x(8);
    theta   = x(9);
    gamma   = x(10);
%    
    beta     = asin((sin(theta)*sin(gamma)*cos(Psi-psi) - cos(gamma)*sin(Psi-psi))*cos(Theta) - ...
                     cos(theta)*sin(gamma)*sin(Theta));
    alpha    = asin((sin(theta)*cos(gamma)*cos(Psi-psi) + sin(gamma)*sin(Psi-psi))*cos(Theta) - ...
                     cos(theta)*cos(gamma)*sin(Theta))/cos(Theta); 
    gamma_c  = asin((cos(alpha)*sin(beta)*sin(theta) - (sin(alpha)*sin(beta)*cos(gamma) - ...
                     cos(beta)*sin(gamma))*cos(theta))/cos(Theta));    
%
%   Accelerations
    [m,G,P,J_x,J_y,J_z,x_G,x_C] ...
             = prm_rock(t,V,H);                                    % rocket parameters 
    [Q,Y,Z]  = frc_rock(t,V,H,alpha,beta,delta_x,delta_y,delta_z); % N aerodynamic forces
%
    n_x = (P*cos(alpha)*cos(beta) - Q)/G;
    n_y = (P*sin(alpha) + Y)/G;
    n_z = (-P*cos(alpha)*sin(beta) + Z)/G;
%    
    n_x1 = n_x*cos(alpha)*cos(beta) + n_y*sin(alpha) - n_z*cos(alpha)*sin(beta);
    n_y1 = -n_x*sin(alpha)*cos(beta) + n_y*cos(alpha) + n_z*sin(alpha)*sin(beta);
    n_z1 = n_x*sin(beta) + n_z*cos(beta);
%
    sys = [gamma omega_y omega_z n_y1 n_z1 V H Theta Psi psi theta alpha beta]'; 
    
case { 2, 4, 9 } 
%
%   Unused flags
%
    sys = [];
otherwise
    error(['Unhandled flag = ',num2str(flag)]); % Error handling
end
% End of s_rock