dcm_eulr.m

来自「惯性导航系统传递对准系统模型程序」· M 代码 · 共 45 行

function eul_vect = dcm_eulr(DCMnb)            %论文的P68。DCMnb就是Cms*估计矩阵

  if nargin<1,error('insufficient number of input arguments'),end

  theta = asin(DCMnb(2,3));
  phi = atan(-DCMnb(1,3)/DCMnb(3,3));
  psi = atan(-DCMnb(2,1)/DCMnb(2,2));
  
  if DCMnb(3,3) < 0
      if phi < 0
          phi = phi + pi;
      elseif phi>0
          phi = phi - pi;
      end
  end
  
  if DCMnb(2,2) > 0
      if psi < 0
          psi = psi+2*pi;
      end
  elseif DCMnb(2,2) < 0
      psi = psi+pi;
  end
  if psi>pi
      psi=psi-2*pi;
  end
% 
%    if DCMnb(3,3) < 0
%       if phi < 0
%           phi = phi + pi;
%       elseif phi>0
%           phi = phi - pi;
%       end
%   end
%   
%   if DCMnb(3,3) < 0
%       if psi < 0
%           psi = psi + pi;
%       elseif psi>0
%           psi = psi - pi;
%       end
%   end
  
  eul_vect = [theta phi psi]';