Convert GPS (Latitude and Longitude) to XYZ

lat_long_2_XYZ.py

""" Convert GPS (Latitude and Longitude) to XYZ wrt a set Lat Long as origin """


def geodedic_to_ecef( lati, longi, alti ):
    """ lati in degrees, longi in degrees. alti in meters (mean sea level) """
    # Adopted from https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates
    phi = lati / 180. * np.pi
    lambada = longi / 180. * np.pi
    h = alti

    #N = 6371000 #in meters
    e = 0.081819191 #earth ecentricity
    q = np.sin( phi )
    N = 6378137.0 / np.sqrt( 1 - e*e * q*q )
    X = (N + h) * np.cos( phi ) * np.cos( lambada )
    Y = (N + h) * np.cos( phi ) * np.sin( lambada )
    Z = (N*(1-e*e) + h) * np.sin( phi )

    return X,Y,Z

def compute_ecef_to_enu_transform( lati_r, longi_r ):
    """ Computes a matrix_3x3 which transforms a ecef diff-point to ENU (East-North-Up)
        Needs as input the latitude and longitude (in degrees) of the reference point
    """
    # Adopted from https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ECEF_to_ENU

    phi = lati_r / 180. * np.pi
    lambada = longi_r / 180. * np.pi

    cp = np.cos( phi ) #cos(phi)
    sp = np.sin( phi ) #cos(phi)
    cl = np.cos( lambada )
    sl = np.sin( lambada )

    T = np.zeros( (3,3), dtype='float64' )
    T[0,0] = -sl
    T[0,1] = cl
    T[0,2] = 0

    T[1,0] = -sp * cl
    T[1,1] = -sp * sl
    T[1,2] = cp

    T[2,0] = cp * cl
    T[2,1] = cp * sl
    T[2,2] = sp

    T_enu_ecef = T
    return T_enu_ecef

  
# GPS (geodedic to Earth-center cords, ie. ecef )
Xr, Yr, Zr = geodedic_to_ecef( 22.3349060891, 114.263170818, 173.073608398 ) #of radar
T_enu_ecef = compute_ecef_to_enu_transform( 22.3349060891, 114.263170818 )

Xp, Yp, Zp = geodedic_to_ecef( data.latitude, data.longitude, data.altitude ) #curr pos of drone

#
# ECEF to ENU (East-North-Up)
delta = np.array( [Xp-Xr, Yp-Yr, Zp-Zr] )
p = np.dot( T_enu_ecef, delta )

XYZ_2_latlong.py

""" Give a) points in localcoordinate system b) a gps lat long of the origin of the local coordinate system,
this script helps to convert XYZ to latlong. 

Beware that the transformation will be off by an yaw angle. This is because the local cordinate frame is may/or may not align with the East-North-Up frame. 

The way it works is XYZ --> ECEF --> geodedic (latlong)


main reference is still the wiki  https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ECEF_to_ENU. 
However the procedure to convert from ECEF to geodedic in wikip does not give accurate results. Instead the algorithm 
from https://hal.archives-ouvertes.fr/hal-01704943v2/document gives a better result. 
"""


#### Mandatory inputs
# 46.7183003,6.9636191,741.498
radar_lat = 46.7183003
radar_lon = 6.9636191
radar_msl = 741.498


def ecef_to_geodedic_2( ecef_X ):
    """ ECEF --> lat (PHI), long (LAMBDA)

    algorithm2 https://hal.archives-ouvertes.fr/hal-01704943v2/document
    """

    a = 6378137 #(earth's semimarjor axis in meters)
    e = 0.081819191 #earth ecentricity
    b = a * np.sqrt( 1.0 - e*e )
    _X = ecef_X[0]
    _Y = ecef_X[1]
    _Z = ecef_X[2]

    w_2 = _X**2 + _Y**2
    l = e**2 / 2.0
    m = w_2 / a**2
    n = _Z**2 * (1.0 - e*e) / (a*a)
    p = (m+n - 4*l*l)/6.0
    G = m*n*l*l
    H = 2*p**3 + G

    C = np.cbrt( H+G+2*np.sqrt(H*G) ) / np.cbrt(2)
    i = -(2.*l*l + m + n ) / 2.0
    P = p*p
    beta = i/3.0 - C -P/C
    k = l*l * ( l*l - m - n )
    t = np.sqrt( np.sqrt( beta**2 - k ) - (beta+i)/2.0 ) - np.sign( m-n ) * np.sqrt( np.abs(beta-i) / 2.0 )
    F = t**4 + 2*i*t*t + 2.*l*(m-n)*t + k
    dF_dt = 4*t**3 + 4*i*t + 2*l*(m-n)
    delta_t = -F / dF_dt
    u = t + delta_t + l
    v = t + delta_t - l
    w = np.sqrt( w_2 )
    __phi = np.arctan2( _Z*u, w*v )
    delta_w = w* (1.0-1.0/u )
    delta_z = _Z*( 1- (1-e*e) / v )
    h = np.sign( u-1.0 ) * np.sqrt( delta_w**2 + delta_z**2 )
    __lambda = np.arctan2( _Y, _X )


    return (__phi, __lambda)


# Main:
# ENU --> ECEF
T_enu_ecef = np.linalg.inv( compute_ecef_to_enu_transform( radar_lat, radar_lon ) )
ecef_radar = geodedic_to_ecef( radar_lat, radar_lon, radar_msl )

enu_X = np.array( [  data.pose.position.x, data.pose.position.y, data.pose.position.z ] )
# print( "enu_X: " , enu_X )
ecef_X = np.dot( T_enu_ecef, enu_X ) + ecef_radar
# print( "ecef_X: ", ecef_X )


# ECEF --> lat (PHI), long (LAMBDA)
(__phi, __lambda) = ecef_to_geodedic_2( ecef_X )

lambda_long_indegs = __lambda /np.pi * 180.
phi_lat_indegs = __phi / np.pi * 180.