vllwb.m

来自「GPS TOOLBOX包含以下内容： 1、GPS相关常量和转换因子； 2、角」· M 代码 · 共 41 行

%                                vllwb.m
%  Scope:   This MATLAB macro performs the transformation from wander azimuth 
%           local level frame to GPS body frame for a given position vector 
%           and reference yaw/pitch/roll angles.
%  Usage:   pb = vllwb(pllw,yaw,pitch,roll)
%  Description of parameters:
%           pllw  -  input, local level wander azimuth position vector (with 
%                    components in meters)
%           yaw   -  input, yaw angle in radians
%           pitch -  input, pitch angle in radians
%           roll  -  input, roll angle in radians
%           pb    -  output, GPS body frame position vector (with components 
%                    in same units as the input local level wander azimuth 
%                    position vector)
%  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/04/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.

function   pb = vllwb(pllw,yaw,pitch,roll)

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

temp1 = sy * pllw(1) + cy * pllw(2);
temp2 = cy * pllw(1) - sy * pllw(2);
temp3 = sp * temp1 - cp * pllw(3);

pb = zeros(3,1);
pb(1) = sr * temp3 + cr * temp2;
pb(2) = cp * temp1 + sp * pllw(3);
pb(3) = sr * temp2 - cr * temp3;