aboutsummaryrefslogtreecommitdiffstats
path: root/fg21sim/uvsim
diff options
context:
space:
mode:
Diffstat (limited to 'fg21sim/uvsim')
-rw-r--r--fg21sim/uvsim/wgs84.py97
1 files changed, 97 insertions, 0 deletions
diff --git a/fg21sim/uvsim/wgs84.py b/fg21sim/uvsim/wgs84.py
new file mode 100644
index 0000000..f80e537
--- /dev/null
+++ b/fg21sim/uvsim/wgs84.py
@@ -0,0 +1,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)