getcombinedrotationmatrix.m

来自「利用gps多天线载波相位技术」· M 代码 · 共 39 行

%%========================================
%%     Toolbox for attitude determination
%%     Zhen Dai
%%     [e-mail]
%%     ZESS, University of Siegen, Germany
%%     Last Modified  : 1.Sep.2008
%%========================================
%% Functions:
%%      Calculate the combined rotation matrix according to the given yaw,
%%      roll and pitch
%% Input parameters:
%%      yaw_deg -> Yaw in degree
%%      roll_deg -> Roll in degree
%%      pitch_deg -> Pitch in degree
%% Output:
%%      mR -> combined rotation matrix in the sequence of 3-1-2
%% Reference:
%%       B.Hofmann-Wellenhof, H.Lichtenegger and J.Collins: GPS Theory
%%       and practice. 2001. Fifth revised edition. Springer, Wien, New York.
%%       p. 327       

function mR=GetCombinedRotationMatrix(yaw_deg,roll_deg,pitch_deg)

%% angles in arc
roll_arc=roll_deg*pi/180;
pitch_arc=pitch_deg*pi/180;
yaw_arc=yaw_deg*pi/180;

cr=cos(roll_arc);
sr=sin(roll_arc);
cp=cos(pitch_arc);
sp=sin(pitch_arc);
cy=cos(yaw_arc);
sy=sin(yaw_arc);

mR =[cr*cy-sr*sp*sy    cr*sy+sr*sp*cy    -sr*cp;
             -cp*sy                       cp*cy                sp;
           sr*cy+cr*sp*sy    sr*sy-cr*sp*cy     cr*cp];