mirror of
https://github.com/Mobile-Robotics-W20-Team-9/UMICH-NCLT-SLAP.git
synced 2025-09-08 04:03:14 +00:00
adding file to change standard lat long alt to xyz coords
This commit is contained in:
24
src/dataset/dataManipulation/GPSmanip.py
Normal file
24
src/dataset/dataManipulation/GPSmanip.py
Normal file
@@ -0,0 +1,24 @@
|
||||
import sys
|
||||
import numpy as np
|
||||
|
||||
|
||||
def gpstoLocalFrame(lat, lng, alt):
|
||||
lat0 = 0.7381566413
|
||||
lng0 = -1.4610097151
|
||||
alt0 = 265.8
|
||||
|
||||
print(np.deg2rad(lng))
|
||||
dLat = np.deg2rad(lat) - lat0
|
||||
print(dLat)
|
||||
dLng = np.deg2rad(lng) - lng0
|
||||
dAlt = alt - alt0
|
||||
|
||||
r = 6400000 # approx. radius of earth (m)
|
||||
y = r * np.cos(lat0) * np.sin(dLng)
|
||||
x = r * np.sin(dLat)
|
||||
z = dAlt
|
||||
|
||||
return [x,y,z]
|
||||
#Example
|
||||
# x = gpstoLocalFrame(42.29360387311647,-83.71222615242006,272)
|
||||
# print(x)
|
Reference in New Issue
Block a user