quat_dyn.m

导航系统设计数学基础应用代码（matlab),包括各章节的代码。

function [sys,x0,str,ts]=quat_dyn(t,x,u,flag,yawi,pitchi,rolli)
%quaternion dynamics.
%   
%   initial conditions (yawi,pitchi,rolli).
%   
   qdot = zeros(4);
   
switch flag

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0         
    [sys,x0,str,ts] = mdlInitializeSizes(yawi,pitchi,rolli);

  %%%%%%%%%%%%%%%
  % Derivatives %
  %%%%%%%%%%%%%%%
  case 1
    sys = mdlDerivatives(t,x,u);

  %%%%%%%%%%%%%%%%%%%%%%%%
  % Update and Terminate %
  %%%%%%%%%%%%%%%%%%%%%%%%
  case {2,9}
    sys = []; % do nothing

  %%%%%%%%%%
  % Output %
  %%%%%%%%%%
  case 3
    sys = mdlOutputs(t,x,u); 

  otherwise
    error(['unhandled flag = ',num2str(flag)]);
    
end
% end attdyn_DCM
%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [sys,x0,str,ts] = mdlInitializeSizes(yawi,pitchi,rolli)

sizes = simsizes;
sizes.NumContStates  = 4;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 4;
sizes.NumInputs      = 3;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1;

sys = simsizes(sizes);
str = [];

   yaw = yawi*pi/180.0;
   pitch = pitchi*pi/180.0;
   roll = rolli*pi/180.0;
   cxxi = cos(pitch)*cos(yaw);
   cxyi = sin(roll)*sin(pitch)*cos(yaw)-cos(roll)*sin(yaw);
   cyxi = cos(pitch)*sin(yaw);
   cyyi = sin(roll)*sin(pitch)*sin(yaw)+cos(roll)*cos(yaw);
   czxi = -sin(pitch);
   czyi = sin(roll)*cos(pitch);
   cxzi = cyxi*czyi-cyyi*czxi;
   cyzi = cxyi*czxi-cxxi*czyi;
   czzi = cxxi*cyyi-cxyi*cyxi;
   q0 = sqrt((cxxi+cyyi+czzi+1.0)/4.0);
   q1 = (czyi-cyzi)/(4.0*q0);
   q2 = (cxzi-czxi)/(4.0*q0);
   q3 = (cyxi-cxyi)/(4.0*q0);
   x0(1) = q1;
   x0(2) = q2;
   x0(3) = q3;
   x0(4) = q0;

ts  = [0 0];   % sample time: [period, offset]

% end mdlInitializeSizes
%
%=============================================================================
% mdlDerivatives
% Compute derivatives for continuous states.
%=============================================================================
%
function sys = mdlDerivatives(t,x,u)

   qdot(4) = -0.5*(             u(1)*x(1) + u(2)*x(2) + u(3)*x(3) );
   qdot(1) = -0.5*(-u(1)*x(4)             - u(3)*x(2) + u(2)*x(3) );
   qdot(2) = -0.5*(-u(2)*x(4) + u(3)*x(1)             - u(1)*x(3) );
   qdot(3) = -0.5*(-u(3)*x(4) - u(2)*x(1) + u(1)*x(2)             );
   
   sys = qdot;
  
% end mdlDerivatives
%
%=============================================================================
% mdlOutputs
% Return the output vector for the S-function
%=============================================================================
%
function sys = mdlOutputs(t,x,u)

   sys = x;

% end mdlOutputs