added "az_elev_dist" function to coordinates and test

Author: aqreed

src/skaero/geometry/coordinates.py

@@ -5,7 +5,7 @@
 """
 
 import numpy as np
-from numpy import sin, cos, deg2rad, array
+from numpy import sin, cos, deg2rad, rad2deg, array
 
 
 def lla2ecef(lat, lng, h):
@@ -289,3 +289,50 @@ def wind2body(v_wind, alpha, beta):
     v_body = Lbw.dot(v_wind)
 
     return v_body
+
+
+def az_elev_dist(lla, lla_ref):
+    """
+    Returns distance, azimuth angle and elevation angle that define the
+    position in the sky of a point (defined using lla coordinates) as viewed
+    from a point of reference (also defined by lla coordinates)
+    Parameters
+    ----------
+    lla : array-like
+        contains latitude and longitude (in degrees) and geometric altitude
+        above sea level in meters
+    lla_ref : array-like
+        contains reference point latitude and longitude (in degrees) and
+        geometric altitude above sea level in meters
+    Returns
+    -------
+    out : tuple-like
+        distance aircraft to reference point in m
+        azimuth angle (from the reference point) in degrees
+        elevation angle (from the reference point) in degrees
+    """
+    lat, lng, h = lla
+    lat_ref, lng_ref, h_ref = lla_ref
+
+    if abs(lat) > 90 or abs(lat_ref) > 90:
+        raise ValueError('latitude should be -90º <= latitude <= 90º')
+
+    if abs(lng) > 180 or abs(lng_ref) > 180:
+        raise ValueError('longitude should be -180º <= longitude <= 180º')
+
+    v = lla2ecef(lat, lng, h) - lla2ecef(lat_ref, lng_ref, h_ref)
+
+    v_unit_ecef = v / np.linalg.norm(v)
+    v_unit_ned = ecef2ned(v_unit_ecef, lat_ref, lng_ref)
+
+    azimuth = np.arctan2(-v_unit_ned[1], v_unit_ned[0])
+
+    if v_unit_ned[0] == v_unit_ned[1] == 0:
+        elevation = np.pi / 2
+    else:
+        elevation = np.arctan(-v_unit_ned[2] / np.sqrt(v_unit_ned[0]**2 +
+                                                       v_unit_ned[1]**2))
+
+    distance = np.linalg.norm(v)
+
+    return rad2deg(azimuth), rad2deg(elevation), distance

tests/test_coordinates.py

@@ -821,3 +821,49 @@ def test_wind2body2wind(self):
         expected_value = v
         self.assertTrue(np.allclose(body2wind(v_body, alpha, beta),
                                     expected_value))
+
+
+class Test_az_elev_dist(ut.TestCase):
+    """
+    Test function that calculates distance, azimuth and elevation of a point
+    as seen from a reference point
+    """
+    def test_latitude_wrong_input(self):
+        lla = array([91, 0, 0])
+        lla_ref = array([0, 0, 0])
+        self.assertRaises(ValueError, az_elev_dist, lla, lla_ref)
+
+        lla = array([0, 0, 0])
+        lla_ref = array([-210, 0, 0])
+        self.assertRaises(ValueError, az_elev_dist, lla, lla_ref)
+
+        lla = array(['a', 0, 0])
+        lla_ref = array([0, 0, 0])
+        self.assertRaises(TypeError, az_elev_dist, lla, lla_ref)
+
+    def test_longitude_wrong_input(self):
+        lla = array([0, -181, 0])
+        lla_ref = array([0, 0, 0])
+        self.assertRaises(ValueError, az_elev_dist, lla, lla_ref)
+
+        lla = array([0, 0, 0])
+        lla_ref = array([0, 189, 0])
+        self.assertRaises(ValueError, az_elev_dist, lla, lla_ref)
+
+        lla = array([0, 0, 0])
+        lla_ref = array([0, 'a', 0])
+        self.assertRaises(TypeError, az_elev_dist, lla, lla_ref)
+
+    def test_1(self):
+        lla = array([0, 0, 0])
+        lla_ref = array([0, 0.00001, 0])
+        expected_value = (90, 0, 1.113)
+        self.assertTrue(np.allclose(az_elev_dist(lla, lla_ref),
+                                    expected_value, atol=1e-3))
+
+    def test_2(self):
+        lla = array([0, 0, 0])
+        lla_ref = array([0, 0, 10])
+        expected_value = (0, 90, 10)
+        self.assertTrue(np.allclose(az_elev_dist(lla, lla_ref),
+                                    expected_value))