%                                 uverv.m
%  Scope:   This MATLAB macro computes the unit vertical vector (up)
%           for a given ECEF position vector; WGS-84 constants are used.
%  Usage:   up = uverv(p)
%  Description of parameters:
%           p   -  input, ECEF position vector, the components are in meters
%           up  -  output, ECEF unit vertical vector (up), the components are 
%                  in meters
%  External Matlab macros used:  wgs84con
%  Last update:  06/14/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.

function  up = uverv(p)

%  Initialize the constants; the statement can be eliminated if the initialization
%  has be done before in the calling macro

wgs84con;  
% global constants used: a_smaxis, b_smaxis, eccentr2, onemecc2

%  Compute the unit vertical vector (up)

up = zeros(3,1);
w = sqrt(p(1) * p(1) + p(2) * p(2));
if  w >= 0.1 
   rn = a_smaxis;
   h = 0.;
   q = p(3) / w;
   for iter = 1:5
      tantemp = (q * (rn + h)) / (rn * onemecc2 + h);
      temp = tantemp * tantemp;
      sin2 = temp / (1. + temp);
      rn = a_smaxis / sqrt(1. - eccentr2 * sin2);
      h = w * sqrt(1. + temp) - rn;
   end
else
   rn = b_smaxis / onemecc2;
   h = abs(p(3)) - b_smaxis;
end
temp = rn + h;
up(1) = p(1) / temp;
up(2) = p(2) / temp;
up(3) = p(3) / (rn * onemecc2 + h);