Code covered by the BSD License

### Highlights fromMATLAB simulation of fixed-mass rigid-body 6DOF

from MATLAB simulation of fixed-mass rigid-body 6DOF by Stacey Gage
Calculate aircraft fixed-mass rigid-body six-degrees-of-freedom equations of motion using MATLAB ODE

sixdof(Forces, Moments, Mass, Inertia, tarray, varargin )
```function [t,y] = sixdof(Forces, Moments, Mass, Inertia, tarray, varargin )
% SIXDOF  Calculate aircraft fixed-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
% Mass = fixed mass of the aircraft
% Inertia = 3x3 Inertia Tensor matrix
% 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
% 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 sixdof
%
% Ipos_i = [10 20 100];
% Ivel_b = [30 10 20];
% Ieuler = [0 0 0];
% Irates_b = [1 1 1];
% mass = 1.0;
% inertia = eye(3);
% Moments = [1 1 1]';
% Forces = [10 5 8]';
% tout = 1:100;
%
% [t_expected,y_expected]=sixdof(Forces,Moments,mass,inertia,tout,Ipos_i, ...
%                                  Ivel_b,Irates_b,Ieuler);

if nargin < 5
error('Less than the minimum 5 inputs.')
elseif nargin > 9
error('More than maximum 9 inputs.')
else
switch nargin
case 5
Ipos_i = [0 0 0];
Ivel_b = [0 0 0];
Irates_b = [0 0 0];
Ieuler = [0 0 0];
case 6
Ipos_i = varargin{1};
Ivel_b = [0 0 0];
Irates_b = [0 0 0];
Ieuler = [0 0 0];
case 7
Ipos_i = varargin{1};
Ivel_b = varargin{2};
Irates_b = [0 0 0];
Ieuler = [0 0 0];
case 8
Ipos_i = varargin{1};
Ivel_b = varargin{2};
Irates_b = varargin{3};
Ieuler = [0 0 0];
case 9
Ipos_i = varargin{1};
Ivel_b = varargin{2};
Irates_b = varargin{3};
Ieuler = varargin{4};
% 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 Mass <= 0
error('Mass should be a positive non-zero value.')
else
y0 = [Irates_b Ivel_b Ipos_i];
options = odeset('MaxStep',1/50); % set max step size to Simulink's
[t,y] = ode45(@f,tarray,y0,options,Forces,Mass,Moments,Inertia);

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function dydt = f(t,y,Forces,Mass,Moments,Inertia)

% dydt(1:3) = rates_dot
% dydt(4:6) = accel
% dydt(7:9) = vel
% y(1:3) = rates
% y(4:6) = vel
% y(7:9) = pos

dydt = [inv(Inertia)*(Moments - cross(y(1:3),Inertia*y(1:3))); ...
Forces/Mass - cross(y(1:3),y(4:6)); ...
y(4:6)];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
```