Instantly share code, notes, and snippets.

SkybuckFlying/XYZ_2_latlong.py

Forked from mpkuse/XYZ_2_latlong.py
Created August 6, 2022 10:53
Star You must be signed in to star a gist
Convert GPS (Latitude and Longitude) to XYZ
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters. Learn more about bidirectional Unicode characters
 """ 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 )
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters. Learn more about bidirectional Unicode characters