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
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
|
# Copyright (c) 2017 Weitian LI <weitian@aaronly.me>
# MIT license
"""
WGS84 Earth geodetic coordinate conversion utilities.
NOTE
----
The WGS84 coordinates (φ, λ, h) are *ellipsoidal*, not spheroidal
(geocentric).
References
----------
[1] Converting GPS Coordinates (φλh) to Navigation Coordinates (ENU)
http://digext6.defence.gov.au/dspace/bitstream/1947/3538/1/DSTO-TN-0432.pdf
[2] Convert WGS-84 geodetic locations to Cartesian coordinates
in a local tangent plane
https://gist.github.com/govert/1b373696c9a27ff4c72a
"""
import numpy as np
class Earth:
# WGS84 Earth semi-major & semi-minor axis [m]
a = 6378137.0
b = 6356752.3142
# Ellipsoid flatness
f = (a-b) / a
# Eccentricity
e2 = 1.0 - (b/a)**2
e = e2 ** 0.5
def geodetic2ecef(p):
"""
Convert the WGS84 geodetic coordinate to the ECEF (Earth Centered
Earth Fixed) Cartesian coordinate.
Parameters
----------
p : (lon, lat, h)-tuple
The WGS84 geodetic point to be converted to ENU coordinate,
units: lon, lat -> [deg]; h -> [m]
Returns
-------
ecef : (x, y, z)-tuple
The converted ECEF coordinate. unit: [m]
"""
lon, lat, h = p
phi, lam = np.deg2rad([lon, lat])
sin_phi, sin_lam = np.sin([phi, lam])
cos_phi, cos_lam = np.cos([phi, lam])
chi = np.sqrt(1.0 - Earth.e2 * sin_lam * sin_lam)
v = Earth.a / chi
x = (v + h) * cos_lam * cos_phi
y = (v + h) * cos_lam * sin_phi
z = (v*(1-Earth.e2) + h) * sin_lam
return (x, y, z)
def geodetic2enu(p, ref):
"""
Convert the WGS84 geodetic coordinate (longitude, latitude, height)
to East-North-Up coordinates in a local tangent plane that is
centered at the reference WGS84 geodetic point.
Parameters
----------
p : (lon, lat, h)-tuple
The WGS84 geodetic point to be converted to ENU coordinate,
units: lon, lat -> [deg]; h -> [m]
ref : (lon0, lat0, h0)-tuple
The reference WGS84 geodetic point to determine the local
tangent plane.
Returns
-------
enu : (east, north, up)-tuple
The converted ENU coordinate in the determined local tangent
plane. unit: [m]
"""
pxyz = np.array(geodetic2ecef(p))
pxyz0 = np.array(geodetic2ecef(ref))
dxyz = pxyz - pxyz0
lon0, lat0, h0 = ref
phi0, lam0 = np.deg2rad([lon0, lat0])
sin_phi0, sin_lam0 = np.sin([phi0, lam0])
cos_phi0, cos_lam0 = np.cos([phi0, lam0])
# Rotation
M = np.array([[-sin_phi0, cos_phi0, 0.0],
[-cos_phi0*sin_lam0, -sin_phi0*sin_lam0, cos_lam0],
[cos_phi0*cos_lam0, sin_phi0*cos_lam0, sin_lam0]])
east, north, up = M.dot(dxyz)
return (east, north, up)
|
