| 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 NiellMappingFunction.cpp | ||
| 10 | /// @brief Niell Mapping Function (NMF) | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2024-10-17 | ||
| 13 | /// @note See https://gssc.esa.int/navipedia/index.php/Mapping_of_Niell | ||
| 14 | /// @note See \cite Niell1996 Niell1996 | ||
| 15 | |||
| 16 | #include "NiellMappingFunction.hpp" | ||
| 17 | #include "Navigation/Time/InsTime.hpp" | ||
| 18 | #include "Navigation/Transformations/Units.hpp" | ||
| 19 | #include "Navigation/Math/Math.hpp" | ||
| 20 | #include "util/Logger.hpp" | ||
| 21 | |||
| 22 | namespace NAV::internal::NMF | ||
| 23 | { | ||
| 24 | |||
| 25 | constexpr std::array<double, 5> latitude{ deg2rad(15.0), deg2rad(30.0), deg2rad(45.0), deg2rad(60.0), deg2rad(75.0) }; | ||
| 26 | constexpr double a_ht = 2.53e-5; | ||
| 27 | constexpr double b_ht = 5.49e-3; | ||
| 28 | constexpr double c_ht = 1.14e-3; | ||
| 29 | |||
| 30 | namespace | ||
| 31 | { | ||
| 32 | |||
| 33 | 1 | std::array<double, 3> interpolateAverage(double lat) | |
| 34 | { | ||
| 35 | 1 | std::array<double, 5> a{ 1.2769934e-3, 1.2683230e-3, 1.2465397e-3, 1.2196049e-3, 1.2045996e-3 }; | |
| 36 | 1 | std::array<double, 5> b{ 2.9153695e-3, 2.9152299e-3, 2.9288445e-3, 2.9022565e-3, 2.9024912e-3 }; | |
| 37 | 1 | std::array<double, 5> c{ 62.610505e-3, 62.837393e-3, 63.721774e-3, 63.824265e-3, 64.258455e-3 }; | |
| 38 | |||
| 39 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | auto ls = math::lerpSearch(latitude, lat); |
| 40 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | ls.t = std::clamp(ls.t, 0.0, 1.0); |
| 41 | |||
| 42 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | return { std::lerp(a.at(ls.l), a.at(ls.u), ls.t), |
| 43 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | std::lerp(b.at(ls.l), b.at(ls.u), ls.t), |
| 44 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
3 | std::lerp(c.at(ls.l), c.at(ls.u), ls.t) }; |
| 45 | } | ||
| 46 | |||
| 47 | 1 | std::array<double, 3> interpolateAmplitude(double lat) | |
| 48 | { | ||
| 49 | 1 | std::array<double, 5> a{ 0.0000000e-0, 1.2709626e-5, 2.6523662e-5, 3.4000452e-5, 4.1202191e-5 }; | |
| 50 | 1 | std::array<double, 5> b{ 0.0000000e-0, 2.1414979e-5, 3.0160779e-5, 7.2562722e-5, 11.723375e-5 }; | |
| 51 | 1 | std::array<double, 5> c{ 0.0000000e-0, 9.0128400e-5, 4.3497037e-5, 84.795348e-5, 170.37206e-5 }; | |
| 52 | |||
| 53 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | auto ls = math::lerpSearch(latitude, lat); |
| 54 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | ls.t = std::clamp(ls.t, 0.0, 1.0); |
| 55 | |||
| 56 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | return { std::lerp(a.at(ls.l), a.at(ls.u), ls.t), |
| 57 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | std::lerp(b.at(ls.l), b.at(ls.u), ls.t), |
| 58 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
3 | std::lerp(c.at(ls.l), c.at(ls.u), ls.t) }; |
| 59 | } | ||
| 60 | |||
| 61 | 1 | std::array<double, 3> interpolateWet(double lat) | |
| 62 | { | ||
| 63 | 1 | std::array<double, 5> a{ 5.8021897e-4, 5.6794847e-4, 5.8118019e-4, 5.9727542e-4, 6.1641693e-4 }; | |
| 64 | 1 | std::array<double, 5> b{ 1.4275268e-3, 1.5138625e-3, 1.4572752e-3, 1.5007428e-3, 1.7599082e-3 }; | |
| 65 | 1 | std::array<double, 5> c{ 4.3472961e-2, 4.6729510e-2, 4.3908931e-2, 4.4626982e-2, 5.4736038e-2 }; | |
| 66 | |||
| 67 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | auto ls = math::lerpSearch(latitude, lat); |
| 68 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | ls.t = std::clamp(ls.t, 0.0, 1.0); |
| 69 | |||
| 70 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | return { std::lerp(a.at(ls.l), a.at(ls.u), ls.t), |
| 71 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | std::lerp(b.at(ls.l), b.at(ls.u), ls.t), |
| 72 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
3 | std::lerp(c.at(ls.l), c.at(ls.u), ls.t) }; |
| 73 | } | ||
| 74 | |||
| 75 | // mapping normalised to unity at zenith [Marini, 1972] | ||
| 76 | 3 | double mappingNormalizedAtZenith(double elevation, double a, double b, double c) | |
| 77 | { | ||
| 78 | 3 | auto sinE = std::sin(elevation); | |
| 79 | 3 | return (1.0 + a / (1.0 + b / (1.0 + c))) | |
| 80 | 3 | / (sinE + a / (sinE + b / (sinE + c))); | |
| 81 | } | ||
| 82 | |||
| 83 | } // namespace | ||
| 84 | |||
| 85 | } // namespace NAV::internal::NMF | ||
| 86 | |||
| 87 | 1 | double NAV::calcTropoMapFunc_NMFH(const InsTime& epoch, const Eigen::Vector3d& lla_pos, double elevation) | |
| 88 | { | ||
| 89 | using namespace internal::NMF; // NOLINT(google-build-using-namespace) | ||
| 90 | |||
| 91 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | auto yDoySod = epoch.toYDoySod(); |
| 92 | 1 | double doy = yDoySod.doy + static_cast<double>(yDoySod.sod) / InsTimeUtil::SECONDS_PER_DAY; | |
| 93 | |||
| 94 | // reference day is 28 January | ||
| 95 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
|
1 | double year = (doy - 28.0) / 365.25 + (lla_pos(0) < 0.0 ? 0.5 : 0.0); // Add half a year for southern latitudes |
| 96 | 1 | double cosY = std::cos(2 * M_PI * year); | |
| 97 | |||
| 98 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | double lat = std::abs(lla_pos(0)); |
| 99 | |||
| 100 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | auto avg = interpolateAverage(lat); |
| 101 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | auto amp = interpolateAmplitude(lat); |
| 102 | 1 | double a_d = avg[0] - amp[0] * cosY; | |
| 103 | 1 | double b_d = avg[1] - amp[1] * cosY; | |
| 104 | 1 | double c_d = avg[2] - amp[2] * cosY; | |
| 105 | |||
| 106 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | double dm = (1.0 / std::sin(elevation) - mappingNormalizedAtZenith(elevation, a_ht, b_ht, c_ht)) * lla_pos(2) / 1e3; |
| 107 | // ellipsoidal height instead of height above sea level | ||
| 108 | 2 | return mappingNormalizedAtZenith(elevation, a_d, b_d, c_d) + dm; | |
| 109 | } | ||
| 110 | |||
| 111 | 1 | double NAV::calcTropoMapFunc_NMFW(const Eigen::Vector3d& lla_pos, double elevation) | |
| 112 | { | ||
| 113 | using namespace internal::NMF; // NOLINT(google-build-using-namespace) | ||
| 114 | |||
| 115 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | auto [a_w, b_w, c_w] = interpolateWet(std::abs(lla_pos(0))); |
| 116 | 2 | return mappingNormalizedAtZenith(elevation, a_w, b_w, c_w); | |
| 117 | } | ||
| 118 |