rocket.rar_nonlinear control_robust control_火箭_火箭六自由度_火箭模型

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