导弹6自由度运动方程计算MATLAB.zip

function [t,y] = sixdofvm(Forces, Moments, dMass, dInertia, tarray, varargin ) % SIXDOFVM Calculate aircraft variable-mass rigid-body six-degrees-of-freedom % equations of motion using MATLAB ODE45 solver. % % =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= % Inputs: % =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= % Forces = 3x1 vector of forces in body coordinates % Moments = 3x1 vectory of moments in body coordinates % dMass = mass rate of change for the aircraft % dInertia = 3x3 Inertia Tensor matrix rate of change % tarray = time series vector % % OPTIONAL INPUTS: % Ipos_i = 3x1 vector of initial position % Ivel_b = 3x1 vector of initial velocity (body) % Irates_b = 3x1 vector of initial body rates % Imass = initial mass of aircraft % IInertia = initial 3x3 Inertia Tensor matrix % Ieuler = 3x1 vector of initial euler angles % % =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= % Outputs: % =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= % t = simulation time % y(1:3) = body rates % y(4:6) = velocity in body coordinates % y(7:9) = position in body coordinates % % =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= % An example of how to call sixdofvm % % Ipos_i = [10 20 100]; % Ivel_b = [30 10 20]; % Ieuler = [0 0 0]; % Irates_b = [1 1 1]; % Imass = 1.0; % IInertia = eye(3); % Moments = [1 1 1]'; % Forces = [10 5 8]'; % dmass = -0.01; % dinertia = -0.01*ones(3); % tout = 1:100; % % [t_expected,y_expected]=sixdofvm(Forces,Moments,dmass,dinertia,tout,Ipos_i, ... % Ivel_b,Irates_b,Imass,IInertia,Ieuler); if nargin < 5 error('Less than the minimum 5 inputs.') elseif nargin > 11 error('More than maximum 11 inputs.') else switch nargin case 5 Ipos_i = [0 0 0]; Ivel_b = [0 0 0]; Irates_b = [0 0 0]; Imass = 1.0; IInertia = eye(3); Ieuler = [0 0 0]; case 6 Ipos_i = varargin{ 1 }; Ivel_b = [0 0 0]; Irates_b = [0 0 0]; Imass = 1.0; IInertia = eye(3); Ieuler = [0 0 0]; case 7 Ipos_i = varargin{ 1 }; Ivel_b = varargin{ 2 }; Irates_b = [0 0 0]; Imass = 1.0; IInertia = eye(3); Ieuler = [0 0 0]; case 8 Ipos_i = varargin{ 1 }; Ivel_b = varargin{ 2 }; Irates_b = varargin{ 3 }; Imass = 1.0; IInertia = eye(3); Ieuler = [0 0 0]; case 9 Ipos_i = varargin{ 1 }; Ivel_b = varargin{ 2 }; Irates_b = varargin{ 3 }; Imass = varargin{ 4 }; IInertia = eye(3); Ieuler = [0 0 0]; case 10 Ipos_i = varargin{ 1 }; Ivel_b = varargin{ 2 }; Irates_b = varargin{ 3 }; Imass = varargin{ 4 }; IInertia = varargin{ 5 }; Ieuler = [0 0 0]; case 11 Ipos_i = varargin{ 1 }; Ivel_b = varargin{ 2 }; Irates_b = varargin{ 3 }; Imass = varargin{ 4 }; IInertia = varargin{ 5 }; Ieuler = varargin{ 6 }; % convert Ipos_i into body coordinates cph = cos(Ieuler(1)); sph = sin(Ieuler(1)); cth = cos(Ieuler(2)); sth = sin(Ieuler(2)); cps = cos(Ieuler(3)); sps = sin(Ieuler(3)); dcm = [cth*cps cth*sps -sth;... sph*sth*cps-cph*sps sph*sth*sps+cph*cps sph*cth; ... cph*sth*cps+sph*sps cph*sth*sps-sph*cps cph*cth]; Ipos_i = dcm*Ipos_i; end end if Imass <= 0 error('Mass should be a positive non-zero value.') else y0 = [Irates_b Ivel_b Ipos_i Imass reshape(IInertia,1,9)]; options = odeset('MaxStep',1/50,'Mass',@masscvm,... 'MStateDependence','strong'); [t,y] = ode45(@fcvm,tarray,y0,options,Forces,dMass,Moments,dInertia); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function dydt = fcvm(t,y,Forces,dMass,Moments,dInertia) % dydt(1:3) = rates_dot % dydt(4:6) = accel % dydt(7:9) = vel % dydt(10) = dMass % dydt(11:19) = dInertia % y(1:3) = rates % y(4:6) = vel % y(7:9) = pos % y(10) = Mass % y(11:19) = Inertia dydt = [Moments - cross(y(1:3),reshape(y(11:19),3,3)*y(1:3)) ... - dInertia*y(1:3); ... Forces - y(10)*cross(y(1:3),y(4:6)) - dMass*y(4:6); ... y(4:6);... dMass;... reshape(dInertia,9,1)]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function M = masscvm(t,y,Forces,dMass,Moments,dInertia) % Compute mass matrix M = zeros(19,19); M(1,1) = y(11); M(1,2) = y(14); M(1,3) = y(17); M(2,1) = y(12); M(2,2) = y(15); M(2,3) = y(18); M(3,1) = y(13); M(3,2) = y(16); M(3,3) = y(19); M(4,4) = y(10); M(5,5) = y(10); M(6,6) = y(10); M(7,7) = 1; M(8,8) = 1; M(9,9) = 1; M(10,10) = 1; M(11,11) = 1; M(12,12) = 1; M(13,13) = 1; M(14,14) = 1; M(15,15) = 1; M(16,16) = 1; M(17,17) = 1; M(18,18) = 1; M(19,19) = 1;