vbllw.m

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

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

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

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

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