stat_up.m

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

function [sys,x0,str,ts]=quat_upd(t,x,u,flag,dt,tiltxi,tiltyi,tiltzi,gbxi,gbyi,gbzi,gsfxi,gsfyi,gsfzi,dx,dy,dz)
%state update.

   s1 = zeros(3,1);
   s1x = zeros(3,3);
   s1los = zeros(3,1);
   c = zeros(3,3);
   odiag = zeros(3,3);
   s1b = zeros(3,1);
   s1bx = zeros(3,3);
   z = zeros(3,1);

   State_Phi = zeros(12,12);
   Q_tlt = zeros(3);
   P_mat = zeros(12,12);
   pm = zeros(12,12);
   State_Vector = zeros(12,1);
   sv = zeros(12,1);
   resid_mat = zeros(3,3);
   K_mat = zeros(12,3);
   H_mat = zeros(3,12);
   R_mat = zeros(3,3);
   Y_mat = zeros(12,12);
   y = zeros(12,1);

switch flag

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0         
    [sys,x0,str,ts] = mdlInitializeSizes(dt,tiltxi,tiltyi,tiltzi,gbxi,gbyi,gbzi,gsfxi,gsfyi,gsfzi,dx,dy,dz);

  %%%%%%%%%%%%%%%%%%%%%%%%
  % Others  &  Terminate %
  %%%%%%%%%%%%%%%%%%%%%%%%
  case {1,9}
    sys = []; % do nothing

  %%%%%%%%%%%%%%%
  % Update      %
  %%%%%%%%%%%%%%%
  case 2
    sys = mdlUpdate(t,x,u,dt);

  %%%%%%%%%%
  % Output %
  %%%%%%%%%%  
  case 3
    sys = mdlOutputs(t,x,u); 
  %%%%%%%%%%%%%%%
  % Update      %
  %%%%%%%%%%%%%%%
  
  case 4
    sys = mdlGetTimeofNextVarHit(t,x,u,dt);

    
  otherwise
    error(['unhandled flag = ',num2str(flag)]);
end

% end
%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [sys,x0,str,ts] = mdlInitializeSizes(dt,tiltxi,tiltyi,tiltzi,gbxi,gbyi,gbzi,gsfxi,gsfyi,gsfzi,dx,dy,dz)

   sizes = simsizes;
   sizes.NumContStates  = 0;
   sizes.NumDiscStates  =12*(12+1)/2+12;
   sizes.NumOutputs     =12;
   sizes.NumInputs      =13;
   sizes.DirFeedthrough = 1;
   sizes.NumSampleTimes = 1;
%   
   sys = simsizes(sizes);
   str = [];

   x0 = 0.0;
   x0(1)  = tiltxi^2;
   x0(13) = tiltyi^2;
   x0(24) = tiltzi^2;
   x0(34) = gbxi^2;
   x0(43) = gbyi^2;
   x0(51) = gbzi^2;
   x0(58) = gsfxi^2;
   x0(64) = gsfyi^2;
   x0(69) = gsfzi^2;
   x0(73) = dx^2;
   x0(76) = dy^2;
   x0(78) = dz^2;
   x0(90) = 0.0;
   
   ts  = [dt 0];   % sample time: [period, offset]

% end mdlInitializeSizes
%
%=============================================================================
% mdlUpdate
% Compute updates for discrete states.
%=============================================================================
%
function sys = mdlUpdate(t,x,u,dt)
%
   NumberStates = 12;

   s1 = [ u(11);
          u(12);
          u(13) ];
   s1x = [ 0.0     -s1(3,1)  s1(2,1);
           s1(3,1)  0.0     -s1(1,1);
          -s1(2,1)  s1(1,1)  0.0    ];
   
   q1 = u(1);
   q2 = u(2);
   q3 = u(3);
   q0 = u(4);
   ox = u(5);
   oy = u(6);
   oz = u(7);
   
   c(1,1) = q0^2+q1^2-q2^2-q3^2;
   c(1,2) = 2.0*(q1*q2-q0*q3);
   c(1,3) = 2.0*(q1*q3+q0*q2);
   c(2,1) = 2.0*(q1*q2+q0*q3);
   c(2,2) = q0^2-q1^2+q2^2-q3^2;
   c(2,3) = 2.0*(q2*q3-q0*q1);
   c(3,1) = 2.0*(q1*q3-q0*q2);
   c(3,2) = 2.0*(q2*q3+q0*q1);
   c(3,3) = q0^2-q1^2-q2^2+q3^2;

   odiag = [ ox  0.0 0.0;
             0.0 oy  0.0;
             0.0 0.0 oz ];
   
   s1b = c'*s1;
   s1bx = [  0        -s1b(3,1)  s1b(2,1);
             s1b(3,1)  0        -s1b(1,1);
            -s1b(2,1)  s1b(1,1)  0       ];

      k = 0;
      for i=1:NumberStates
          for j=i:NumberStates
             k = k+1;
             P_mat(i,j) = x(k);
             P_mat(j,i) = P_mat(i,j);
         end
         State_Vector(i,1) = x(i+78);
      end
%
% mapping to MATLAB states
% covariance matrix
%   P_mat(1,1) = x(1); P_mat(1,2) = x(2);  P_mat(1,3) = x(3);  P_mat(1,4) = x(4);  ...
%                      P_mat(2,2) = x(13); P_mat(2,3) = x(14); P_mat(2,4) = x(15); ...
%                                          P_mat(3,3) = x(24); P_mat(3,4) = x(25); ...
%                                                              P_mat(4,4) = x(34); ...
% state vector
%   State_Vector(1) = x(79); ...
  
      State_Phi = [ eye(3,3)   -c*dt      -c*odiag*dt zeros(3,3);
                    zeros(3,3) eye(3,3)   zeros(3,3)  zeros(3,3);
                    zeros(3,3) zeros(3,3) eye(3,3)    zeros(3,3);
                    zeros(3,3) zeros(3,3) zeros(3,3)  eye(3,3)  ];
                
      Q_tlt(1) = (0.005/(57.296*60.0))^2;
      Q_tlt(2) = (0.005/(57.296*60.0))^2;
      Q_tlt(3) = (0.005/(57.296*60.0))^2;
   
%  state vector propagation

      sv = State_Phi*State_Vector;
      State_Vector = sv;

%  propagate error covariance matrix

      pm = State_Phi*P_mat*State_Phi';
      P_mat = pm;
 
      for istate = 1:3
         P_mat(istate,istate) = P_mat(istate,istate) + Q_tlt(istate)*dt;
      end

%  measurement updates

      s1los(1,1) = u(8);
      s1los(2,1) = u(9);
      s1los(3,1) = u(10);

      z = s1los - c'*s1;
      H_mat = [ c'*s1x zeros(3,3) zeros(3,3) -s1bx ];
      
      R_var = (3.0e-5)^2;
      R_mat(1,1) = R_var;
      R_mat(2,2) = R_var;
      R_mat(3,3) = R_var;

      Y_mat = eye(NumberStates);

      resid_mat = H_mat*P_mat*H_mat' + R_mat;
      K_mat = P_mat*H_mat'*inv(resid_mat);
      pm = (Y_mat - K_mat*H_mat)*P_mat;
      P_mat = pm;

      sv = State_Vector + K_mat*(z - H_mat*State_Vector);
      State_Vector = sv;

      k = 0;
      for i=1:NumberStates
         for j=i:NumberStates
             k = k+1;
             x(k) = P_mat(i,j);
         end
         x(i+78) = State_Vector(i,1);
      end
   
   sys = x;
    
% end mdlUpdate

%
%=============================================================================
% mdlOutputs
% Return the output vector for the S-function
%=============================================================================
%
function sys = mdlOutputs(t,x,u)

   y(1) = x(79);
   y(2) = x(80);
   y(3) = x(81);
   y(4) = x(82);
   y(5) = x(83);
   y(6) = x(84);
   y(7) = x(85);
   y(8) = x(86);
   y(9) = x(87);
   y(10) = x(88);
   y(11) = x(89);
   y(12) = x(90);
  
   sys = y;
   
% end mdlOutputs

%=============================================================================
% mdlGetTimeofNextVarHit
%=============================================================================
function sys = mdlGetTimeofNextVarHit(t,x,u,dt)

   sys = t + dt;
   
% end mdlGetTimeofNextVarHit