1
|
|
|
from math import sqrt, pi, sin, cos, log, atan, atan2 |
2
|
|
|
|
3
|
|
|
|
4
|
|
|
def deg2rad(deg): |
5
|
|
|
"""Converts degrees to radians""" |
6
|
|
|
return deg * pi / 180 |
7
|
|
|
|
8
|
|
|
|
9
|
|
|
def rad2deg(rad): |
10
|
|
|
"""Converts radians to degrees""" |
11
|
|
|
return rad * 180 / pi |
12
|
|
|
|
13
|
|
|
|
14
|
|
|
class WGS84: |
15
|
|
|
"""General parameters defined by the WGS84 system""" |
16
|
|
|
#Semimajor axis length (m) |
17
|
|
|
a = 6378137.0 |
18
|
|
|
#Semiminor axis length (m) |
19
|
|
|
b = 6356752.3142 |
20
|
|
|
#Ellipsoid flatness (unitless) |
21
|
|
|
f = (a - b) / a |
22
|
|
|
#Eccentricity (unitless) |
23
|
|
|
e = sqrt(f * (2 - f)) |
24
|
|
|
#Speed of light (m/s) |
25
|
|
|
c = 299792458. |
26
|
|
|
#Relativistic constant |
27
|
|
|
F = -4.442807633e-10 |
28
|
|
|
#Earth's universal gravitational constant |
29
|
|
|
mu = 3.986005e14 |
30
|
|
|
#Earth rotation rate (rad/s) |
31
|
|
|
omega_ie = 7.2921151467e-5 |
32
|
|
|
|
33
|
|
|
def g0(self, L): |
34
|
|
|
"""acceleration due to gravity at the elipsoid surface at latitude L""" |
35
|
|
|
return 9.7803267715 * (1 + 0.001931851353 * sin(L)**2) / \ |
36
|
|
|
sqrt(1 - 0.0066943800229 * sin(L)**2) |
37
|
|
|
|
38
|
|
|
|
39
|
|
|
class GPS: |
40
|
|
|
"""Working class for GPS module""" |
41
|
|
|
wgs84 = WGS84() |
42
|
|
|
fGPS = 1023 |
43
|
|
|
fL1 = fGPS * 1.54e6 |
44
|
|
|
fL2 = fGPS * 1.2e6 |
45
|
|
|
|
46
|
|
|
def lla2ecef(self, lla): |
47
|
|
|
"""Convert lat, lon, alt to Earth-centered, Earth-fixed coordinates. |
48
|
|
|
Input: lla - (lat, lon, alt) in (decimal degrees, decimal degees, m) |
49
|
|
|
Output: ecef - (x, y, z) in (m, m, m) |
50
|
|
|
""" |
51
|
|
|
#Decompose the input |
52
|
|
|
lat = deg2rad(lla[0]) |
53
|
|
|
lon = deg2rad(lla[1]) |
54
|
|
|
alt = lla[2] |
55
|
|
|
#Calculate length of the normal to the ellipsoid |
56
|
|
|
N = self.wgs84.a / sqrt(1 - (self.wgs84.e * sin(lat))**2) |
57
|
|
|
#Calculate ecef coordinates |
58
|
|
|
x = (N + alt) * cos(lat) * cos(lon) |
59
|
|
|
y = (N + alt) * cos(lat) * sin(lon) |
60
|
|
|
z = (N * (1 - self.wgs84.e**2) + alt) * sin(lat) |
61
|
|
|
#Return the ecef coordinates |
62
|
|
|
return (x, y, z) |
63
|
|
|
|
64
|
|
|
def ecef2lla(self, ecef, tolerance=1e-9): |
65
|
|
|
"""Convert Earth-centered, Earth-fixed coordinates to lat, lon, alt. |
66
|
|
|
Input: ecef - (x, y, z) in (m, m, m) |
67
|
|
|
Output: lla - (lat, lon, alt) in (decimal degrees, decimal degrees, m) |
68
|
|
|
""" |
69
|
|
|
#Decompose the input |
70
|
|
|
x = ecef[0] |
71
|
|
|
y = ecef[1] |
72
|
|
|
z = ecef[2] |
73
|
|
|
#Calculate lon |
74
|
|
|
lon = atan2(y, x) |
75
|
|
|
#Initialize the variables to calculate lat and alt |
76
|
|
|
alt = 0 |
77
|
|
|
N = self.wgs84.a |
78
|
|
|
p = sqrt(x**2 + y**2) |
79
|
|
|
lat = 0 |
80
|
|
|
previousLat = 90 |
81
|
|
|
#Iterate until tolerance is reached |
82
|
|
|
while abs(lat - previousLat) >= tolerance: |
83
|
|
|
previousLat = lat |
84
|
|
|
sinLat = z / (N * (1 - self.wgs84.e**2) + alt) |
85
|
|
|
lat = atan((z + self.wgs84.e**2 * N * sinLat) / p) |
86
|
|
|
N = self.wgs84.a / sqrt(1 - (self.wgs84.e * sinLat)**2) |
87
|
|
|
alt = p / cos(lat) - N |
88
|
|
|
#Return the lla coordinates |
89
|
|
|
return (rad2deg(lat), rad2deg(lon), alt) |
90
|
|
|
|