Convert WGS-84 geodetic locations (GPS readings) to Cartesian coordinates in a local tangent plane (Geodetic to ECEF to ENU)

GpsUtils.cs

using System;
using System.Diagnostics;

    // Some helpers for converting GPS readings from the WGS84 geodetic system to a local North-East-Up cartesian axis.

    // The implementation here is according to the paper:
    // "Conversion of Geodetic coordinates to the Local Tangent Plane" Version 2.01.
    // "The basic reference for this paper is J.Farrell & M.Barth 'The Global Positioning System & Inertial Navigation'"
    // Also helpful is Wikipedia: http://en.wikipedia.org/wiki/Geodetic_datum
    public class GpsUtils
    {
	// WGS-84 geodetic constants
	const double a = 6378137;           // WGS-84 Earth semimajor axis (m)
	const double b = 6356752.3142;      // WGS-84 Earth semiminor axis (m)
	const double f = (a - b) / a;           // Ellipsoid Flatness
	const double e_sq = f * (2 - f);    // Square of Eccentricity

	// Converts WGS-84 Geodetic point (lat, lon, h) to the 
	// Earth-Centered Earth-Fixed (ECEF) coordinates (x, y, z).
	public static void GeodeticToEcef(double lat, double lon, double h,
									  out double x, out double y, out double z)
	{
		// Convert to radians in notation consistent with the paper:
		var lambda = DegreesToRadians(lat);
		var phi = DegreesToRadians(lon);
		var s = Math.Sin(lambda);
		var N = a / Math.Sqrt(1 - e_sq * s * s);

		var sin_lambda = Math.Sin(lambda);
		var cos_lambda = Math.Cos(lambda);
		var cos_phi = Math.Cos(phi);
		var sin_phi = Math.Sin(phi);

		x = (h + N) * cos_lambda * cos_phi;
		y = (h + N) * cos_lambda * sin_phi;
		z = (h + (1 - e_sq) * N) * sin_lambda;
	}

	// Converts the Earth-Centered Earth-Fixed (ECEF) coordinates (x, y, z) to 
	// East-North-Up coordinates in a Local Tangent Plane that is centered at the 
	// (WGS-84) Geodetic point (lat0, lon0, h0).
	public static void EcefToEnu(double x, double y, double z,
								 double lat0, double lon0, double h0,
								 out double xEast, out double yNorth, out double zUp)
	{
		// Convert to radians in notation consistent with the paper:
		var lambda = DegreesToRadians(lat0);
		var phi = DegreesToRadians(lon0);
		var s = Math.Sin(lambda);
		var N = a / Math.Sqrt(1 - e_sq * s * s);

		var sin_lambda = Math.Sin(lambda);
		var cos_lambda = Math.Cos(lambda);
		var cos_phi = Math.Cos(phi);
		var sin_phi = Math.Sin(phi);

		double x0 = (h0 + N) * cos_lambda * cos_phi;
		double y0 = (h0 + N) * cos_lambda * sin_phi;
		double z0 = (h0 + (1 - e_sq) * N) * sin_lambda;

		double xd, yd, zd;
		xd = x - x0;
		yd = y - y0;
		zd = z - z0;

		// This is the matrix multiplication
		xEast = -sin_phi * xd + cos_phi * yd;
		yNorth = -cos_phi * sin_lambda * xd - sin_lambda * sin_phi * yd + cos_lambda * zd;
		zUp = cos_lambda * cos_phi * xd + cos_lambda * sin_phi * yd + sin_lambda * zd;
	}

	// Converts the geodetic WGS-84 coordinated (lat, lon, h) to 
	// East-North-Up coordinates in a Local Tangent Plane that is centered at the 
	// (WGS-84) Geodetic point (lat0, lon0, h0).
	public static void GeodeticToEnu(double lat, double lon, double h,
									 double lat0, double lon0, double h0,
									 out double xEast, out double yNorth, out double zUp)
	{
		double x, y, z;
		GeodeticToEcef(lat, lon, h, out x, out y, out z);
		EcefToEnu(x, y, z, lat0, lon0, h0, out xEast, out yNorth, out zUp);
	}

	// Just check that we get the same values as the paper for the main calculations.
	public static void Test()
	{
		var latLA = 34.00000048;
		var lonLA = -117.3335693;
		var hLA = 251.702;

		double x0, y0, z0;
		GeodeticToEcef(latLA, lonLA, hLA, out x0, out y0, out z0);

		Debug.Assert(AreClose(-2430601.8, x0));
		Debug.Assert(AreClose(-4702442.7, y0));
		Debug.Assert(AreClose(3546587.4, z0));

		// Checks to read out the matrix entries, to compare to the paper
		double x, y, z;
		double xEast, yNorth, zUp;

		// First column
		x = x0 + 1;
		y = y0;
		z = z0;
		EcefToEnu(x, y, z, latLA, lonLA, hLA, out xEast, out yNorth, out zUp);
		Debug.Assert(AreClose(0.88834836, xEast));
		Debug.Assert(AreClose(0.25676467, yNorth));
		Debug.Assert(AreClose(-0.38066927, zUp));

		x = x0;
		y = y0 + 1;
		z = z0;
		EcefToEnu(x, y, z, latLA, lonLA, hLA, out xEast, out yNorth, out zUp);
		Debug.Assert(AreClose(-0.45917011, xEast));
		Debug.Assert(AreClose(0.49675810, yNorth));
		Debug.Assert(AreClose(-0.73647416, zUp));

		x = x0;
		y = y0;
		z = z0 + 1;
		EcefToEnu(x, y, z, latLA, lonLA, hLA, out xEast, out yNorth, out zUp);
		Debug.Assert(AreClose(0.00000000, xEast));
		Debug.Assert(AreClose(0.82903757, yNorth));
		Debug.Assert(AreClose(0.55919291, zUp));

	}

	static bool AreClose(double x0, double x1)
	{
		var d = x1 - x0;
		return (d * d) < 1e-2;
	}

	static double DegreesToRadians(double degrees)
	{
		return Math.PI / 180.0 * degrees;
	}

	static double RadiansToDegrees(double radians)
	{
		return 180.0 / Math.PI * radians;
	}
}