getps.m

自己编写的鲁棒滤波算法的应用

function [Ps] = GetPs(Cbn)
%*********************************************************
%************ 从方向余弦阵中解算姿态角 *******************
% Ps -- 姿态角向量;       Cbn - 方向余弦阵;   参考书P 65
%***********************************************姿态角解算
Pothi = atan(-Cbn(1,2)/Cbn(2,2));
Theta = asin(Cbn(3,2));
Garma = atan(-Cbn(3,1)/Cbn(3,3));
%*****************************************
if Cbn(3,3)<0         %Garma横滚角符号判别
      if   Garma<0
           Garma = Garma+pi;
      else Garma = Garma-pi;
      end
end
if Cbn(2,2)<0         %Pothi航向角符号判别
      Pothi = Pothi+pi;
elseif Pothi<0
      Pothi = Pothi+2*pi;
end
%************************************************姿态角装订
Ps = [Pothi Theta Garma]';
%**********************************************************