From f2bd655b80aef43ae612870aa2638ef6a8c07964 Mon Sep 17 00:00:00 2001 From: aqreed Date: Tue, 27 Aug 2019 01:26:23 +0200 Subject: [PATCH 1/8] added coordinates module, with tests. Updated setup to include it --- skaero/geometry/coordinates.py | 253 ++++++++++++ tests/test_coordinates.py | 698 +++++++++++++++++++++++++++++++++ 2 files changed, 951 insertions(+) create mode 100644 skaero/geometry/coordinates.py create mode 100644 tests/test_coordinates.py diff --git a/skaero/geometry/coordinates.py b/skaero/geometry/coordinates.py new file mode 100644 index 0000000..9e8096d --- /dev/null +++ b/skaero/geometry/coordinates.py @@ -0,0 +1,253 @@ +# coding: utf-8 + +""" + Coordinate transformations used in flight dynamics +""" + +import numpy as np +from numpy import sin, cos, deg2rad, array + + +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)**(.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 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 diff --git a/tests/test_coordinates.py b/tests/test_coordinates.py new file mode 100644 index 0000000..7250fcd --- /dev/null +++ b/tests/test_coordinates.py @@ -0,0 +1,698 @@ +# 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)) + + +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)) + + +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)) + + +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)) + + +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)) From 71501da223ce210b30c37439ea1f4bdef5da7fce Mon Sep 17 00:00:00 2001 From: aqreed Date: Sun, 1 Sep 2019 02:51:50 +0200 Subject: [PATCH 2/8] added cross checks for functions "ned2body", "body2ned", "wind2body" and "body2wind" --- tests/test_coordinates.py | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/tests/test_coordinates.py b/tests/test_coordinates.py index 7250fcd..4e53abe 100644 --- a/tests/test_coordinates.py +++ b/tests/test_coordinates.py @@ -316,6 +316,15 @@ def test_OZb(self): 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): """ @@ -471,6 +480,15 @@ def test_OZn(self): 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): """ @@ -584,6 +602,15 @@ def test_OZb(self): 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): """ @@ -696,3 +723,12 @@ def test_OZw(self): 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)) From db1a51cc0df2e59520baeb72e0681fcf7d7329e9 Mon Sep 17 00:00:00 2001 From: aqreed Date: Sun, 1 Sep 2019 03:08:02 +0200 Subject: [PATCH 3/8] added "ecef2ned" function and tests --- skaero/geometry/coordinates.py | 38 ++++++++++++++++++ tests/test_coordinates.py | 71 ++++++++++++++++++++++++++++++++++ 2 files changed, 109 insertions(+) diff --git a/skaero/geometry/coordinates.py b/skaero/geometry/coordinates.py index 9e8096d..2fae341 100644 --- a/skaero/geometry/coordinates.py +++ b/skaero/geometry/coordinates.py @@ -93,6 +93,44 @@ def ned2ecef(v_ned, lat, lng): 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 diff --git a/tests/test_coordinates.py b/tests/test_coordinates.py index 4e53abe..a268b41 100644 --- a/tests/test_coordinates.py +++ b/tests/test_coordinates.py @@ -162,6 +162,77 @@ def test_3(self): 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)) + + class Test_body2ned(ut.TestCase): """ Test function that transforms body-basis vectors to ned-basis From 114780ca2d85ce08c2a72739b38ad537cacfe5b7 Mon Sep 17 00:00:00 2001 From: aqreed Date: Sun, 1 Sep 2019 03:15:13 +0200 Subject: [PATCH 4/8] added cross checks for functions "ecef2ned" and "ned2ecef" --- tests/test_coordinates.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/tests/test_coordinates.py b/tests/test_coordinates.py index a268b41..7c99ec0 100644 --- a/tests/test_coordinates.py +++ b/tests/test_coordinates.py @@ -161,6 +161,15 @@ def test_3(self): 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): """ @@ -232,6 +241,15 @@ def test_3(self): 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): """ From 7187e150f8da76a5ac88b4651fd2d128bf56d60e Mon Sep 17 00:00:00 2001 From: aqreed Date: Thu, 12 Mar 2020 02:34:05 +0100 Subject: [PATCH 5/8] moved "coordinates" module to new folder structure. Removed setup.py file --- {skaero => src/skaero}/geometry/coordinates.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename {skaero => src/skaero}/geometry/coordinates.py (100%) diff --git a/skaero/geometry/coordinates.py b/src/skaero/geometry/coordinates.py similarity index 100% rename from skaero/geometry/coordinates.py rename to src/skaero/geometry/coordinates.py From 6b5d0f1f0ed03659358cca491ed3f355e806afd6 Mon Sep 17 00:00:00 2001 From: aqreed Date: Thu, 12 Mar 2020 02:52:14 +0100 Subject: [PATCH 6/8] added "az_elev_dist" function to coordinates and test --- src/skaero/geometry/coordinates.py | 49 +++++++++++++++++++++++++++++- tests/test_coordinates.py | 46 ++++++++++++++++++++++++++++ 2 files changed, 94 insertions(+), 1 deletion(-) diff --git a/src/skaero/geometry/coordinates.py b/src/skaero/geometry/coordinates.py index 2fae341..e64441f 100644 --- a/src/skaero/geometry/coordinates.py +++ b/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 diff --git a/tests/test_coordinates.py b/tests/test_coordinates.py index 7c99ec0..09efccf 100644 --- a/tests/test_coordinates.py +++ b/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)) From 9f1a6609054976b291872b0b63184f85fef20bb4 Mon Sep 17 00:00:00 2001 From: aqreed Date: Thu, 12 Mar 2020 03:13:52 +0100 Subject: [PATCH 7/8] changed import sorting so toxenv=check doesnt fail --- src/skaero/geometry/coordinates.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/skaero/geometry/coordinates.py b/src/skaero/geometry/coordinates.py index e64441f..2b9dff8 100644 --- a/src/skaero/geometry/coordinates.py +++ b/src/skaero/geometry/coordinates.py @@ -5,7 +5,7 @@ """ import numpy as np -from numpy import sin, cos, deg2rad, rad2deg, array +from numpy import array, cos, deg2rad, rad2deg, sin def lla2ecef(lat, lng, h): From 7b9422bdd73de06a4a1ac8a991f0c3ee727c5596 Mon Sep 17 00:00:00 2001 From: aqreed Date: Sat, 14 Mar 2020 20:58:49 +0100 Subject: [PATCH 8/8] changed format to pass tox checks --- src/skaero/geometry/coordinates.py | 159 +++++++++++++++++------------ 1 file changed, 92 insertions(+), 67 deletions(-) diff --git a/src/skaero/geometry/coordinates.py b/src/skaero/geometry/coordinates.py index 2b9dff8..ac10666 100644 --- a/src/skaero/geometry/coordinates.py +++ b/src/skaero/geometry/coordinates.py @@ -29,13 +29,13 @@ def lla2ecef(lat, lng, h): ECEF coordinates in meters """ if abs(lat) > 90: - raise ValueError('latitude should be -90º <= latitude <= 90º') + raise ValueError("latitude should be -90º <= latitude <= 90º") if abs(lng) > 180: - raise ValueError('longitude should be -180º <= longitude <= 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' + msg = "pressure model is only valid if 0 <= h <= 84852.05" raise ValueError(msg) a = 6378137 # [m] Earth equatorial axis @@ -45,11 +45,11 @@ def lla2ecef(lat, lng, h): lat = deg2rad(lat) # degrees to radians lng = deg2rad(lng) # degrees to radians - N = a / (1 - (e * sin(lat))**2)**(.5) + 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) + z = (((b / a) ** 2) * N + h) * sin(lat) return array([x, y, z]) @@ -75,17 +75,21 @@ def ned2ecef(v_ned, lat, lng): vector expressed in ECEF coordinates """ if abs(lat) > 90: - raise ValueError('latitude should be -90º <= latitude <= 90º') + raise ValueError("latitude should be -90º <= latitude <= 90º") if abs(lng) > 180: - raise ValueError('longitude should be -180º <= longitude <= 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)]]) + 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) @@ -114,17 +118,21 @@ def ecef2ned(v_ecef, lat, lng): vector expressed in NED coordinates """ if abs(lat) > 90: - raise ValueError('latitude should be -90º <= latitude <= 90º') + raise ValueError("latitude should be -90º <= latitude <= 90º") if abs(lng) > 180: - raise ValueError('longitude should be -180º <= longitude <= 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)]]) + 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) @@ -152,24 +160,30 @@ def body2ned(v_body, theta, phi, psi): 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(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)]]) + 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) @@ -197,24 +211,30 @@ def ned2body(v_ned, theta, phi, psi): 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(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)]]) + 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) @@ -239,15 +259,19 @@ def body2wind(v_body, alpha, beta): 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(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') + 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)]]) + 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) @@ -272,19 +296,19 @@ def wind2body(v_wind, alpha, beta): 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(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') + 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)]]) + 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) @@ -315,10 +339,10 @@ def az_elev_dist(lla, lla_ref): 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º') + 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º') + raise ValueError("longitude should be -180º <= longitude <= 180º") v = lla2ecef(lat, lng, h) - lla2ecef(lat_ref, lng_ref, h_ref) @@ -330,8 +354,9 @@ def az_elev_dist(lla, lla_ref): 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)) + elevation = np.arctan( + -v_unit_ned[2] / np.sqrt(v_unit_ned[0] ** 2 + v_unit_ned[1] ** 2) + ) distance = np.linalg.norm(v)