vinsecef.m

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

%                              vinsecef.m
%  Scope:   This MATLAB macro performs the transformation from INS platform 
%           frame (Wander / North, West, Up) to ECEF coordinates for a given 
%           position vector and reference latitude/longitude/wander azimuth
%           angles.
%  Usage:   pecef = vinsecef(pins,lat,lon,waz)
%  Description of parameters:
%           pins  -  input, INS platform position vector (with components in 
%                    meters)
%           lat   -  input, reference latitude in radians
%           lon   -  input, reference longitude in radians
%           waz   -  input, reference wander azimuth angle in radians
%           pecef -  output, ECEF position vector, with components in same
%                    units as the input INS platform position vector
%  Last update:  04/04/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.

function   pecef = vinsecef(pins,lat,lon,waz)

slat = sin(lat);
clat = cos(lat);
slon = sin(lon);
clon = cos(lon);
swaz = sin(waz);
cwaz = cos(waz);

x1 = - pins(1) * cwaz + pins(2) * swaz;
y1 = - pins(1) * swaz - pins(2) * cwaz;
z1 = pins(3);

x2 = x1 * slat + z1 * clat;
y2 = y1;
z2 = - x1 * clat + z1 * slat;

pecef = zeros(3,1);
pecef(1) = x2 * clon - y2 * slon;
pecef(2) = x2 * slon + y2 * clon;
pecef(3) = z2;