| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // This file is part of INSTINCT, the INS Toolkit for Integrated | ||
| 2 | // Navigation Concepts and Training by the Institute of Navigation of | ||
| 3 | // the University of Stuttgart, Germany. | ||
| 4 | // | ||
| 5 | // This Source Code Form is subject to the terms of the Mozilla Public | ||
| 6 | // License, v. 2.0. If a copy of the MPL was not distributed with this | ||
| 7 | // file, You can obtain one at https://mozilla.org/MPL/2.0/. | ||
| 8 | |||
| 9 | /// @file Ellipsoid.hpp | ||
| 10 | /// @brief Functions concerning the ellipsoid model | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2021-11-28 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <concepts> | ||
| 17 | #include "Navigation/Constants.hpp" | ||
| 18 | |||
| 19 | namespace NAV | ||
| 20 | { | ||
| 21 | |||
| 22 | /// @brief r_eS^e The distance of a point on the Earth's surface from the center of the Earth | ||
| 23 | /// @param[in] latitude 𝜙 Latitude in [rad] | ||
| 24 | /// @param[in] R_E Prime vertical radius of curvature (East/West) in [m] | ||
| 25 | /// @param[in] e_squared Square of the first eccentricity of the ellipsoid | ||
| 26 | /// @return Geocentric Radius in [m] | ||
| 27 | /// @note \cite Groves2013 Groves, ch. 2.4.7, eq. 2.137, p. 71 | ||
| 28 | template<std::floating_point Scalar> | ||
| 29 | 447973 | [[nodiscard]] Scalar calcGeocentricRadius(const Scalar& latitude, const Scalar& R_E, const Scalar& e_squared = InsConst::WGS84::e_squared) | |
| 30 | { | ||
| 31 | 447973 | return R_E * std::sqrt(std::pow(std::cos(latitude), 2) + std::pow((1.0 - e_squared) * std::sin(latitude), 2)); | |
| 32 | } | ||
| 33 | |||
| 34 | /// @brief Calculates the North/South (meridian) earth radius | ||
| 35 | /// @param[in] latitude 𝜙 Latitude in [rad] | ||
| 36 | /// @param[in] a Semi-major axis | ||
| 37 | /// @param[in] e_squared Square of the first eccentricity of the ellipsoid | ||
| 38 | /// @return North/South (meridian) earth radius [m] | ||
| 39 | /// @note See \cite Groves2013 Groves, ch. 2.4.2, eq. 2.105, p. 59 | ||
| 40 | /// @note See \cite Titterton2004 Titterton, ch. 3.7.2, eq. 3.83, p. 49 | ||
| 41 | template<std::floating_point Scalar> | ||
| 42 | 5904376 | [[nodiscard]] Scalar calcEarthRadius_N(const Scalar& latitude, const Scalar& a = InsConst::WGS84::a, const Scalar& e_squared = InsConst::WGS84::e_squared) | |
| 43 | { | ||
| 44 | 5904376 | Scalar k = std::sqrt(1 - e_squared * std::pow(std::sin(latitude), 2)); | |
| 45 | |||
| 46 | // North/South (meridian) earth radius [m] | ||
| 47 | 5916912 | return a * (1 - e_squared) / std::pow(k, 3); | |
| 48 | } | ||
| 49 | |||
| 50 | /// @brief Calculates the East/West (prime vertical) earth radius | ||
| 51 | /// @param[in] latitude 𝜙 Latitude in [rad] | ||
| 52 | /// @param[in] a Semi-major axis | ||
| 53 | /// @param[in] e_squared Square of the first eccentricity of the ellipsoid | ||
| 54 | /// @return East/West (prime vertical) earth radius [m] | ||
| 55 | /// @note See \cite Groves2013 Groves, ch. 2.4.2, eq. 2.106, p. 59 | ||
| 56 | /// @note See \cite Titterton2004 Titterton, ch. 3.7.2, eq. 3.84, p. 49 | ||
| 57 | template<std::floating_point Scalar> | ||
| 58 | 25220124 | [[nodiscard]] Scalar calcEarthRadius_E(const Scalar& latitude, const Scalar& a = InsConst::WGS84::a, const Scalar& e_squared = InsConst::WGS84::e_squared) | |
| 59 | { | ||
| 60 | // East/West (prime vertical) earth radius [m] | ||
| 61 | 25220124 | return a / std::sqrt(1 - e_squared * std::pow(std::sin(latitude), 2)); | |
| 62 | } | ||
| 63 | |||
| 64 | /// @brief Conversion matrix between cartesian and curvilinear perturbations to the position | ||
| 65 | /// @param[in] lla_position Position as Lat Lon Alt in [rad rad m] | ||
| 66 | /// @param[in] R_N Meridian radius of curvature in [m] | ||
| 67 | /// @param[in] R_E Prime vertical radius of curvature (East/West) [m] | ||
| 68 | /// @return T_rn_p A 3x3 matrix | ||
| 69 | /// @note See \cite Groves2013 Groves, ch. 2.4.3, eq. 2.119, p. 63 | ||
| 70 | template<typename Derived> | ||
| 71 | 62626 | [[nodiscard]] Eigen::Matrix3<typename Derived::Scalar> conversionMatrixCartesianCurvilinear(const Eigen::MatrixBase<Derived>& lla_position, | |
| 72 | const typename Derived::Scalar& R_N, const typename Derived::Scalar& R_E) | ||
| 73 | { | ||
| 74 |
1/2✓ Branch 1 taken 62626 times.
✗ Branch 2 not taken.
|
62626 | return Eigen::DiagonalMatrix<typename Derived::Scalar, 3>{ 1.0 / (R_N + lla_position(2)), |
| 75 |
2/4✓ Branch 1 taken 62626 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62626 times.
✗ Branch 5 not taken.
|
62626 | 1.0 / ((R_E + lla_position(2)) * std::cos(lla_position(0))), |
| 76 |
2/4✓ Branch 1 taken 62626 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62626 times.
✗ Branch 5 not taken.
|
125252 | -1.0 }; |
| 77 | } | ||
| 78 | |||
| 79 | /// @brief Measure the distance between two points on a sphere | ||
| 80 | /// @param[in] lat1 Latitude of first point in [rad] | ||
| 81 | /// @param[in] lon1 Longitude of first point in [rad] | ||
| 82 | /// @param[in] lat2 Latitude of second point in [rad] | ||
| 83 | /// @param[in] lon2 Longitude of second point in [rad] | ||
| 84 | /// @return The distance in [m] | ||
| 85 | /// | ||
| 86 | /// @note See Haversine Formula (https://www.movable-type.co.uk/scripts/latlong.html) | ||
| 87 | template<std::floating_point Scalar> | ||
| 88 | 198988 | [[nodiscard]] Scalar calcGreatCircleDistance(Scalar lat1, Scalar lon1, Scalar lat2, Scalar lon2) | |
| 89 | { | ||
| 90 |
2/4✓ Branch 1 taken 198988 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 198988 times.
✗ Branch 5 not taken.
|
198988 | Scalar R = calcGeocentricRadius(lat1, calcEarthRadius_E(lat1)); |
| 91 | 198988 | Scalar dLat = lat2 - lat1; | |
| 92 | 198988 | Scalar dLon = lon2 - lon1; | |
| 93 | 198988 | Scalar a = std::pow(std::sin(dLat / 2.0), 2) + std::cos(lat1) * std::cos(lat2) * std::pow(std::sin(dLon / 2.0), 2); | |
| 94 | 198988 | Scalar c = 2.0 * std::atan2(std::sqrt(a), std::sqrt(1.0 - a)); | |
| 95 | 198988 | return R * c; // meters | |
| 96 | } | ||
| 97 | |||
| 98 | /// @brief Measure the distance between two points over an ellipsoidal-surface | ||
| 99 | /// @param[in] lat1 Latitude of first point in [rad] | ||
| 100 | /// @param[in] lon1 Longitude of first point in [rad] | ||
| 101 | /// @param[in] lat2 Latitude of second point in [rad] | ||
| 102 | /// @param[in] lon2 Longitude of second point in [rad] | ||
| 103 | /// @return The distance in [m] | ||
| 104 | /// | ||
| 105 | /// @note See Lambert's formula for long lines (https://en.wikipedia.org/wiki/Geographical_distance#Lambert's_formula_for_long_lines) | ||
| 106 | template<std::floating_point Scalar> | ||
| 107 | 198992 | [[nodiscard]] Scalar calcGeographicalDistance(Scalar lat1, Scalar lon1, Scalar lat2, Scalar lon2) | |
| 108 | { | ||
| 109 |
4/4✓ Branch 0 taken 99498 times.
✓ Branch 1 taken 99494 times.
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 99494 times.
|
198992 | if (lat1 == lat2 && lon1 == lon2) |
| 110 | { | ||
| 111 | 4 | return 0; | |
| 112 | } | ||
| 113 | // First convert the latitudes 𝜙₁,𝜙₂ of the two points to reduced latitudes 𝛽₁,𝛽₂ | ||
| 114 | 198988 | Scalar beta1 = std::atan((1 - InsConst::WGS84::f) * std::tan(lat1)); | |
| 115 | 198988 | Scalar beta2 = std::atan((1 - InsConst::WGS84::f) * std::tan(lat2)); | |
| 116 | |||
| 117 | // Then calculate the central angle 𝜎 in radians between two points 𝛽₁,𝜆₁ and 𝛽₂,𝜆₂ on a sphere using the | ||
| 118 | // Great-circle distance method (law of cosines or haversine formula), with longitudes 𝜆₁ and 𝜆₂ being the same on the sphere as on the spheroid. | ||
| 119 | 198988 | Scalar sigma = calcGreatCircleDistance(beta1, lon1, beta2, lon2) | |
| 120 |
2/4✓ Branch 1 taken 198988 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 198988 times.
✗ Branch 5 not taken.
|
198988 | / calcGeocentricRadius(lat1, calcEarthRadius_E(lat1)); |
| 121 | |||
| 122 | 198988 | Scalar P = (beta1 + beta2) / 2; | |
| 123 | 198988 | Scalar Q = (beta2 - beta1) / 2; | |
| 124 | |||
| 125 | 198988 | Scalar X = (sigma - std::sin(sigma)) * std::pow((std::sin(P) * std::cos(Q)) / std::cos(sigma / 2), 2); | |
| 126 | 198988 | Scalar Y = (sigma + std::sin(sigma)) * std::pow((std::cos(P) * std::sin(Q)) / std::sin(sigma / 2), 2); | |
| 127 | |||
| 128 | 198988 | return InsConst::WGS84::a * (sigma - InsConst::WGS84::f / 2.0 * (X + Y)); | |
| 129 | } | ||
| 130 | |||
| 131 | } // namespace NAV | ||
| 132 |