getcbn.m

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

function [Cbn] = GetCbn(Ps)
%***************************************************************
%***** Get the Direction Cos Array from Positon Anganl *********
% Cbn - 方向余弦阵    Ps = [Pothi Theta Garma]' - 姿态角向量
%***************************************************************
Pothi = Ps(1);  Theta = Ps(2);   Garma = Ps(3);
%***********************************
T11 =  cos(Garma)*cos(Pothi) - sin(Garma)*sin(Theta)*sin(Pothi);
T12 = -cos(Theta)*sin(Pothi);
T13 =  sin(Garma)*cos(Pothi) + cos(Garma)*sin(Theta)*sin(Pothi);
T21 =  cos(Garma)*sin(Pothi) + sin(Garma)*sin(Theta)*cos(Pothi);
T22 =  cos(Theta)*cos(Pothi);
T23 =  sin(Garma)*sin(Pothi) - cos(Garma)*sin(Theta)*cos(Pothi);
T31 = -sin(Garma)*cos(Theta);
T32 =  sin(Theta);
T33 =  cos(Garma)*cos(Theta);
%***********************************
Cbn = [T11  T12  T13;
       T21  T22  T23;
       T31  T32  T33];
%****************************************************************