Skip to content

Instantly share code, notes, and snippets.

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;
// 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
// Taken from https://gist.github.com/govert/1b373696c9a27ff4c72a
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 = DegreeToRadian(lat);
var phi = DegreeToRadian(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 = DegreeToRadian(lat0);
var phi = DegreeToRadian(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);
}
private static double DegreeToRadian(double angle)
{
return Math.PI * angle / 180.0;
}
}
@rfrank5356

This comment has been minimized.

Copy link

rfrank5356 commented Aug 5, 2019

Hi Joost

Sorry to trouble you – but I am having issues with GPSUtils. I downloaded the GpsUtils.cs from github

I have a vb.net program that reads ECEF IMU data from a csv file (Lat, Lon, Roll, Pitch, Heading) and I need to transform to ENU.

I downloaded the GpsUtils.cs from github

I first translated the ECEFtoENU function to vb.net (in VS2019) and the returned valued were unusable. I was concerned that I may have mis-typed a code line or missed a constant. So I created a new C# project from the GPSUtils source, created a DLL, referenced that in my vb program with the same variable values, and got another strange - and different - set of values.

Here is the sub that calls the ECEFtoECU

Sub Use_CSharp_EcefToENU()
RollValue = lineArray(INS_Roll_Position)
PitchValue = lineArray(INS_Pitch_Position)
YawValue = lineArray(INS_HDG_Position)
DoubleX = RollValue
DoubleY = PitchValue
DoubleZ = YawValue
'Get WGS84 position data from CSV file
Lambda = lineArray(PhotoLat_Position)
Phi = lineArray(PhotoLon_Position)
HeightValue = lineArray(Altitude_Position)
DoubleLat = Lambda
DoubleLon = Phi
DoubleH0 = HeightValue
GPS_Utils.GpsUtils.EcefToEnu(DoubleX, DoubleY, DoubleZ, DoubleLat, DoubleLon, DoubleH0, NewEast, NewNorth, NewUp)
KappaValue = NewNorth
OmegaValue = NewEast
PhiValue = NewUp
End Sub

Here is the results . .

OBJECTID Picture PhotoLat PhotoLon PlannedHeading GPS_Track INS_HDG True_HDG INS_Pitch INS_Roll Altitude Datum POINT_X POINT_Y oldOmega oldPhi oldKappa VB_SUb_Omega VB_SUb_Phi VB_SUb_Kappa GPSUTils_Omega GPSUTils_Phi GPSUTils_Kappa
1 RGB_43093.JPG 37.56793 -121.181 180 180.87 -178.639 181.361 -0.066 -0.539 1770.1 WGS84 660636.3 4159432 0.078784 0.066 180.361 -8754.668129 -365474933.8 79178.70129 -0.42696 -6372075 20518.36
49 RGB_43159.JPG 37.30616 -121.012 90 90.57 91.364 268.636 0 -0.702 1762.8 WGS84 676209.5 4130689 -0.702 -0.01671 269.636 4486.524168 -365481494.4 83990.22157 -0.60166 -6371997 20681.34

Any help you can give would be greatly appreciated!

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.