| 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 | #include "Klobuchar.hpp" | ||
| 10 | |||
| 11 | #include <algorithm> | ||
| 12 | #include "Navigation/GNSS/Functions.hpp" | ||
| 13 | #include "Navigation/Time/InsTime.hpp" | ||
| 14 | #include "Navigation/Transformations/Units.hpp" | ||
| 15 | #include "util/Assert.h" | ||
| 16 | #include "util/Logger.hpp" | ||
| 17 | |||
| 18 | namespace NAV | ||
| 19 | { | ||
| 20 | |||
| 21 | 22237 | double calcIonosphericTimeDelay_Klobuchar(double tow, Frequency freq, int8_t freqNum, | |
| 22 | double latitude, double longitude, | ||
| 23 | double elevation, double azimuth, | ||
| 24 | const std::array<double, 4>& alpha, const std::array<double, 4>& beta) | ||
| 25 | { | ||
| 26 | 22237 | double phi_u = rad2semicircles(latitude); // User geodetic latitude [semi-circles] WGS 84 | |
| 27 | 22237 | double lambda_u = rad2semicircles(longitude); // User geodetic longitude [semi-circles] WGS 84 | |
| 28 | 22237 | double E = rad2semicircles(elevation); // Angle between the user and satellite [semi-circles] | |
| 29 | |||
| 30 | // Earth's central angle between the user position and the earth projection of ionospheric intersection point [semi-circles] | ||
| 31 | 22237 | double psi = 0.0137 / (E + 0.11) - 0.022; | |
| 32 | LOG_DATA("psi {} [semi-circles] (Earth's central angle between the user position and the earth projection of ionospheric intersection point)", psi); | ||
| 33 | |||
| 34 | // Sub-ionospheric latitude (geodetic latitude of the earth projection of the ionospheric intersection point) [semi-circles] | ||
| 35 | // Also known as Ionospheric Pierce Point IPP | ||
| 36 |
1/2✓ Branch 1 taken 22237 times.
✗ Branch 2 not taken.
|
22237 | double phi_I = std::clamp(phi_u + psi * std::cos(azimuth), -0.416, 0.416); |
| 37 | LOG_DATA("phi_I {} [semi-circles] (Sub-ionospheric latitude)", phi_I); | ||
| 38 | |||
| 39 | // Sub-ionospheric longitude (geodetic longitude of the earth projection of the ionospheric intersection point) [semi-circles] | ||
| 40 | // Also known as Ionospheric Pierce Point IPP | ||
| 41 | 22237 | double lambda_I = lambda_u + (psi * std::sin(azimuth)) / std::cos(semicircles2rad(phi_I)); | |
| 42 | LOG_DATA("lambda_I {} [semi-circles] (Sub-ionospheric longitude)", lambda_I); | ||
| 43 | |||
| 44 | // Geomagnetic latitude of the earth projection of the ionospheric intersection point (mean ionospheric height assumed 350 km) [semi-circles] | ||
| 45 | 22237 | double phi_m = phi_I + 0.064 * std::cos(semicircles2rad(lambda_I - 1.617)); | |
| 46 | LOG_DATA("phi_m {} [semi-circles] (Geomagnetic latitude)", phi_m); | ||
| 47 | |||
| 48 | // Local time [s] | ||
| 49 | 22237 | double t = std::fmod(4.32e4 * lambda_I + tow, InsTimeUtil::SECONDS_PER_DAY); | |
| 50 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 22237 times.
|
22237 | if (t < 0) |
| 51 | { | ||
| 52 | ✗ | t += InsTimeUtil::SECONDS_PER_DAY; | |
| 53 | } | ||
| 54 | LOG_DATA("t {} [s] (Local time)", t); | ||
| 55 | |||
| 56 | // Slant factor / obliquity factor [-] | ||
| 57 | 22237 | double F = 1.0 + 16.0 * std::pow(0.53 - E, 3); | |
| 58 | LOG_DATA("F {} [-] (Slant factor / obliquity factor)", F); | ||
| 59 | |||
| 60 | // Period of the model in [s] | ||
| 61 | 22237 | double PER = 0.0; | |
| 62 |
2/2✓ Branch 0 taken 88948 times.
✓ Branch 1 taken 22237 times.
|
133422 | for (size_t n = 0; n < beta.size(); ++n) |
| 63 | { | ||
| 64 |
1/2✓ Branch 1 taken 88948 times.
✗ Branch 2 not taken.
|
88948 | PER += beta.at(n) * std::pow(phi_m, n); |
| 65 | } | ||
| 66 | 22237 | PER = std::max(PER, 72000.0); | |
| 67 | LOG_DATA("PER {} [s] (Period of the model)", PER); | ||
| 68 | |||
| 69 | // Amplitude of the vertical delay in [s] | ||
| 70 | 22237 | double AMP = 0.0; | |
| 71 |
2/2✓ Branch 0 taken 88948 times.
✓ Branch 1 taken 22237 times.
|
133422 | for (size_t n = 0; n < beta.size(); ++n) |
| 72 | { | ||
| 73 |
1/2✓ Branch 1 taken 88948 times.
✗ Branch 2 not taken.
|
88948 | AMP += alpha.at(n) * std::pow(phi_m, n); |
| 74 | } | ||
| 75 | 22237 | AMP = std::max(AMP, 0.0); | |
| 76 | LOG_DATA("AMP {} [s] (Amplitude of the vertical delay)", AMP); | ||
| 77 | |||
| 78 | // Phase [rad] | ||
| 79 | 22237 | double x = (2 * M_PI * (t - 50400.0)) / PER; | |
| 80 | LOG_DATA("x {} [rad] (Phase)", x); | ||
| 81 | |||
| 82 | // Ionospheric delay [s] | ||
| 83 |
2/2✓ Branch 1 taken 21781 times.
✓ Branch 2 taken 456 times.
|
22237 | double T_iono = std::abs(x) < 1.57 ? F * (5e-9 + AMP * (1.0 - std::pow(x, 2) / 2.0 + std::pow(x, 4) / 24.0)) |
| 84 | 22237 | : F * 5e-9; | |
| 85 | LOG_DATA("T_iono_L1 {} [s] (Ionospheric delay)", T_iono); | ||
| 86 | |||
| 87 | // T_iono is referred to the L1 frequency; if the user is operating on the L2 frequency, the correction term must be multiplied by γ | ||
| 88 |
1/2✓ Branch 2 taken 22237 times.
✗ Branch 3 not taken.
|
22237 | T_iono *= ratioFreqSquared(G01, freq, 0, freqNum); |
| 89 | LOG_DATA("T_iono {} [s] (Ionospheric delay for requested frequency)", T_iono); | ||
| 90 | |||
| 91 | 22237 | return T_iono; | |
| 92 | } | ||
| 93 | |||
| 94 | } // namespace NAV | ||
| 95 |