sixdofvm.m

变质量刚体模型的6自由度运动matlab仿真程序

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;