{{ message }}

Instantly share code, notes, and snippets.

LocalJoost/GpsUtils.cs

Forked from govert/GpsUtils.cs
Last active Aug 5, 2019
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 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 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