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;
using static System.Math;

// 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
// Also helpful are the guidance notes here: http://www.epsg.org/Guidancenotes.aspx
public class GpsUtils
{

    // WGS-84 geodetic constants
    const double a = 6378137.0;         // WGS-84 Earth semimajor axis (m)

    const double b = 6356752.314245;     // Derived Earth semiminor axis (m)
    const double f = (a - b) / a;           // Ellipsoid Flatness
    const double f_inv = 1.0 / f;       // Inverse flattening

    //const double f_inv = 298.257223563; // WGS-84 Flattening Factor of the Earth 
    //const double b = a - a / f_inv;
    //const double f = 1.0 / f_inv;

    const double a_sq = a * a;
    const double b_sq = b * b;
    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 = Sin(lambda);
        var N = a / Sqrt(1 - e_sq * s * s);

        var sin_lambda = Sin(lambda);
        var cos_lambda = Cos(lambda);
        var cos_phi = Cos(phi);
        var sin_phi = 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 
    // (WGS-84) Geodetic point (lat, lon, h).
    public static void EcefToGeodetic(double x, double y, double z,
                                        out double lat, out double lon, out double h)
    {
        var eps = e_sq / (1.0 - e_sq);
        var p = Sqrt(x * x + y * y);
        var q = Atan2((z * a), (p * b));
        var sin_q = Sin(q);
        var cos_q = Cos(q);
        var sin_q_3 = sin_q * sin_q * sin_q;
        var cos_q_3 = cos_q * cos_q * cos_q;
        var phi = Atan2((z + eps * b * sin_q_3), (p - e_sq * a * cos_q_3));
        var lambda = Atan2(y, x);
        var v = a / Sqrt(1.0 - e_sq * Sin(phi) * Sin(phi));
        h = (p / Cos(phi)) - v;

        lat = RadiansToDegrees(phi);
        lon = RadiansToDegrees(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 = Sin(lambda);
        var N = a / Sqrt(1 - e_sq * s * s);

        var sin_lambda = Sin(lambda);
        var cos_lambda = Cos(lambda);
        var cos_phi = Cos(phi);
        var sin_phi = 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;
    }

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

        var sin_lambda = Sin(lambda);
        var cos_lambda = Cos(lambda);
        var cos_phi = Cos(phi);
        var sin_phi = 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 = -sin_phi * xEast - cos_phi * sin_lambda * yNorth + cos_lambda * cos_phi * zUp;
        double yd = cos_phi * xEast - sin_lambda * sin_phi * yNorth + cos_lambda * sin_phi * zUp;
        double zd = cos_lambda * yNorth + sin_lambda * zUp;

        x = xd + x0;
        y = yd + y0;
        z = zd + z0;
    }

    // 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);
    }

    // South African Coordinate Reference System (Hartebeesthoek94) to Geodetic
    // From "CDNGI Coordinate Conversion Utility v1 Sep 2009.xls"
    public static void SacrsToGeodetic(int loMeridian, double yWesting, double xSouthing,
        out double lat, out double lon)
    {
        var loMeridianRadians = DegreesToRadians(loMeridian);
        const double ec_sq = (a_sq - b_sq) / a_sq;   // e^2   in "CDNGI Coordinate Conversion Utility v1 Sep 2009.xls"
        const double ep_sq = (a_sq - b_sq) / b_sq;   // e'^2
        const double n = (a - b) / (a + b);
        const double n_2 = n * n;
        const double n_3 = n_2 * n;
        const double n_4 = n_2 * n_2;
        const double n_5 = n_2 * n_3;
        const double p2 = 3.0 / 2.0 * n - 27.0 / 32.0 * n_3 + 269.0 / 512.0 * n_5;
        const double p4 = 21.0 / 16.0 * n_2 - 55.0 / 32.0 * n_4;
        const double p6 = 151.0 / 96.0 * n_3 - 417.0 / 128.0 * n_5;
        const double p8 = 1097.0 / 512.0 * n_4;
        const double p10 = 8011.0 / 2560.0 * n_5;
        const double a0 = 1.0 / (n + 1.0) * (1.0 + 1.0 / 4.0 * n_2 + 1.0 / 64.0 * n_4);
        var footBar = xSouthing / (a * a0);
        var foot = footBar + p2 * Sin(2.0 * footBar) + p4 * Sin(4.0 * footBar) + p6 * Sin(6.0 * footBar) + p8 * Sin(8.0 * footBar) + p10 * Sin(10.0 * footBar);
        var Nf = a / Sqrt(1.0 - ec_sq * Pow(Sin(foot), 2));

        var b1 = 1.0 / (Nf * Cos(foot));
        var b2 = Tan(foot) / (2.0 * Nf * Nf * Cos(foot));
        var b3 = (1.0 + 2.0 * Pow(Tan(foot), 2) + ep_sq * Pow(Cos(foot), 2)) / (6.0 * Pow(Nf, 3) * Cos(foot));
        var b4 = (Tan(foot) * (5.0 + 6.0 * Pow(Tan(foot), 2) + ep_sq * Pow(Cos(foot), 2))) / (24.0 * Pow(Nf, 4) * Cos(foot));
        var b5 = (5.0 + 28.0 * Pow(Tan(foot), 2) + 24.0 * Pow(Tan(foot), 4)) / (120.0 * Pow(Nf, 5) * Cos(foot));
        var d1 = Cos(foot) * (1.0 + ep_sq * Pow(Cos(foot), 2));
        var d2 = -1.0 / 2.0 * Pow(Cos(foot), 2) * Tan(foot) * (1.0 + 4.0 * ep_sq * Pow(Cos(foot), 2));

        var latRadians = -(foot - b2 * d1 * Pow(yWesting, 2) + (b4 * d1 + b2 * b2 * d2) * Pow(yWesting, 4));
        var lonRadians = loMeridianRadians - (b1 * yWesting - b3 * Pow(yWesting, 3) + b5 * Pow(yWesting, 5));
        lat = RadiansToDegrees(latRadians);
        lon = RadiansToDegrees(lonRadians);

#if DEBUGx
        GeodeticToSacrs(lat, lon, out int loMeridianCheck, out var yWestingCheck, out var xSouthingCheck);
        Debug.Assert(Abs(yWesting - yWestingCheck) < 1e-6);
        Debug.Assert(Abs(xSouthing - xSouthingCheck) < 1e-6);
        Debug.Assert(loMeridian == loMeridianCheck);
#endif
    }

    // South African Coordinate Reference System (Hartebeesthoek94) to Geodetic
    // From "CDNGI Coordinate Conversion Utility v1 Sep 2009.xls"
    public static void GeodeticToSacrs(double lat, double lon,
        out int loMeridian, out double yWesting, out double xSouthing)
    {
        // longitude of origin
        loMeridian = lon == 0 ? 1 : (int)Truncate(lon / 2.0) * 2 + Sign(lon);    // Take integer part of lon, rounded up to nearest odd number (or down if negative), so =ODD(TRUNC(lon))
        double loMeridianRadians = DegreesToRadians(loMeridian);
        const double ec_sq = (a_sq - b_sq) / a_sq;   // e^2   in "CDNGI Coordinate Conversion Utility v1 Sep 2009.xls"
        const double ep_sq = (a_sq - b_sq) / b_sq;   // e'^2
        const double n = (a - b) / (a + b);
        const double n_2 = n * n;
        const double n_3 = n_2 * n;
        const double n_4 = n_2 * n_2;
        const double n_5 = n_2 * n_3;
        const double A0 = 1.0 / (n + 1.0) * (1.0 + 1.0 / 4.0 * n_2 + 1.0 / 64.0 * n_4);
        const double A2 = -1.0 / (n + 1.0) * (3.0 / 2.0 * n - 3.0 / 16.0 * n_3 - 3.0 / 128.0 * n_5);
        const double A4 = 1.0 / (n + 1.0) * (15.0 / 16.0 * n_2 - 15.0 / 64.0 * n_4);
        const double A6 = -1.0 / (n + 1.0) * (35.0 / 48.0 * n_3 - 175.0 / 768.0 * n_5);
        const double A8 = 1.0 / (n + 1.0) * (315.0 / 512.0 * n_4);
        const double A10 = 1.0 / (n + 1.0) * (693.0 / 1280.0 * n_5);

        double latRadians = DegreesToRadians(-lat);
        double lonRadians = DegreesToRadians(lon);
        double G = a * (A0 * latRadians + A2 * Sin(2 * latRadians) + A4 * Sin(4 * latRadians) + A6 * Sin(6 * latRadians) + A8 * Sin(8 * latRadians) + A10 * Sin(10 * latRadians));
        double N = a / Sqrt(1.0 - ec_sq * Sin(latRadians) * Sin(latRadians));

        double latCos = Cos(latRadians);
        double latCos_2 = latCos * latCos;
        double latCos_3 = latCos_2 * latCos;
        double latCos_4 = latCos_2 * latCos_2;
        double latCos_5 = latCos_4 * latCos;
        double latTan = Tan(latRadians);
        double latTan_2 = latTan * latTan;
        double latTan_4 = latTan_2 * latTan_2;
        double a1 = N * latCos;
        double a2 = -1.0 / 2.0 * N * latCos_2 * latTan;
        double a3 = -1.0 / 6.0 * N * latCos_3 * (1.0 - latTan_2 + ep_sq * latCos_2);
        double a4 = 1.0 / 24.0 * N * latCos_4 * latTan * (5 - latTan_2 + 9.0 * ep_sq * latCos_2);
        double a5 = 1.0 / 120.0 * N * latCos_5 * (5.0 - 18.0 * latTan_2 + latTan_4);
        double l = lonRadians - loMeridianRadians;
        double l_2 = l * l;
        double l_3 = l * l_2;
        double l_4 = l_2 * l_2;
        double l_5 = l_2 * l_3;
        xSouthing = G - a2 * l_2 + a4 * l_4;
        yWesting = -(a1 * l - a3 * l_3 + a5 * l_5);
    }

    public static void GeodeticToGaussConformal(double lat, out double lon,
                                                out double yWesting, out double xSouthing, out int l0Meridian)
    {
        throw new NotImplementedException();
    }

    // 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));

    }

    public static void Test2()
    {
        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));

        EcefToEnu(x0, y0, z0, latLA, lonLA, hLA, out double xEast, out double yNorth, out double zUp);

        Debug.Assert(AreClose(0,xEast));
        Debug.Assert(AreClose(0, yNorth));
        Debug.Assert(AreClose(0, zUp));

        EnuToEcef(xEast, yNorth, zUp, latLA, lonLA, hLA, out double xTest, out double yTest, out double zTest);
        EcefToGeodetic(xTest, yTest, zTest, out double latTest, out double lonTest, out double hTest);

        Debug.Assert(AreClose(latLA, latTest));
        Debug.Assert(AreClose(lonLA, lonTest));
        Debug.Assert(AreClose(hTest, hLA));

    }

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


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

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

The code here is in C#.
I think there are many libraries with these kind of conversions, though I am not so familiar myself.
For example:

• https://github.com/dotMorten/ProjNET
• https://github.com/ethz-asl/geodetic_utils

Thank you for this code it has been a life saver - I am using it in a java program to convert ECEF to lat/long/alt and its working really well - I was asked in a code review to add some additional test cases - I used the test data you included and it works well. Its also working perfectly for the location of the eiffel tower and mt everest but I was asked to test the north and south pole coordinates and Im getting weird results for the altitude - I got the coordinates of the south pole as: -90.0, 0.0 and 2835.0m - i converted that to ECEF using https://www.oc.nps.edu/oc2902w/coord/llhxyz.htm => which gave me 0, 0, and Z : -6359.587 km. If I take that as input to the ECEF conversion code its giving me => -90.00000000000001, 0.0 and -639953.625758674 - can you help me work out what is going on with the height ?

Hi @kieranshanley - I'm not an expert in the field, but longitude is not well defined at the poles. So I can see how calculations there might go wrong. One would have to dig in a bit more to understand what is exactly is happening in the code.

For the following section of code:
// Converts the Earth-Centered Earth-Fixed (ECEF) coordinates (x, y, z) to
// (WGS-84) Geodetic point (lat, lon, h).
public static void EcefToGeodetic(double x, double y, double z,
out double lat, out double lon, out double h)
{
var eps = e_sq / (1.0 - e_sq);
var p = Sqrt(x * x + y * y);
var q = Atan2((z * a), (p * b));
var sin_q = Sin(q);
var cos_q = Cos(q);
var sin_q_3 = sin_q * sin_q * sin_q;
var cos_q_3 = cos_q * cos_q * cos_q;
var phi = Atan2((z + eps * b * sin_q_3), (p - e_sq * a * cos_q_3));
var lambda = Atan2(y, x);
var v = a / Sqrt(1.0 - e_sq * Sin(phi) * Sin(phi));
h = (p / Cos(phi)) - v;

    lat = RadiansToDegrees(phi);
    lon = RadiansToDegrees(lambda);
}

can someone please explain why h is sometimes negavite? i thought this meant height

The height is relative to the WGS-84 reference ellipsoid (a very simple shape). The earth's surface (even local sea level) does not fit the ellipsoid perfectly. So, some points may have negative height relative to the ellipsoid because the ellipsoid and earth surface shape diverge (or indeed in places where the surface is below sea level. I would not expect very large negative heights, though.
What are you seeing?