Skip to content

Instantly share code, notes, and snippets.

@govert govert/GpsUtils.cs
Last active Dec 13, 2019

Embed
What would you like to do?
Convert WGS-84 geodetic locations (GPS readings) to Cartesian coordinates in a local tangent plane (Geodetic to ECEF to ENU)
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;
}
}
@vadikrobot

This comment has been minimized.

Copy link

vadikrobot commented Jun 11, 2017

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

@govert

This comment has been minimized.

Copy link
Owner Author

govert commented Sep 6, 2017

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

@davidrusu

This comment has been minimized.

Copy link

davidrusu commented Oct 4, 2017

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

@sbarratt

This comment has been minimized.

Copy link

sbarratt commented Oct 16, 2017

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.