diff --git a/src/skaero/geometry/coordinates.py b/src/skaero/geometry/coordinates.py new file mode 100644 index 0000000..ac10666 --- /dev/null +++ b/src/skaero/geometry/coordinates.py @@ -0,0 +1,363 @@ +# coding: utf-8 + +""" + Coordinate transformations used in flight dynamics +""" + +import numpy as np +from numpy import array, cos, deg2rad, rad2deg, sin + + +def lla2ecef(lat, lng, h): + """ + Calculates geocentric coordinates (ECEF - Earth Centered, Earth Fixed) + for a given set of latitude, longitude and altitude inputs, following + the WGS84 system. + + Parameters + ---------- + lat : float + latitude in degrees + lng : float + longitude in degrees + h : float + geometric altitude above sea level in meters + + Returns + ------- + array-like + ECEF coordinates in meters + """ + if abs(lat) > 90: + raise ValueError("latitude should be -90º <= latitude <= 90º") + + if abs(lng) > 180: + raise ValueError("longitude should be -180º <= longitude <= 180º") + + if not (0 <= h <= 84852.05): + msg = "pressure model is only valid if 0 <= h <= 84852.05" + raise ValueError(msg) + + a = 6378137 # [m] Earth equatorial axis + b = 6356752.3142 # [m] Earth polar axis + e = 0.081819190842622 # Earth eccentricity + + lat = deg2rad(lat) # degrees to radians + lng = deg2rad(lng) # degrees to radians + + N = a / (1 - (e * sin(lat)) ** 2) ** (0.5) + + x = (N + h) * cos(lat) * cos(lng) + y = (N + h) * cos(lat) * sin(lng) + z = (((b / a) ** 2) * N + h) * sin(lat) + + return array([x, y, z]) + + +def ned2ecef(v_ned, lat, lng): + """ + Converts vector from local geodetic horizon reference frame (NED - North, + East, Down) at a given latitude and longitude to geocentric coordinates + (ECEF - Earth Centered, Earth Fixed). + + Parameters + ---------- + v_ned: array-like + vector expressed in NED coordinates + lat : float + latitude in degrees + lng : float + longitude in degrees + + Returns + ------- + v_ecef : array-like + vector expressed in ECEF coordinates + """ + if abs(lat) > 90: + raise ValueError("latitude should be -90º <= latitude <= 90º") + + if abs(lng) > 180: + raise ValueError("longitude should be -180º <= longitude <= 180º") + + lat = deg2rad(lat) + lng = deg2rad(lng) + + Lne = array( + [ + [-sin(lat) * cos(lng), -sin(lat) * sin(lng), cos(lat)], + [-sin(lng), cos(lng), 0], + [-cos(lat) * cos(lng), -cos(lat) * sin(lng), -sin(lat)], + ] + ) + + Len = Lne.transpose() + v_ecef = Len.dot(v_ned) + + return v_ecef + + +def ecef2ned(v_ecef, lat, lng): + """ + Converts vector from geocentric coordinates (ECEF - Earth Centered, + Earth Fixed) at a given latitude and longitude to local geodetic horizon + reference frame (NED - North, East, Down). + + Parameters + ---------- + v_ecef: array-like + vector expressed in ECEF coordinates + lat : float + latitude in degrees + lng : float + longitude in degrees + + Returns + ------- + v_ned : array-like + vector expressed in NED coordinates + """ + if abs(lat) > 90: + raise ValueError("latitude should be -90º <= latitude <= 90º") + + if abs(lng) > 180: + raise ValueError("longitude should be -180º <= longitude <= 180º") + + lat = deg2rad(lat) + lng = deg2rad(lng) + + Lne = array( + [ + [-sin(lat) * cos(lng), -sin(lat) * sin(lng), cos(lat)], + [-sin(lng), cos(lng), 0], + [-cos(lat) * cos(lng), -cos(lat) * sin(lng), -sin(lat)], + ] + ) + + v_ned = Lne.dot(v_ecef) + + return v_ned + + +def body2ned(v_body, theta, phi, psi): + """ + Converts vector from body reference frame to local geodetic horizon + reference frame (NED - North, East, Down) + + Parameters + ---------- + v_body: array-like + vector expressed in body coordinates + theta : float + pitch angle in radians + phi : float + bank angle in radians + psi : float + yaw angle in radians + + Returns + ------- + v_ned : array_like + vector expressed in local horizon (NED) coordinates + """ + if abs(theta) > np.pi / 2: + raise ValueError("theta should be -pi/2 <= theta <= pi/2") + + if abs(phi) > np.pi: + raise ValueError("phi should be -pi <= phi <= pi") + + if not 0 <= psi <= 2 * np.pi: + raise ValueError("psi should be 0 <= psi <= 2*pi") + + Lnb = array( + [ + [ + cos(theta) * cos(psi), + sin(phi) * sin(theta) * cos(psi) - cos(phi) * sin(psi), + cos(phi) * sin(theta) * cos(psi) + sin(phi) * sin(psi), + ], + [ + cos(theta) * sin(psi), + sin(phi) * sin(theta) * sin(psi) + cos(phi) * cos(psi), + cos(phi) * sin(theta) * sin(psi) - sin(phi) * cos(psi), + ], + [-sin(theta), sin(phi) * cos(theta), cos(phi) * cos(theta)], + ] + ) + + v_ned = Lnb.dot(v_body) + + return v_ned + + +def ned2body(v_ned, theta, phi, psi): + """ + Converts vector from local geodetic horizon reference frame (NED - + North, East, Down) to body reference frame + + Parameters + ---------- + v_ned : array_like + vector expressed in local horizon (NED) coordinates + theta : float + pitch angle in radians + phi : float + bank angle in radians + psi : float + yaw angle in radians + + Returns + ------- + v_body: array-like + vector expressed in body coordinates + """ + if abs(theta) > np.pi / 2: + raise ValueError("theta should be -pi/2 <= theta <= pi/2") + + if abs(phi) > np.pi: + raise ValueError("phi should be -pi <= phi <= pi") + + if not 0 <= psi <= 2 * np.pi: + raise ValueError("psi should be 0 <= psi <= 2*pi") + + Lbn = array( + [ + [cos(theta) * cos(psi), cos(theta) * sin(psi), -sin(theta)], + [ + sin(phi) * sin(theta) * cos(psi) - cos(phi) * sin(psi), + sin(phi) * sin(theta) * sin(psi) + cos(phi) * cos(psi), + sin(phi) * cos(theta), + ], + [ + cos(phi) * sin(theta) * cos(psi) + sin(phi) * sin(psi), + cos(phi) * sin(theta) * sin(psi) - sin(phi) * cos(psi), + cos(phi) * cos(theta), + ], + ] + ) + + v_body = Lbn.dot(v_ned) + + return v_body + + +def body2wind(v_body, alpha, beta): + """ + Converts vector from body reference frame to wind reference frame + + Parameters + ---------- + v_body : array_like + vector expressed in body coordinates + alpha : float + angle of attack in radians + beta : float + sideslip angle in radians + + Returns + ------- + v_wind : array_like + vector expressed in wind coordinates + """ + if abs(alpha) > np.pi / 2: + raise ValueError("alpha should be -pi/2 <= alpha <= pi/2") + + if abs(beta) > np.pi: + raise ValueError("beta should be -pi <= beta <= pi") + + Lwb = array( + [ + [cos(alpha) * cos(beta), sin(beta), sin(alpha) * cos(beta)], + [-cos(alpha) * sin(beta), cos(beta), -sin(alpha) * sin(beta)], + [-sin(alpha), 0, cos(alpha)], + ] + ) + + v_wind = Lwb.dot(v_body) + + return v_wind + + +def wind2body(v_wind, alpha, beta): + """ + Converts vector from wind reference frame to body reference frame + + Parameters + ---------- + v_wind : array_like + vector expressed in wind coordinates + alpha : float + angle of attack in radians + beta : float + sideslip angle in radians + + Returns + ------- + v_body : array_like + vector expressed in body coordinates + """ + if abs(alpha) > np.pi / 2: + raise ValueError("alpha should be -pi/2 <= alpha <= pi/2") + + if abs(beta) > np.pi: + raise ValueError("beta should be -pi <= beta <= pi") + + Lbw = array( + [ + [cos(alpha) * cos(beta), -cos(alpha) * sin(beta), -sin(alpha)], + [sin(beta), cos(beta), 0], + [sin(alpha) * cos(beta), -sin(alpha) * sin(beta), cos(alpha)], + ] + ) + + 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 diff --git a/tests/test_coordinates.py b/tests/test_coordinates.py new file mode 100644 index 0000000..09efccf --- /dev/null +++ b/tests/test_coordinates.py @@ -0,0 +1,869 @@ +# coding: utf-8 + +""" + Tests ofc the coordinate transformations +""" + +from __future__ import division, absolute_import + +import numpy as np +from numpy.testing import assert_array_almost_equal +from numpy import array, deg2rad, rad2deg +import unittest as ut + +from skaero.geometry.coordinates import * + + +class Test_lla2ecef(ut.TestCase): + """ + Test function that returns ecef position from lat, long, altitude + """ + def test_latitude_wrong_input(self): + self.assertRaises(ValueError, lla2ecef, 91.0, 0, 0) + self.assertRaises(ValueError, lla2ecef, -90.001, 0, 0) + self.assertRaises(TypeError, lla2ecef, '91.0', 0, 0) + + def test_longitude_wrong_input(self): + self.assertRaises(ValueError, lla2ecef, 0, -189, 0) + self.assertRaises(ValueError, lla2ecef, 0, 181.9, 0) + self.assertRaises(TypeError, lla2ecef, 8, '0', 0) + + def test_altitude_wrong_input(self): + self.assertRaises(ValueError, lla2ecef, 0, 0, -1.0) + self.assertRaises(ValueError, lla2ecef, 0, 0, 85000.0) + self.assertRaises(TypeError, lla2ecef, 0, 0, 'e') + + def test_OX(self): + a = 6378137 # [m] Earth equatorial axis + b = 6356752.3142 # [m] Earth polar axis + + # OX-axis + lat = 0 + lng = 0 + h = 0 + expected_value = array([a, 0, 0]) + self.assertTrue(np.allclose(lla2ecef(lat, lng, h), + expected_value)) + + lat = 0 + lng = 180 + h = 0 + expected_value = array([-a, 0, 0]) + self.assertTrue(np.allclose(lla2ecef(lat, lng, h), + expected_value)) + + def test_OY(self): + a = 6378137 # [m] Earth equatorial axis + b = 6356752.3142 # [m] Earth polar axis + + # OY-axis + lat = 0 + lng = 90 + h = 0 + expected_value = array([0, a, 0]) + self.assertTrue(np.allclose(lla2ecef(lat, lng, h), + expected_value)) + + lat = 0 + lng = -90 + h = 0 + expected_value = array([0, -a, 0]) + self.assertTrue(np.allclose(lla2ecef(lat, lng, h), + expected_value)) + + def test_OZ(self): + a = 6378137 # [m] Earth equatorial axis + b = 6356752.3142 # [m] Earth polar axis + + # OZ-axis + lat = 90 + lng = 0 + h = 0 + expected_value = array([0, 0, b]) + self.assertTrue(np.allclose(lla2ecef(lat, lng, h), + expected_value)) + + lat = -90 + lng = 0 + h = 0 + expected_value = array([0, 0, -b]) + self.assertTrue(np.allclose(lla2ecef(lat, lng, h), + expected_value)) + + +class Test_ned2ecef(ut.TestCase): + """ + Test function that transforms ned-basis vectors to ecef-basis + """ + def test_latitude_wrong_input(self): + v_aux = array([1, 0, 0]) + self.assertRaises(ValueError, ned2ecef, v_aux, 91.0, 0) + self.assertRaises(ValueError, ned2ecef, v_aux, -90.001, 0) + self.assertRaises(TypeError, ned2ecef, v_aux, 't', 0) + + def test_longitude_wrong_input(self): + v_aux = array([1, 0, 0]) + self.assertRaises(ValueError, ned2ecef, v_aux, 0, -190.1) + self.assertRaises(ValueError, ned2ecef, v_aux, 0, 180.1) + self.assertRaises(TypeError, ned2ecef, v_aux, 0, '0') + + def test_1(self): + lat, lng = 0, 0 + + v_ned = array([1, 0, 0]) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(ned2ecef(v_ned, lat, lng), + expected_value)) + + v_ned = array([0, 1, 0]) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(ned2ecef(v_ned, lat, lng), + expected_value)) + + v_ned = array([0, 0, 1]) + expected_value = array([-1, 0, 0]) + self.assertTrue(np.allclose(ned2ecef(v_ned, lat, lng), + expected_value)) + + def test_2(self): + lat, lng = 0, 90 + + v_ned = array([1, 0, 0]) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(ned2ecef(v_ned, lat, lng), + expected_value)) + + v_ned = array([0, 1, 0]) + expected_value = array([-1, 0, 0]) + self.assertTrue(np.allclose(ned2ecef(v_ned, lat, lng), + expected_value)) + + v_ned = array([0, 0, 1]) + expected_value = array([0, -1, 0]) + self.assertTrue(np.allclose(ned2ecef(v_ned, lat, lng), + expected_value)) + + def test_3(self): + lat, lng = 90, 0 + + v_ned = array([1, 0, 0]) + expected_value = array([-1, 0, 0]) + self.assertTrue(np.allclose(ned2ecef(v_ned, lat, lng), + expected_value)) + + v_ned = array([0, 1, 0]) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(ned2ecef(v_ned, lat, lng), + expected_value)) + + v_ned = array([0, 0, 1]) + expected_value = array([0, 0, -1]) + self.assertTrue(np.allclose(ned2ecef(v_ned, lat, lng), + expected_value)) + + def test_ned2ecef2ned(self): + v = array([1, 0.43, -3]) + + lat, lng = 67, 31.45 + v_ecef = ned2ecef(v, lat, lng) + expected_value = v + self.assertTrue(np.allclose(ecef2ned(v_ecef, lat, lng), + expected_value)) + + +class Test_ecef2ned(ut.TestCase): + """ + Test function that transforms ecef-basis vectors to ned-basis + """ + def test_latitude_wrong_input(self): + v_aux = array([1, 0, 0]) + self.assertRaises(ValueError, ecef2ned, v_aux, 91.0, 0) + self.assertRaises(ValueError, ecef2ned, v_aux, -90.001, 0) + self.assertRaises(TypeError, ecef2ned, v_aux, 't', 0) + + def test_longitude_wrong_input(self): + v_aux = array([1, 0, 0]) + self.assertRaises(ValueError, ecef2ned, v_aux, 0, -190.1) + self.assertRaises(ValueError, ecef2ned, v_aux, 0, 180.1) + self.assertRaises(TypeError, ecef2ned, v_aux, 0, '0') + + def test_1(self): + lat, lng = 0, 0 + + v_ecef = array([1, 0, 0]) + expected_value = array([0, 0, -1]) + self.assertTrue(np.allclose(ecef2ned(v_ecef, lat, lng), + expected_value)) + + v_ecef = array([0, 1, 0]) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(ecef2ned(v_ecef, lat, lng), + expected_value)) + + v_ecef = array([0, 0, 1]) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(ecef2ned(v_ecef, lat, lng), + expected_value)) + + def test_2(self): + lat, lng = 0, 90 + + v_ecef = array([1, 0, 0]) + expected_value = array([0, -1, 0]) + self.assertTrue(np.allclose(ecef2ned(v_ecef, lat, lng), + expected_value)) + + v_ecef = array([0, 1, 0]) + expected_value = array([0, 0, -1]) + self.assertTrue(np.allclose(ecef2ned(v_ecef, lat, lng), + expected_value)) + + v_ecef = array([0, 0, 1]) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(ecef2ned(v_ecef, lat, lng), + expected_value)) + + def test_3(self): + lat, lng = 90, 0 + + v_ecef = array([1, 0, 0]) + expected_value = array([-1, 0, 0]) + self.assertTrue(np.allclose(ecef2ned(v_ecef, lat, lng), + expected_value)) + + v_ecef = array([0, 1, 0]) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(ecef2ned(v_ecef, lat, lng), + expected_value)) + + v_ecef = array([0, 0, 1]) + expected_value = array([0, 0, -1]) + self.assertTrue(np.allclose(ecef2ned(v_ecef, lat, lng), + expected_value)) + + def test_ecef2ned2ecef(self): + v = array([-56, 30.43, -81]) + + lat, lng = -39, 178.45 + v_ned = ecef2ned(v, lat, lng) + expected_value = v + self.assertTrue(np.allclose(ned2ecef(v_ned, lat, lng), + expected_value)) + + +class Test_body2ned(ut.TestCase): + """ + Test function that transforms body-basis vectors to ned-basis + """ + def test_wrong_theta_input(self): + v_aux = array([1, 0, 0]) + + theta, phi, psi = deg2rad(90.1), deg2rad(0), deg2rad(0) + self.assertRaises(ValueError, body2ned, v_aux, theta, phi, psi) + + theta, phi, psi = deg2rad(-90.01), deg2rad(0), deg2rad(0) + self.assertRaises(ValueError, body2ned, v_aux, theta, phi, psi) + + theta, phi, psi = 'a', deg2rad(0), deg2rad(0) + self.assertRaises(TypeError, body2ned, v_aux, theta, phi, psi) + + def test_wrong_phi_input(self): + v_aux = array([1, 0, 0]) + + theta, phi, psi = deg2rad(0), deg2rad(180.1), deg2rad(0) + self.assertRaises(ValueError, body2ned, v_aux, theta, phi, psi) + + theta, phi, psi = deg2rad(0), deg2rad(-181), deg2rad(0) + self.assertRaises(ValueError, body2ned, v_aux, theta, phi, psi) + + theta, phi, psi = deg2rad(0), 'a', deg2rad(0) + self.assertRaises(TypeError, body2ned, v_aux, theta, phi, psi) + + def test_wrong_psi_input(self): + v_aux = array([1, 0, 0]) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(-1) + self.assertRaises(ValueError, body2ned, v_aux, theta, phi, psi) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(361) + self.assertRaises(ValueError, body2ned, v_aux, theta, phi, psi) + + theta, phi, psi = deg2rad(0), deg2rad(0), 'a' + self.assertRaises(TypeError, body2ned, v_aux, theta, phi, psi) + + def test_OXb(self): + v_body = array([1, 0, 0]) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(0) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(90), deg2rad(0), deg2rad(0) + expected_value = array([0, 0, -1]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(-90), deg2rad(0), deg2rad(0) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(90), deg2rad(0) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(-90), deg2rad(0) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(90) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(270) + expected_value = array([0, -1, 0]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + def test_OYb(self): + v_body = array([0, 1, 0]) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(0) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(90), deg2rad(0), deg2rad(0) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(-90), deg2rad(0), deg2rad(0) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(90), deg2rad(0) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(-90), deg2rad(0) + expected_value = array([0, 0, -1]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(90) + expected_value = array([-1, 0, 0]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(270) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + def test_OZb(self): + v_body = array([0, 0, 1]) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(0) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(90), deg2rad(0), deg2rad(0) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(-90), deg2rad(0), deg2rad(0) + expected_value = array([-1, 0, 0]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(90), deg2rad(0) + expected_value = array([0, -1, 0]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(-90), deg2rad(0) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(90) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(270) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + def test_body2ned2body(self): + v = array([7, 12, -31]) + + theta, phi, psi = deg2rad(54), deg2rad(-12), deg2rad(76) + v_ned = body2ned(v, theta, phi, psi) + expected_value = v + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + +class Test_ned2body(ut.TestCase): + """ + Test function that transforms ned-basis vectors to body-basis + """ + def test_wrong_theta_input(self): + v_aux = array([1, 0, 0]) + + theta, phi, psi = deg2rad(90.1), deg2rad(0), deg2rad(0) + self.assertRaises(ValueError, ned2body, v_aux, theta, phi, psi) + + theta, phi, psi = deg2rad(-90.01), deg2rad(0), deg2rad(0) + self.assertRaises(ValueError, ned2body, v_aux, theta, phi, psi) + + theta, phi, psi = 'a', deg2rad(0), deg2rad(0) + self.assertRaises(TypeError, ned2body, v_aux, theta, phi, psi) + + def test_wrong_phi_input(self): + v_aux = array([1, 0, 0]) + + theta, phi, psi = deg2rad(0), deg2rad(180.1), deg2rad(0) + self.assertRaises(ValueError, ned2body, v_aux, theta, phi, psi) + + theta, phi, psi = deg2rad(0), deg2rad(-181), deg2rad(0) + self.assertRaises(ValueError, ned2body, v_aux, theta, phi, psi) + + theta, phi, psi = deg2rad(0), 'a', deg2rad(0) + self.assertRaises(TypeError, ned2body, v_aux, theta, phi, psi) + + def test_wrong_psi_input(self): + v_aux = array([1, 0, 0]) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(-1) + self.assertRaises(ValueError, ned2body, v_aux, theta, phi, psi) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(361) + self.assertRaises(ValueError, ned2body, v_aux, theta, phi, psi) + + theta, phi, psi = deg2rad(0), deg2rad(0), 'a' + self.assertRaises(TypeError, ned2body, v_aux, theta, phi, psi) + + def test_OXn(self): + v_ned = array([1, 0, 0]) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(0) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(90), deg2rad(0), deg2rad(0) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(-90), deg2rad(0), deg2rad(0) + expected_value = array([0, 0, -1]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(90), deg2rad(0) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(-90), deg2rad(0) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(90) + expected_value = array([0, -1, 0]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(270) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + def test_OYn(self): + v_ned = array([0, 1, 0]) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(0) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(90), deg2rad(0), deg2rad(0) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(-90), deg2rad(0), deg2rad(0) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(90), deg2rad(0) + expected_value = array([0, 0, -1]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(-90), deg2rad(0) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(90) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(90), deg2rad(270) + expected_value = array([-1, 0, 0]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + def test_OZn(self): + v_ned = array([0, 0, 1]) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(0) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(90), deg2rad(0), deg2rad(0) + expected_value = array([-1, 0, 0]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(-90), deg2rad(0), deg2rad(0) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(90), deg2rad(0) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(-90), deg2rad(0) + expected_value = array([0, -1, 0]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(90) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + theta, phi, psi = deg2rad(0), deg2rad(0), deg2rad(270) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(ned2body(v_ned, theta, phi, psi), + expected_value)) + + def test_ned2body2ned(self): + v = array([7, -2, 1]) + + theta, phi, psi = deg2rad(5), deg2rad(21), deg2rad(6) + v_body = ned2body(v, theta, phi, psi) + expected_value = v + self.assertTrue(np.allclose(body2ned(v_body, theta, phi, psi), + expected_value)) + + +class Test_body2wind(ut.TestCase): + """ + Test function that transforms body-basis vectors to wind-basis + """ + def test_wrong_alpha_input(self): + v_aux = array([1, 0, 0]) + + alpha, beta = deg2rad(90.1), deg2rad(0) + self.assertRaises(ValueError, body2wind, v_aux, alpha, beta) + + alpha, beta = deg2rad(-90.01), deg2rad(0) + self.assertRaises(ValueError, body2wind, v_aux, alpha, beta) + + alpha, beta = 'a', deg2rad(0) + self.assertRaises(TypeError, body2wind, v_aux, alpha, beta) + + def test_wrong_beta_input(self): + v_aux = array([1, 0, 0]) + + alpha, beta = deg2rad(0), deg2rad(180.1) + self.assertRaises(ValueError, body2wind, v_aux, alpha, beta) + + alpha, beta = deg2rad(0), deg2rad(-181) + self.assertRaises(ValueError, body2wind, v_aux, alpha, beta) + + alpha, beta = deg2rad(0), 'a' + self.assertRaises(TypeError, body2wind, v_aux, alpha, beta) + + def test_OXb(self): + v_body = array([1, 0, 0]) + + alpha, beta = deg2rad(0), deg2rad(0) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(body2wind(v_body, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(90), deg2rad(0) + expected_value = array([0, 0, -1]) + self.assertTrue(np.allclose(body2wind(v_body, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(-90), deg2rad(0) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(body2wind(v_body, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(0), deg2rad(90) + expected_value = array([0, -1, 0]) + self.assertTrue(np.allclose(body2wind(v_body, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(0), deg2rad(-90) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(body2wind(v_body, alpha, beta), + expected_value)) + + def test_OYb(self): + v_body = array([0, 1, 0]) + + alpha, beta = deg2rad(0), deg2rad(0) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(body2wind(v_body, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(90), deg2rad(0) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(body2wind(v_body, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(-90), deg2rad(0) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(body2wind(v_body, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(0), deg2rad(90) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(body2wind(v_body, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(0), deg2rad(-90) + expected_value = array([-1, 0, 0]) + self.assertTrue(np.allclose(body2wind(v_body, alpha, beta), + expected_value)) + + def test_OZb(self): + v_body = array([0, 0, 1]) + + alpha, beta = deg2rad(0), deg2rad(0) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(body2wind(v_body, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(90), deg2rad(0) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(body2wind(v_body, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(-90), deg2rad(0) + expected_value = array([-1, 0, 0]) + self.assertTrue(np.allclose(body2wind(v_body, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(0), deg2rad(90) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(body2wind(v_body, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(0), deg2rad(-90) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(body2wind(v_body, alpha, beta), + expected_value)) + + def test_body2wind2body(self): + v = array([10, -8, -1]) + + alpha, beta = deg2rad(10), deg2rad(-7) + v_wind = body2wind(v, alpha, beta) + expected_value = v + self.assertTrue(np.allclose(wind2body(v_wind, alpha, beta), + expected_value)) + + +class Test_wind2body(ut.TestCase): + """ + Test function that transforms wind-basis vectors to body-basis + """ + def test_wrong_alpha_input(self): + v_aux = array([1, 0, 0]) + + alpha, beta = deg2rad(90.1), deg2rad(0) + self.assertRaises(ValueError, wind2body, v_aux, alpha, beta) + + alpha, beta = deg2rad(-90.01), deg2rad(0) + self.assertRaises(ValueError, wind2body, v_aux, alpha, beta) + + alpha, beta = 'a', deg2rad(0) + self.assertRaises(TypeError, wind2body, v_aux, alpha, beta) + + def test_wrong_beta_input(self): + v_aux = array([1, 0, 0]) + + alpha, beta = deg2rad(0), deg2rad(180.1) + self.assertRaises(ValueError, wind2body, v_aux, alpha, beta) + + alpha, beta = deg2rad(0), deg2rad(-181) + self.assertRaises(ValueError, wind2body, v_aux, alpha, beta) + + alpha, beta = deg2rad(0), 'a' + self.assertRaises(TypeError, wind2body, v_aux, alpha, beta) + + def test_OXw(self): + v_wind = array([1, 0, 0]) + + alpha, beta = deg2rad(0), deg2rad(0) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(wind2body(v_wind, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(90), deg2rad(0) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(wind2body(v_wind, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(-90), deg2rad(0) + expected_value = array([0, 0, -1]) + self.assertTrue(np.allclose(wind2body(v_wind, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(0), deg2rad(90) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(wind2body(v_wind, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(0), deg2rad(-90) + expected_value = array([0, -1, 0]) + self.assertTrue(np.allclose(wind2body(v_wind, alpha, beta), + expected_value)) + + def test_OYw(self): + v_wind = array([0, 1, 0]) + + alpha, beta = deg2rad(0), deg2rad(0) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(wind2body(v_wind, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(90), deg2rad(0) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(wind2body(v_wind, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(-90), deg2rad(0) + expected_value = array([0, 1, 0]) + self.assertTrue(np.allclose(wind2body(v_wind, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(0), deg2rad(90) + expected_value = array([-1, 0, 0]) + self.assertTrue(np.allclose(wind2body(v_wind, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(0), deg2rad(-90) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(wind2body(v_wind, alpha, beta), + expected_value)) + + def test_OZw(self): + v_wind = array([0, 0, 1]) + + alpha, beta = deg2rad(0), deg2rad(0) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(wind2body(v_wind, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(90), deg2rad(0) + expected_value = array([-1, 0, 0]) + self.assertTrue(np.allclose(wind2body(v_wind, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(-90), deg2rad(0) + expected_value = array([1, 0, 0]) + self.assertTrue(np.allclose(wind2body(v_wind, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(0), deg2rad(90) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(wind2body(v_wind, alpha, beta), + expected_value)) + + alpha, beta = deg2rad(0), deg2rad(-90) + expected_value = array([0, 0, 1]) + self.assertTrue(np.allclose(wind2body(v_wind, alpha, beta), + expected_value)) + + def test_wind2body2wind(self): + v = array([0, -2, -16]) + + alpha, beta = deg2rad(19), deg2rad(37) + v_body = wind2body(v, alpha, beta) + 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))