forked from sglvladi/MATLAB
-
Notifications
You must be signed in to change notification settings - Fork 0
/
ecef.py
53 lines (49 loc) · 1.89 KB
/
ecef.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
#ecef.py
#https://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22
from math import pow, degrees, radians
from scipy import mat, cos, sin, arctan, sqrt, pi, arctan2, deg2rad, rad2deg
#TO-DO: UPDATE THESE NUMBERS USING THE earth_radius.py
#
# Constants defined by the World Geodetic System 1984 (WGS84)
a = 6378.137
b = 6356.7523142
esq = 6.69437999014 * 0.001
e1sq = 6.73949674228 * 0.001
f = 1 / 298.257223563
def geodetic2ecef(lat, lon, alt, degrees=True):
"""geodetic2ecef(lat, lon, alt)
[deg][deg][m]
Convert geodetic coordinates to ECEF."""
if degrees:
lat=deg2rad(lat)
lon=deg2rad(lon)
#lat, lon = radians(lat), radians(lon)
xi = sqrt(1 - esq * sin(lat))
x = (a / xi + alt) * cos(lat) * cos(lon)
y = (a / xi + alt) * cos(lat) * sin(lon)
z = (a / xi * (1 - esq) + alt) * sin(lat)
return x, y, z
def ecef2geodetic(x, y, z, degrees=True):
"""ecef2geodetic(x, y, z)
[m][m][m]
Convert ECEF coordinates to geodetic.
J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates \
to geodetic coordinates," IEEE Transactions on Aerospace and \
Electronic Systems, vol. 30, pp. 957-961, 1994."""
r = sqrt(x * x + y * y)
Esq = a * a - b * b
F = 54 * b * b * z * z
G = r * r + (1 - esq) * z * z - esq * Esq
C = (esq * esq * F * r * r) / (pow(G, 3))
S = cbrt(1 + C + sqrt(C * C + 2 * C))
P = F / (3 * pow((S + 1 / S + 1), 2) * G * G)
Q = sqrt(1 + 2 * esq * esq * P)
r_0 = -(P * esq * r) / (1 + Q) + sqrt(0.5 * a * a*(1 + 1.0 / Q) - \
P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r)
U = sqrt(pow((r - esq * r_0), 2) + z * z)
V = sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z)
Z_0 = b * b * z / (a * V)
h = U * (1 - b * b / (a * V))
lat = arctan((z + e1sq * Z_0) / r)
lon = arctan2(y, x)
return rad2deg(lat), rad2deg(lon), z