ad_direct.m

利用gps多天线载波相位技术

%%========================================
%%     Toolbox for attitude determination
%%     Zhen Dai
%%     [e-mail]
%%     ZESS, University of Siegen, Germany
%%     Last Modified  : 1.Sep.2008
%%========================================
%% Functions:
%%      Direct attitude determination
%% Input parameters:     
%%      vSlaveAntLocal1,vSlaveAntLocal2:
%%          ->Local level coordinates of the 1st and 2nd slave antennas.
%%          The order of both can not be confused, otherwise another body
%%          frame will be produced.     
%% Output:
%%      yaw_deg,roll_deg,pitch_deg:
%%          -> Attitude parameters expressed in Euler angles in degrees.
%% Remarks:
%%      Master antenna coordinate is not needed, beccause it is always [0 0 0]
%%  Reference:
%%      B.Hofmann-Wellenhof, H.Lichtenegger and J.Collins: GPS Theory
%%      and practice. 2001. Fifth revised edition. Springer, Wien, New York.
%%      pp.327-328.

function [yaw_deg,roll_deg,pitch_deg]=AD_Direct(vSlaveAntLocal1,vSlaveAntLocal2)
%% ~~~~~~~~~~~~ Step 1 ~~~~~~~~~~~~~~~
%% Using Ant 2 to derive yaw and pitch
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
x2=vSlaveAntLocal1(1);
y2=vSlaveAntLocal1(2);
z2=vSlaveAntLocal1(3);
x3=vSlaveAntLocal2(1);
y3=vSlaveAntLocal2(2);
z3=vSlaveAntLocal2(3);
%% Get yaw
yaw_arc=-atan(x2/y2);
yaw_deg=GetAngleDeg(yaw_arc);
%% Get pitch
pitch_arc=atan(z2/sqrt(x2*x2+y2*y2));
pitch_deg=GetAngleDeg(pitch_arc);

%% ~~~~~~~~~~~~ Step 2 ~~~~~~~~~~~~~~~
%% Rotate Ant 3 to derive roll
%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

%% 1st rotation matrix-- rotate about Z by yaw
R3=[cos(yaw_arc) sin(yaw_arc) 0;...
    -sin(yaw_arc)   cos(yaw_arc) 0;...
    0                  0             1];
%% 2nd rotation matrix-- rotate about X' by pitch
R1=[1                0              0;
    0     cos(pitch_arc) sin(pitch_arc);
    0      -sin(pitch_arc) cos(pitch_arc)];
%% Now rotate the Ant 3
mRotation1=R3*[x3 y3 z3]';
mRotation2=R1*mRotation1;

%% ~~~~~~~~~~~~ Step 2 ~~~~~~~~~~~~~~~
%% Get roll
%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%% Roll
roll_arc=-atan(mRotation2(3)/mRotation2(1));
roll_deg=GetAngleDeg(roll_arc);