function [Ps] = GetPs(Cbn)
%*********************************************************
%************ 從方向余弦陣中解算姿態(tài)角 *******************
% Ps -- 姿態(tài)角向量;       Cbn - 方向余弦陣;   參考書(shū)P 65
%***********************************************姿態(tài)角解算
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橫滾角符號(hào)判別
      if   Garma<0
           Garma = Garma+pi;
      else Garma = Garma-pi;
      end
end
if Cbn(2,2)<0         %Pothi航向角符號(hào)判別
      Pothi = Pothi+pi;
elseif Pothi<0
      Pothi = Pothi+2*pi;
end
%************************************************姿態(tài)角裝訂
Ps = [Pothi Theta Garma]';
%**********************************************************