function ENU = LLH2ENU( Lat, Lon, Hgt, RefLat, RefLon, RefHgt)

% ********************************************************************
% Assumes the inputs are in radians, returns the E/N/U relative the
% reference position.
%
% Input Lat, Lon, Hgt, RefLat, RefLon, RefHgt
% ********************************************************************

% Compute the residuals
dlat = Lat - RefLat;
dlon = Lon - RefLon;
dhgt = Hgt - RefHgt;



% Define WGS84 Ellipsoid Parameters
% PI from st_const.
% PI = 3.14159265358979323846;
a  = 6378137;
f  = 1 / 298.257223563;
e2 = 2*f - f^2;

% Compute Radii of Curvature
W = sqrt( 1 - e2 .* sin(RefLat).^2 );
N = a ./ W;
M = a .* ( 1 - e2 ) ./ W.^3;

% Compute Metric Components
dE = dlon .* N .* cos(RefLat);
dN = dlat .* M;
dU = dhgt;

ENU(:,1) = dE;
ENU(:,2) = dN;
ENU(:,3) = dU;

