mbllw.m

来自「GPS software toolbox for GPS receiver de」· M 代码 · 共 39 行

%                                mbllw.m
%  Scope:   This MATLAB macro constructs the transformation matrix from GPS 
%           body frame to LLWA (Local Level Wander Azimuth) frame.
%  Usage:   moutput = mbllw(yaw,pitch,roll)
%  Description of parameters:
%           yaw      -  input, yaw angle in radians
%           pitch    -  input, pitch angle in radians
%           roll     -  input, roll angle in radians
%           moutput  -  output, two-dimensional real array containing the 
%                       transformation matrix from GPS body frame to LLWA
%                       frame
%  Remark:  The coordinate axes for wander azimuth local level frame are 
%           defined as follows:
%           x - axis at wander azimuth angle counter-clockwise from East in
%               local level plane,
%           y - axis at wander azimuth angle counter-clockwise from North in
%               local level plane,
%           z - axis upward along local normal
%  Last update:  04/05/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.

function   moutput = mbllw(yaw,pitch,roll)

sy = sin(yaw);
cy = cos(yaw);
sp = sin(pitch);
cp = cos(pitch);
sr = sin(roll);
cr = cos(roll);

moutput(1,1) =   sr * sp * sy + cr * cy;
moutput(1,2) =   cp * sy;
moutput(1,3) =   sr * cy - cr * sp * sy;
moutput(2,1) =   sr * sp * cy - cr * sy;
moutput(2,2) =   cp * cy;
moutput(2,3) = - sr * sy - cr * sp * cy;
moutput(3,1) = - sr * cp;
moutput(3,2) =   sp;
moutput(3,3) =   cr * cp;