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

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

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

x1 = pecef(1) * clon + pecef(2) * slon;
y1 = - pecef(1) * slon + pecef(2) * clon;
z1 = pecef(3);

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

pins = zeros(3,1);
pins(1) = - x2 * cwaz - y2 * swaz;
pins(2) = x2 * swaz - y2 * cwaz;
pins(3) = z2;