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;

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

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!