You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
65 lines
1.8 KiB
Python
65 lines
1.8 KiB
Python
from math import *
|
|
import numpy as np
|
|
import pymap3d.ecef
|
|
|
|
EARTH_ECC = 0.01671022
|
|
EARTH_R = 6385607.4105288265 # equator km
|
|
|
|
|
|
def LLA_to_ECEF(lat, lon, h):
|
|
deg2rad = pi / 180
|
|
lat = lat * deg2rad
|
|
lon = lon * deg2rad
|
|
print((EARTH_R + h), cos(lat) ,cos(lon), (EARTH_R + h) * cos(lat) * cos(lon))
|
|
X = (EARTH_R + h) * cos(lat) * cos(lon)
|
|
Y = (EARTH_R + h) * cos(lat) * sin(lon)
|
|
Z = (EARTH_R * (1 - EARTH_ECC**2) + h) * sin(lat)
|
|
print(X, Y, Z)
|
|
return (X, Y, Z)
|
|
|
|
|
|
def local_to_enu(aer):
|
|
A, E, R = aer
|
|
deg2rad = pi / 180
|
|
A = A * deg2rad
|
|
E = E * deg2rad
|
|
e = R * cos(E) * sin(A)
|
|
n = R * cos(E) * cos(A)
|
|
u = R * sin(E)
|
|
return (e, n, u)
|
|
|
|
|
|
def enu_to_ECEF(p0_LLA, enu):
|
|
lat, lon, h = p0_LLA
|
|
e, n, u = enu
|
|
deg2rad = pi / 180
|
|
lat = lat * deg2rad
|
|
lon = lon * deg2rad
|
|
mat = [[-sin(lon), -sin(lat) * cos(lon), cos(lat) * cos(lon)],
|
|
[cos(lon), -sin(lat) * sin(lon), cos(lat) * sin(lon)],
|
|
[0, cos(lat), sin(lat)]]
|
|
mat = np.array(mat)
|
|
# print(mat, enu)
|
|
delta = mat @ np.array([e, n, u]).reshape(-1, 1)
|
|
p0_xyz = np.array([*LLA_to_ECEF(*p0_LLA)]).reshape(-1, 1)
|
|
# print("--------", delta, p0_xyz)
|
|
return p0_xyz + delta
|
|
|
|
|
|
def observe_to_ECEF(station, aer):
|
|
enu = local_to_enu(aer)
|
|
r = enu_to_ECEF(station, enu)
|
|
return r
|
|
|
|
|
|
if __name__ == "__main__":
|
|
# lat, lon, h
|
|
# obs = (13.0344804066577, 77.5111003779841, 0.839009756775573)
|
|
# # aer = (132.67, 32.44, 16945.450)
|
|
# aer = (123.08, 50.06, 37350.340)
|
|
# # obs = (0, 0, 1000)
|
|
# # aer = (0, 90, 1000)
|
|
# ans = observe_to_ECEF(obs, aer)
|
|
# print(ans)
|
|
LLA_to_ECEF(36.2295894, 93.0661482, 746.049)
|
|
print(pymap3d.ecef.geodetic2ecef(36.2295894, 93.0661482, 746.049)) |