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

Hi, I think here is upset mistake
return (d * d) < 1e16;
have to be
return (d * d) < 1e - 16;

@vadikrobot - Indeed - thanks.
The check needs to be much more lenient - the paper has round-off to lower precision.

Hi @govert - Do you mind adding a license to this gist? I am thinking to use it. Something like this would be fantastic: https://gist.github.com/jwage/221634#file-splclassloader-php-L3-L19

Does this output ENU coordinates in meters, feet or nautical miles?

Hi @govert, I was trying to convert an ECEF coordinate to Geodetic Coordinate: X - 430849.2263, Y - 3047526.8085

The output I am getting is: X: 29.840579135433426 Y: 81.953042536541545

But the output should be somewhere around this coordinate: X: 18.02260756 Y: 83.00216488

The actual output is around 1000 km far away from the expected output. Please help me in finding what's wrong here?

@arsalanansari17 What Z are you using for the ECEF input?

@govert, these are my values of XYZ.

740236.7509 6021372.3247 1962265.8382 APPROX POSITION XYZ

I have been using this code from a while now, and it was working perfect. It gave this error for the first time and for this coordinate only!

@arsalanansari17
With the inputs you show, the result of EcefToGeodetic is:
Lat: 18.037
Lon: 82.992
H: 53.376

This seems to match your expectation.

Then GeodeticToEcef -> EcefToEnu also converts back to your inputs correctly.

Check that you are running the latest version of the code snippet - I made an important fix in the last revision in March 2018 that only affects the calculation in some quadrants of space.

Yes I took the latest code snippet and ran with the below mentioned but still I am getting this output values:

lat: 29.840579135433426 long: 81.95304253654155 alt: -2835141.330775927

My main.cs

class Program

{

  static void Main(string[] args)

  {

      double x = 430849.2263;

      double y = 3047526.8085;

      double z = 1744323.4072;

      double lat, lon, alt;

      GPSutils.EcefToGeodetic(x, y, z, out lat, out lon, out alt);

      Console.WriteLine("lat: {0}", lat);
      Console.WriteLine("long: {0}", lon);
      Console.WriteLine("alt: {0}", alt);

      Console.WriteLine("Done");
  }

}

You are showing two sets of xyz values ?
I tried with

740236.7509 6021372.3247 1962265.8382 APPROX POSITION XYZ

and I get the result you expect?

@arsalanansari17 Your x,y,z can't be right - the distance from the origin (center of the earth) is only 3537 km. This is only halfway to the surface of the earth. That's why you get large negative height (2835 km below WGS-84 reference surface).

Sorry about this rather trivial question but I am trying to use GeodeticToEnu
I have the input values i.e. lat, lon and h.
I successfully got the ECEF x, y & z coordinates with GeodeticToEcef
However for the EcefToEnu, I am confused about what corresponding input values do i use for lat0, lon0 and h0 in this case? How do I obtain them?
Finally, would appreciate if you can please write to me a set of successfully tested input (lat, lon, h, lat0, lon0, h0) and output (x, y, z, xEast, yNorth, zUp) values.
Many thanks!

The Enu coordinates are rectangular (Cartesian) coordinates relative to a reference grid that is formed by a 'local tangent plane' on the earth's surface. The lat0, lon0, h0 coordinates specify the position of this plane. You have to specify it for the conversion in both directions. If you have an area where you will be using these coordinates (maybe a construction site) then you choose a particular point, or maybe the middle, as the origin for this system.

You can get more information here: https://archive.psas.pdx.edu/CoordinateSystem/Latitude_to_LocalTangent.pdf
and the other two links:

• Wikipedia: http://en.wikipedia.org/wiki/Geodetic_datum
• the guidance notes here: http://www.epsg.org/Guidancenotes.aspx
  The Test and Test2 methods in the code have examples with numbers that should be successful when you run it.
  Have you tried?

Thank you very much for the response. That explanation helps a lot..I was able to successfully use this code as a guideline to convert LLA to ENU coordinates in Simatic Manager SCL language.

Is the language used in this Code C++ or C?
Would appreciate if you could point me to a code in the same language to convert from LL coordinates to UTM. I was not able to find any such code in pure C language.

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?