| 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 ObservationEstimator.hpp | ||
| 10 | /// @brief Calculates Observation estimates | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2023-12-21 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <algorithm> | ||
| 17 | #include <cmath> | ||
| 18 | #include <cstddef> | ||
| 19 | #include <cstdint> | ||
| 20 | #include <memory> | ||
| 21 | #include <unordered_map> | ||
| 22 | #include <utility> | ||
| 23 | #include <vector> | ||
| 24 | |||
| 25 | #include <imgui.h> | ||
| 26 | #include <application.h> | ||
| 27 | #include <imgui_stdlib.h> | ||
| 28 | #include "Navigation/GNSS/Core/Frequency.hpp" | ||
| 29 | #include "Navigation/GNSS/Core/SatelliteIdentifier.hpp" | ||
| 30 | #include "Navigation/GNSS/Positioning/AntexReader.hpp" | ||
| 31 | #include "Navigation/Transformations/Antenna.hpp" | ||
| 32 | #include "Navigation/Transformations/CoordinateFrames.hpp" | ||
| 33 | #include "NodeData/GNSS/GnssObs.hpp" | ||
| 34 | #include "internal/gui/widgets/imgui_ex.hpp" | ||
| 35 | #include "internal/gui/widgets/HelpMarker.hpp" | ||
| 36 | |||
| 37 | #include "Navigation/Atmosphere/Ionosphere/Ionosphere.hpp" | ||
| 38 | #include "Navigation/Atmosphere/Troposphere/Troposphere.hpp" | ||
| 39 | #include "Navigation/GNSS/Errors/MeasurementErrors.hpp" | ||
| 40 | #include "Navigation/GNSS/Positioning/Observation.hpp" | ||
| 41 | #include "Navigation/GNSS/Positioning/Receiver.hpp" | ||
| 42 | #include "Navigation/GNSS/Satellite/Ephemeris/GLONASSEphemeris.hpp" | ||
| 43 | |||
| 44 | #include "util/Container/UncertainValue.hpp" | ||
| 45 | #include "util/Json.hpp" | ||
| 46 | #include <Eigen/src/Core/MatrixBase.h> | ||
| 47 | #include <fmt/core.h> | ||
| 48 | #include <fmt/format.h> | ||
| 49 | #include <fmt/ranges.h> | ||
| 50 | |||
| 51 | namespace NAV | ||
| 52 | { | ||
| 53 | |||
| 54 | /// @brief Calculates Observation estimates | ||
| 55 | class ObservationEstimator | ||
| 56 | { | ||
| 57 | public: | ||
| 58 | /// @brief Constructor | ||
| 59 | /// @param[in] receiverCount Number of receivers | ||
| 60 | 119 | explicit ObservationEstimator(size_t receiverCount) | |
| 61 | 119 | : _receiverCount(receiverCount) | |
| 62 | { | ||
| 63 |
2/2✓ Branch 0 taken 119 times.
✓ Branch 1 taken 119 times.
|
238 | for (size_t i = 0; i < receiverCount; i++) |
| 64 | { | ||
| 65 |
3/8✓ Branch 2 taken 119 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 119 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 119 times.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
|
119 | _antenna.emplace(i, Antenna{}); |
| 66 | } | ||
| 67 | 119 | } | |
| 68 | |||
| 69 | /// @brief How the observation gets used. Influenced the measurement variance | ||
| 70 | enum ObservationDifference : uint8_t | ||
| 71 | { | ||
| 72 | NoDifference, ///< Estimation is not differenced | ||
| 73 | SingleDifference, ///< Single Difference | ||
| 74 | DoubleDifference, ///< Double Difference | ||
| 75 | }; | ||
| 76 | |||
| 77 | /// @brief Calculates the observation estimates | ||
| 78 | /// @param[in, out] observations List of GNSS observation data used for the calculation of this epoch | ||
| 79 | /// @param[in] ionosphericCorrections Ionospheric correction parameters collected from the Nav data | ||
| 80 | /// @param[in] receiver Receiver | ||
| 81 | /// @param[in] nameId Name and Id of the node used for log messages only | ||
| 82 | /// @param[in] obsDiff Observation Difference type to estimate | ||
| 83 | template<typename ReceiverType> | ||
| 84 | 933 | void calcObservationEstimates(Observations& observations, | |
| 85 | const Receiver<ReceiverType>& receiver, | ||
| 86 | const std::shared_ptr<const IonosphericCorrections>& ionosphericCorrections, | ||
| 87 | [[maybe_unused]] const std::string& nameId, | ||
| 88 | ObservationDifference obsDiff) const | ||
| 89 | { | ||
| 90 | LOG_DATA("{}: Calculating all observation estimates:", nameId); | ||
| 91 |
2/2✓ Branch 7 taken 34903 times.
✓ Branch 8 taken 933 times.
|
35836 | for (auto& [satSigId, observation] : observations.signals) |
| 92 | { | ||
| 93 |
1/2✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
|
34903 | calcObservationEstimate(satSigId, observation, receiver, ionosphericCorrections, nameId, obsDiff); |
| 94 | } | ||
| 95 | 933 | } | |
| 96 | |||
| 97 | /// @brief Calculates the observation estimate for the given signal | ||
| 98 | /// @param[in] satSigId Satellite signal identifier | ||
| 99 | /// @param[in, out] observation GNSS observation data used for the calculation of this epoch | ||
| 100 | /// @param[in] ionosphericCorrections Ionospheric correction parameters collected from the Nav data | ||
| 101 | /// @param[in] receiver Receiver | ||
| 102 | /// @param[in] nameId Name and Id of the node used for log messages only | ||
| 103 | /// @param[in] obsDiff Observation Difference type to estimate | ||
| 104 | template<typename ReceiverType> | ||
| 105 | 34903 | void calcObservationEstimate(const SatSigId& satSigId, | |
| 106 | Observations::SignalObservation& observation, | ||
| 107 | const Receiver<ReceiverType>& receiver, | ||
| 108 | const std::shared_ptr<const IonosphericCorrections>& ionosphericCorrections, | ||
| 109 | [[maybe_unused]] const std::string& nameId, | ||
| 110 | ObservationDifference obsDiff) const | ||
| 111 | { | ||
| 112 | LOG_DATA("{}: Calculating observation estimates for [{}]", nameId, satSigId); | ||
| 113 |
1/2✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
|
34903 | const Frequency freq = satSigId.freq(); |
| 114 |
1/2✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
|
34903 | const SatelliteSystem satSys = freq.getSatSys(); |
| 115 | |||
| 116 |
1/2✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
|
34903 | auto& recvObs = observation.recvObs.at(receiver.type); |
| 117 | |||
| 118 | 34903 | Eigen::Vector3d e_recvPosAPC = e_calcRecvPosAPC(freq, | |
| 119 | 34903 | receiver.type, | |
| 120 | 34903 | receiver.e_posMarker, | |
| 121 |
1/2✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
|
34903 | receiver.gnssObs, nameId); |
| 122 |
1/2✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
|
34903 | Eigen::Vector3d lla_recvPosAPC = trafo::ecef2lla_WGS84(e_recvPosAPC); |
| 123 | |||
| 124 |
1/2✓ Branch 2 taken 34903 times.
✗ Branch 3 not taken.
|
34903 | double elevation = recvObs->satElevation(e_recvPosAPC, lla_recvPosAPC); |
| 125 |
1/2✓ Branch 2 taken 34903 times.
✗ Branch 3 not taken.
|
34903 | double azimuth = recvObs->satAzimuth(e_recvPosAPC, lla_recvPosAPC); |
| 126 |
1/2✓ Branch 2 taken 34903 times.
✗ Branch 3 not taken.
|
34903 | Eigen::Vector3d e_pLOS = recvObs->e_pLOS(e_recvPosAPC); |
| 127 | |||
| 128 | // Receiver-Satellite Range [m] | ||
| 129 |
2/4✓ Branch 3 taken 34903 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 34903 times.
✗ Branch 7 not taken.
|
34903 | double rho_r_s = (recvObs->e_satPos() - e_recvPosAPC).norm(); |
| 130 | 34903 | recvObs->terms.rho_r_s = rho_r_s; | |
| 131 | // Troposphere | ||
| 132 |
1/2✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
|
34903 | auto tropo_r_s = calcTroposphericDelayAndMapping(receiver.gnssObs->insTime, lla_recvPosAPC, |
| 133 | 34903 | elevation, azimuth, _troposphereModels, nameId); | |
| 134 | 34903 | recvObs->terms.tropoZenithDelay = tropo_r_s; | |
| 135 | // Estimated troposphere propagation error [m] | ||
| 136 | 34903 | double dpsr_T_r_s = tropo_r_s.ZHD * tropo_r_s.zhdMappingFactor + tropo_r_s.ZWD * tropo_r_s.zwdMappingFactor; | |
| 137 | 34903 | recvObs->terms.dpsr_T_r_s = dpsr_T_r_s; | |
| 138 | // Estimated ionosphere propagation error [m] | ||
| 139 |
2/4✓ Branch 4 taken 34903 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34903 times.
✗ Branch 8 not taken.
|
69806 | double dpsr_I_r_s = calcIonosphericDelay(static_cast<double>(receiver.gnssObs->insTime.toGPSweekTow().tow), |
| 140 | 34903 | freq, observation.freqNum(), lla_recvPosAPC, elevation, azimuth, | |
| 141 | 34903 | _ionosphereModel, ionosphericCorrections.get()); | |
| 142 | 34903 | recvObs->terms.dpsr_I_r_s = dpsr_I_r_s; | |
| 143 | // Sagnac correction [m] | ||
| 144 |
1/2✓ Branch 3 taken 34903 times.
✗ Branch 4 not taken.
|
34903 | double dpsr_ie_r_s = calcSagnacCorrection(e_recvPosAPC, recvObs->e_satPos()); |
| 145 | 34903 | recvObs->terms.dpsr_ie_r_s = dpsr_ie_r_s; | |
| 146 | |||
| 147 | // Earth's gravitational field causes relativistic signal delay due to space-time curvature (Shapiro effect) [s] | ||
| 148 | // double posNorm = recvObs->e_satPos().norm() + e_recvPosAPC.norm(); | ||
| 149 | // double dt_rel_stc = e_recvPosAPC.norm() > InsConst::WGS84::a / 2.0 ? 2.0 * InsConst::WGS84::MU / std::pow(InsConst::C, 3) * std::log((posNorm + rho_r_s) / (posNorm - rho_r_s)) | ||
| 150 | // : 0.0; | ||
| 151 | |||
| 152 |
1/2✓ Branch 2 taken 34903 times.
✗ Branch 3 not taken.
|
34903 | double cn0 = recvObs->gnssObsData().CN0.value_or(1.0); |
| 153 | |||
| 154 |
1/2✓ Branch 3 taken 34903 times.
✗ Branch 4 not taken.
|
34903 | double pcv = calcPCV(freq, receiver.gnssObs->insTime, elevation, std::nullopt, receiver.type, receiver.gnssObs, nameId); // TODO: use 'azimuth', but need antenna azimuth |
| 155 | |||
| 156 |
2/2✓ Branch 8 taken 68930 times.
✓ Branch 9 taken 34903 times.
|
103833 | for (auto& [obsType, obsData] : recvObs->obs) |
| 157 | { | ||
| 158 | LOG_DATA("{}: [{}][{:11}][{:5}] Observation estimate", nameId, satSigId, obsType, receiver.type); | ||
| 159 |
2/5✓ Branch 0 taken 34903 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 34027 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
68930 | switch (obsType) |
| 160 | { | ||
| 161 | 34903 | case GnssObs::Pseudorange: | |
| 162 | 34903 | obsData.estimate = rho_r_s | |
| 163 | 34903 | + dpsr_ie_r_s | |
| 164 | 34903 | + dpsr_T_r_s | |
| 165 | 34903 | + dpsr_I_r_s | |
| 166 | 34903 | + InsConst::C | |
| 167 |
1/2✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
|
34903 | * (*receiver.recvClk.biasFor(satSys) - recvObs->satClock().bias |
| 168 | // + dt_rel_stc | ||
| 169 |
4/6✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 16485 times.
✓ Branch 4 taken 18418 times.
✓ Branch 6 taken 16485 times.
✗ Branch 7 not taken.
|
34903 | + (receiver.interFrequencyBias.contains(freq) ? receiver.interFrequencyBias.at(freq).value : 0.0)) |
| 170 | 34903 | + pcv; | |
| 171 |
1/2✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
|
34903 | obsData.measVar = _gnssMeasurementErrorModel.psrMeasErrorVar(satSys, elevation, cn0); |
| 172 | LOG_DATA("{}: [{}][{:11}][{:5}] {:.4f} [m] Geometrical range", nameId, satSigId, obsType, receiver.type, rho_r_s); | ||
| 173 | LOG_DATA("{}: [{}][{:11}][{:5}] + {:.4f} [m] Sagnac correction", nameId, satSigId, obsType, receiver.type, dpsr_ie_r_s); | ||
| 174 | if (dpsr_T_r_s != 0.0) { LOG_DATA("{}: [{}][{:11}][{:5}] + {:.4f} [m] Tropospheric delay", nameId, satSigId, obsType, receiver.type, dpsr_T_r_s); } | ||
| 175 | if (dpsr_I_r_s != 0.0) { LOG_DATA("{}: [{}][{:11}][{:5}] + {:.4f} [m] Ionospheric delay", nameId, satSigId, obsType, receiver.type, dpsr_I_r_s); } | ||
| 176 |
1/2✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
|
34903 | if (*receiver.recvClk.biasFor(satSys) != 0.0) { LOG_DATA("{}: [{}][{:11}][{:5}] + {:.4f} [m] Receiver clock bias", nameId, satSigId, obsType, receiver.type, InsConst::C * *receiver.recvClk.biasFor(satSys)); } |
| 177 | LOG_DATA("{}: [{}][{:11}][{:5}] - {:.4f} [m] Satellite clock bias", nameId, satSigId, obsType, receiver.type, InsConst::C * recvObs->satClock().bias); | ||
| 178 | // LOG_DATA("{}: [{}][{:11}][{:5}] + {:.4f} [m] Shapiro effect", nameId, satSigId, obsType, receiver.type, InsConst::C * dt_rel_stc); | ||
| 179 |
1/2✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
|
34903 | if (receiver.interFrequencyBias.contains(freq)) { LOG_DATA("{}: [{}][{:11}][{:5}] + {:.4f} [m] Inter-frequency bias", nameId, satSigId, obsType, receiver.type, InsConst::C * receiver.interFrequencyBias.at(freq).value); } |
| 180 | LOG_DATA("{}: [{}][{:11}][{:5}] = {:.4f} [m] Pseudorange estimate", nameId, satSigId, obsType, receiver.type, obsData.estimate); | ||
| 181 | LOG_DATA("{}: [{}][{:11}][{:5}] {:.4f} [m] Pseudorange measurement", nameId, satSigId, obsType, receiver.type, obsData.measurement); | ||
| 182 | LOG_DATA("{}: [{}][{:11}][{:5}] {:.4e} [m] Difference to measurement", nameId, satSigId, obsType, receiver.type, obsData.measurement - obsData.estimate); | ||
| 183 | 34903 | break; | |
| 184 | ✗ | case GnssObs::Carrier: | |
| 185 | ✗ | obsData.estimate = rho_r_s | |
| 186 | ✗ | + dpsr_ie_r_s | |
| 187 | ✗ | + dpsr_T_r_s | |
| 188 | ✗ | - dpsr_I_r_s | |
| 189 | ✗ | + InsConst::C | |
| 190 | ✗ | * (*receiver.recvClk.biasFor(satSys) - recvObs->satClock().bias | |
| 191 | // + dt_rel_stc | ||
| 192 | ) | ||
| 193 | ✗ | + pcv; | |
| 194 | ✗ | obsData.measVar = _gnssMeasurementErrorModel.carrierMeasErrorVar(satSys, elevation, cn0); | |
| 195 | LOG_DATA("{}: [{}][{:11}][{:5}] {:.4f} [m] Geometrical range", nameId, satSigId, obsType, receiver.type, rho_r_s); | ||
| 196 | LOG_DATA("{}: [{}][{:11}][{:5}] + {:.4f} [m] Sagnac correction", nameId, satSigId, obsType, receiver.type, dpsr_ie_r_s); | ||
| 197 | if (dpsr_T_r_s != 0.0) { LOG_DATA("{}: [{}][{:11}][{:5}] + {:.4f} [m] Tropospheric delay", nameId, satSigId, obsType, receiver.type, dpsr_T_r_s); } | ||
| 198 | if (dpsr_I_r_s != 0.0) { LOG_DATA("{}: [{}][{:11}][{:5}] - {:.4f} [m] Ionospheric delay", nameId, satSigId, obsType, receiver.type, dpsr_I_r_s); } | ||
| 199 | ✗ | if (*receiver.recvClk.biasFor(satSys) != 0.0) { LOG_DATA("{}: [{}][{:11}][{:5}] + {:.4f} [m] Receiver clock bias", nameId, satSigId, obsType, receiver.type, InsConst::C * *receiver.recvClk.biasFor(satSys)); } | |
| 200 | LOG_DATA("{}: [{}][{:11}][{:5}] - {:.4f} [m] Satellite clock bias", nameId, satSigId, obsType, receiver.type, InsConst::C * recvObs->satClock().bias); | ||
| 201 | // LOG_DATA("{}: [{}][{:11}][{:5}] + {:.4f} [m] Shapiro effect", nameId, satSigId, obsType, receiver.type, InsConst::C * dt_rel_stc); | ||
| 202 | LOG_DATA("{}: [{}][{:11}][{:5}] = {:.4f} [m] Carrier-phase estimate", nameId, satSigId, obsType, receiver.type, obsData.estimate); | ||
| 203 | LOG_DATA("{}: [{}][{:11}][{:5}] {:.4f} [m] Carrier-phase measurement", nameId, satSigId, obsType, receiver.type, obsData.measurement); | ||
| 204 | LOG_DATA("{}: [{}][{:11}][{:5}] {:.4e} [m] Difference to measurement", nameId, satSigId, obsType, receiver.type, obsData.measurement - obsData.estimate); | ||
| 205 | ✗ | break; | |
| 206 | 34027 | case GnssObs::Doppler: | |
| 207 |
2/4✓ Branch 3 taken 34027 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 34027 times.
✗ Branch 7 not taken.
|
34027 | obsData.estimate = e_pLOS.dot(recvObs->e_satVel() - receiver.e_vel) |
| 208 |
1/2✓ Branch 5 taken 34027 times.
✗ Branch 6 not taken.
|
34027 | - calcSagnacRateCorrection(e_recvPosAPC, recvObs->e_satPos(), receiver.e_vel, recvObs->e_satVel()) |
| 209 | 34027 | + InsConst::C | |
| 210 |
1/2✓ Branch 1 taken 34027 times.
✗ Branch 2 not taken.
|
34027 | * (*receiver.recvClk.driftFor(satSys) |
| 211 | 34027 | - recvObs->satClock().drift); | |
| 212 |
1/2✓ Branch 2 taken 34027 times.
✗ Branch 3 not taken.
|
34027 | obsData.measVar = _gnssMeasurementErrorModel.psrRateMeasErrorVar(freq, observation.freqNum(), elevation, cn0); |
| 213 | LOG_DATA("{}: [{}][{:11}][{:5}] {:.4f} [m/s] ", nameId, satSigId, obsType, receiver.type, e_pLOS.dot(recvObs->e_satVel() - receiver.e_vel)); | ||
| 214 | LOG_DATA("{}: [{}][{:11}][{:5}] - {:.4f} [m/s] Sagnac rate correction", nameId, satSigId, obsType, receiver.type, calcSagnacRateCorrection(e_recvPosAPC, recvObs->e_satPos(), receiver.e_vel, recvObs->e_satVel())); | ||
| 215 |
1/2✓ Branch 1 taken 34027 times.
✗ Branch 2 not taken.
|
34027 | if (*receiver.recvClk.driftFor(satSys) != 0.0) { LOG_DATA("{}: [{}][{:11}][{:5}] + {:.4f} [m/s] Receiver clock drift", nameId, satSigId, obsType, receiver.type, InsConst::C * *receiver.recvClk.driftFor(satSys)); } |
| 216 | LOG_DATA("{}: [{}][{:11}][{:5}] - {:.4f} [m/s] Satellite clock drift", nameId, satSigId, obsType, receiver.type, InsConst::C * recvObs->satClock().drift); | ||
| 217 | LOG_DATA("{}: [{}][{:11}][{:5}] = {:.4f} [m/s] Doppler estimate", nameId, satSigId, obsType, receiver.type, obsData.estimate); | ||
| 218 | LOG_DATA("{}: [{}][{:11}][{:5}] {:.4f} [m/s] Doppler measurement", nameId, satSigId, obsType, receiver.type, obsData.measurement); | ||
| 219 | LOG_DATA("{}: [{}][{:11}][{:5}] {:.4e} [m/s] Difference to measurement", nameId, satSigId, obsType, receiver.type, obsData.measurement - obsData.estimate); | ||
| 220 | 34027 | break; | |
| 221 | ✗ | case GnssObs::ObservationType_COUNT: | |
| 222 | ✗ | break; | |
| 223 | } | ||
| 224 | LOG_DATA("{}: [{}][{:11}][{:5}] Observation error variance", nameId, satSigId, obsType, receiver.type); | ||
| 225 | LOG_DATA("{}: [{}][{:11}][{:5}] {:.4g} [{}] Measurement error variance", nameId, satSigId, obsType, receiver.type, obsData.measVar, | ||
| 226 | obsType == GnssObs::Doppler ? "m^2/s^2" : "m^2"); | ||
| 227 | |||
| 228 |
1/2✓ Branch 0 taken 68930 times.
✗ Branch 1 not taken.
|
68930 | if (obsDiff == NoDifference) |
| 229 | { | ||
| 230 |
3/4✓ Branch 0 taken 34027 times.
✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 34027 times.
|
68930 | if (obsType == GnssObs::Pseudorange || obsType == GnssObs::Carrier) |
| 231 | { | ||
| 232 |
1/2✓ Branch 3 taken 34903 times.
✗ Branch 4 not taken.
|
34903 | obsData.measVar += observation.navData()->calcSatellitePositionVariance() |
| 233 |
1/2✓ Branch 2 taken 34903 times.
✗ Branch 3 not taken.
|
34903 | + ionoErrorVar(dpsr_I_r_s, freq, observation.freqNum()) |
| 234 |
1/2✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
|
34903 | + tropoErrorVar(dpsr_T_r_s, elevation); |
| 235 | LOG_DATA("{}: [{}][{:11}][{:5}] + {:.4f} [m^2] Satellite position variance", nameId, satSigId, obsType, receiver.type, observation.navData()->calcSatellitePositionVariance()); | ||
| 236 | if (dpsr_I_r_s != 0.0) { LOG_DATA("{}: [{}][{:11}][{:5}] + {:.4f} [m^2] Ionosphere variance", nameId, satSigId, obsType, receiver.type, ionoErrorVar(dpsr_I_r_s, freq, observation.freqNum())); } | ||
| 237 | if (dpsr_T_r_s != 0.0) { LOG_DATA("{}: [{}][{:11}][{:5}] + {:.4f} [m^2] Troposphere variance", nameId, satSigId, obsType, receiver.type, tropoErrorVar(dpsr_T_r_s, elevation)); } | ||
| 238 | } | ||
| 239 |
2/2✓ Branch 0 taken 34903 times.
✓ Branch 1 taken 34027 times.
|
68930 | if (obsType == GnssObs::Pseudorange) |
| 240 | { | ||
| 241 |
1/2✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
|
34903 | obsData.measVar += _gnssMeasurementErrorModel.codeBiasErrorVar(); |
| 242 | LOG_DATA("{}: [{}][{:11}][{:5}] + {:.4f} [m^2] Code bias variance", nameId, satSigId, obsType, receiver.type, _gnssMeasurementErrorModel.codeBiasErrorVar()); | ||
| 243 | } | ||
| 244 | } | ||
| 245 | LOG_DATA("{}: [{}][{:11}][{:5}] = {:.4g} [{}] Observation error variance", nameId, satSigId, obsType, receiver.type, obsData.measVar, | ||
| 246 | obsType == GnssObs::Doppler ? "m^2/s^2" : "m^2"); | ||
| 247 | } | ||
| 248 | 34903 | } | |
| 249 | |||
| 250 | /// Satellite Info used for estimate calculation | ||
| 251 | struct SatelliteInfo | ||
| 252 | { | ||
| 253 | /// @brief Constructor | ||
| 254 | /// @param[in] e_satPos Satellite position in ECEF coordinates [m] | ||
| 255 | /// @param[in] e_satVel Satellite velocity in ECEF frame [m/s] | ||
| 256 | /// @param[in] satClockBias Satellite clock bias [s] | ||
| 257 | /// @param[in] satClockDrift Satellite clock drift [s/s] | ||
| 258 | SatelliteInfo(Eigen::Vector3d e_satPos, Eigen::Vector3d e_satVel, double satClockBias, double satClockDrift) | ||
| 259 | : e_satPos(std::move(e_satPos)), e_satVel(std::move(e_satVel)), satClockBias(satClockBias), satClockDrift(satClockDrift) {} | ||
| 260 | |||
| 261 | Eigen::Vector3d e_satPos; ///< Satellite position in ECEF coordinates [m] | ||
| 262 | Eigen::Vector3d e_satVel; ///< Satellite velocity in ECEF frame [m/s] | ||
| 263 | double satClockBias{}; ///< Satellite clock bias [s] | ||
| 264 | double satClockDrift{}; ///< Satellite clock drift [s/s] | ||
| 265 | }; | ||
| 266 | |||
| 267 | /// @brief Calculates the pseudorange estimate | ||
| 268 | /// @param[in] freq Signal frequency | ||
| 269 | /// @param[in] receiverType Receiver type to select the correct antenna | ||
| 270 | /// @param[in] e_recvPosAPC Receiver antenna phase center position in ECEF coordinates [m] | ||
| 271 | /// @param[in] recvClockBias Receiver clock bias [s] | ||
| 272 | /// @param[in] interFreqBias Receiver inter-frequency bias [s] | ||
| 273 | /// @param[in] dpsr_T_r_s Estimated troposphere propagation error [m] | ||
| 274 | /// @param[in] dpsr_I_r_s Estimated ionosphere propagation error [m] | ||
| 275 | /// @param[in] satInfo Satellite Information (pos, vel, clock) | ||
| 276 | /// @param[in] elevation Elevation of the satellite [rad] | ||
| 277 | /// @param[in] gnssObs GNSS observation | ||
| 278 | /// @param[in] nameId Name and Id of the node used for log messages only | ||
| 279 | /// @return The estimate [m] and the position line-of-sight unit vector (used for the Jacobian) | ||
| 280 | template<typename Derived> | ||
| 281 | auto calcPseudorangeEstimate(Frequency freq, | ||
| 282 | size_t receiverType, | ||
| 283 | const Eigen::MatrixBase<Derived>& e_recvPosAPC, | ||
| 284 | const auto& recvClockBias, | ||
| 285 | const auto& interFreqBias, | ||
| 286 | const auto& dpsr_T_r_s, | ||
| 287 | const auto& dpsr_I_r_s, | ||
| 288 | const std::shared_ptr<const SatelliteInfo>& satInfo, | ||
| 289 | const auto& elevation, | ||
| 290 | const std::shared_ptr<const GnssObs>& gnssObs, | ||
| 291 | [[maybe_unused]] const std::string& nameId) const | ||
| 292 | { | ||
| 293 | // Receiver-Satellite Range [m] | ||
| 294 | auto rho_r_s = (satInfo->e_satPos - e_recvPosAPC).norm(); | ||
| 295 | |||
| 296 | // Sagnac correction [m] | ||
| 297 | auto dpsr_ie_r_s = calcSagnacCorrection(e_recvPosAPC, satInfo->e_satPos); | ||
| 298 | |||
| 299 | // Earth's gravitational field causes relativistic signal delay due to space-time curvature (Shapiro effect) [s] | ||
| 300 | // double posNorm = e_satPos.norm() + e_recvPosAPC.norm(); | ||
| 301 | // double dt_rel_stc = e_recvPosAPC.norm() > InsConst::WGS84::a / 2.0 ? 2.0 * InsConst::WGS84::MU / std::pow(InsConst::C, 3) * std::log((posNorm + rho_r_s) / (posNorm - rho_r_s)) | ||
| 302 | // : 0.0; | ||
| 303 | |||
| 304 | auto pcv = calcPCV(freq, gnssObs->insTime, elevation, std::nullopt, receiverType, gnssObs, nameId); // TODO: use 'azimuth', but need antenna azimuth | ||
| 305 | |||
| 306 | return rho_r_s | ||
| 307 | + dpsr_ie_r_s | ||
| 308 | + dpsr_T_r_s | ||
| 309 | + dpsr_I_r_s | ||
| 310 | + InsConst::C * (recvClockBias - satInfo->satClockBias /* + dt_rel_stc */ + interFreqBias) | ||
| 311 | + pcv; | ||
| 312 | } | ||
| 313 | |||
| 314 | /// @brief Calculates the carrier-phase estimate | ||
| 315 | /// @param[in] freq Signal frequency | ||
| 316 | /// @param[in] freqNum Frequency number. Only used for GLONASS G1 and G2 | ||
| 317 | /// @param[in] receiverType Receiver type to select the correct antenna | ||
| 318 | /// @param[in] e_recvPosMarker Receiver marker position in ECEF coordinates [m] | ||
| 319 | /// @param[in] recvClockBias Receiver clock bias [s] | ||
| 320 | /// @param[in] satInfo Satellite Information (pos, vel, clock) | ||
| 321 | /// @param[in] gnssObs GNSS observation | ||
| 322 | /// @param[in] ionosphericCorrections Ionospheric correction parameters collected from the Nav data | ||
| 323 | /// @param[in] nameId Name and Id of the node used for log messages only | ||
| 324 | /// @return The estimate [m] and the position line-of-sight unit vector (used for the Jacobian) | ||
| 325 | template<typename Derived> | ||
| 326 | std::pair<double, Eigen::Vector3d> | ||
| 327 | calcCarrierEstimate(Frequency freq, | ||
| 328 | int8_t freqNum, | ||
| 329 | size_t receiverType, | ||
| 330 | const Eigen::MatrixBase<Derived>& e_recvPosMarker, | ||
| 331 | double recvClockBias, | ||
| 332 | const std::shared_ptr<const SatelliteInfo>& satInfo, | ||
| 333 | const std::shared_ptr<const GnssObs>& gnssObs, | ||
| 334 | const std::shared_ptr<const IonosphericCorrections>& ionosphericCorrections, | ||
| 335 | [[maybe_unused]] const std::string& nameId) const | ||
| 336 | { | ||
| 337 | Eigen::Vector3d e_recvPosAPC = e_calcRecvPosAPC(freq, receiverType, e_recvPosMarker, gnssObs, nameId); | ||
| 338 | Eigen::Vector3d lla_recvPosAPC = trafo::ecef2lla_WGS84(e_recvPosAPC); | ||
| 339 | |||
| 340 | Eigen::Vector3d e_pLOS = e_calcLineOfSightUnitVector(e_recvPosAPC, satInfo->e_satPos); | ||
| 341 | Eigen::Vector3d n_lineOfSightUnitVector = trafo::n_Quat_e(lla_recvPosAPC(0), lla_recvPosAPC(1)) * e_pLOS; | ||
| 342 | double elevation = calcSatElevation(n_lineOfSightUnitVector); | ||
| 343 | double azimuth = calcSatAzimuth(n_lineOfSightUnitVector); | ||
| 344 | |||
| 345 | // Receiver-Satellite Range [m] | ||
| 346 | double rho_r_s = (satInfo->e_satPos - e_recvPosAPC).norm(); | ||
| 347 | // Estimated troposphere propagation error [m] | ||
| 348 | double dpsr_T_r_s = calculateTroposphericDelay(lla_recvPosAPC, gnssObs, elevation, azimuth, nameId); | ||
| 349 | // Estimated ionosphere propagation error [m] | ||
| 350 | double dpsr_I_r_s = calculateIonosphericDelay(freq, freqNum, lla_recvPosAPC, gnssObs, ionosphericCorrections, elevation, azimuth); | ||
| 351 | // Sagnac correction [m] | ||
| 352 | double dpsr_ie_r_s = calcSagnacCorrection(e_recvPosAPC, satInfo->e_satPos); | ||
| 353 | |||
| 354 | // Earth's gravitational field causes relativistic signal delay due to space-time curvature (Shapiro effect) [s] | ||
| 355 | // double posNorm = e_satPos.norm() + e_recvPosAPC.norm(); | ||
| 356 | // double dt_rel_stc = e_recvPosAPC.norm() > InsConst::WGS84::a / 2.0 ? 2.0 * InsConst::WGS84::MU / std::pow(InsConst::C, 3) * std::log((posNorm + rho_r_s) / (posNorm - rho_r_s)) | ||
| 357 | // : 0.0; | ||
| 358 | |||
| 359 | auto pcv = calcPCV(freq, gnssObs->insTime, elevation, std::nullopt, receiverType, gnssObs, nameId); // TODO: use 'azimuth', but need antenna azimuth | ||
| 360 | |||
| 361 | double estimate = rho_r_s | ||
| 362 | + dpsr_ie_r_s | ||
| 363 | + dpsr_T_r_s | ||
| 364 | - dpsr_I_r_s | ||
| 365 | + InsConst::C * (recvClockBias - satInfo->satClockBias /* + dt_rel_stc */) | ||
| 366 | + pcv; | ||
| 367 | |||
| 368 | return { estimate, e_pLOS }; | ||
| 369 | } | ||
| 370 | |||
| 371 | /// @brief Calculates the doppler estimate | ||
| 372 | /// @param[in] freq Signal frequency | ||
| 373 | /// @param[in] receiverType Receiver type to select the correct antenna | ||
| 374 | /// @param[in] e_recvPosMarker Receiver marker position in ECEF coordinates [m] | ||
| 375 | /// @param[in] e_recvVel Receiver velocity in ECEF frame [m/s] | ||
| 376 | /// @param[in] recvClockDrift Receiver clock drift [s/s] | ||
| 377 | /// @param[in] satInfo Satellite Information (pos, vel, clock) | ||
| 378 | /// @param[in] gnssObs GNSS observation | ||
| 379 | /// @param[in] nameId Name and Id of the node used for log messages only | ||
| 380 | /// @return The estimate [m/s] and the position line-of-sight unit vector (used for the Jacobian) | ||
| 381 | template<typename DerivedPos, typename DerivedVel> | ||
| 382 | auto calcDopplerEstimate(Frequency freq, | ||
| 383 | size_t receiverType, | ||
| 384 | const Eigen::MatrixBase<DerivedPos>& e_recvPosMarker, | ||
| 385 | const Eigen::MatrixBase<DerivedVel>& e_recvVel, | ||
| 386 | const auto& recvClockDrift, | ||
| 387 | const std::shared_ptr<const SatelliteInfo>& satInfo, | ||
| 388 | const std::shared_ptr<const GnssObs>& gnssObs, | ||
| 389 | [[maybe_unused]] const std::string& nameId) const | ||
| 390 | { | ||
| 391 | auto e_recvPosAPC = e_calcRecvPosAPC(freq, receiverType, e_recvPosMarker, gnssObs, nameId); | ||
| 392 | |||
| 393 | auto e_pLOS = e_calcLineOfSightUnitVector(e_recvPosAPC, satInfo->e_satPos); | ||
| 394 | |||
| 395 | return e_pLOS.dot(satInfo->e_satVel - e_recvVel) | ||
| 396 | - calcSagnacRateCorrection(e_recvPosAPC, satInfo->e_satPos, e_recvVel, satInfo->e_satVel) | ||
| 397 | + InsConst::C * (recvClockDrift - satInfo->satClockDrift); | ||
| 398 | } | ||
| 399 | |||
| 400 | /// @brief Calculates the observation variance | ||
| 401 | /// @param[in] freq Signal Frequency | ||
| 402 | /// @param[in] freqNum Frequency number. Only used for GLONASS G1 and G2 | ||
| 403 | /// @param[in] receiverType Receiver type to select the correct antenna | ||
| 404 | /// @param[in] obsType Observation type to calculate the variance for | ||
| 405 | /// @param[in] e_recvPosMarker Receiver marker position in ECEF coordinates [m] | ||
| 406 | /// @param[in] e_satPos Satellite position in ECEF coordinates [m] | ||
| 407 | /// @param[in] cn0 Carrier-to-Noise density [dBHz] | ||
| 408 | /// @param[in] gnssObs GNSS observation | ||
| 409 | /// @param[in] navData Navigation data including this satellite | ||
| 410 | /// @param[in] ionosphericCorrections Ionospheric correction parameters collected from the Nav data | ||
| 411 | /// @param[in] nameId Name and Id of the node used for log messages only | ||
| 412 | /// @param[in] obsDiff Observation Difference type to estimate | ||
| 413 | /// @return Observation variance in [m] (pseudorange, carrier) or [m/s] (doppler) | ||
| 414 | double calcObservationVariance(Frequency freq, | ||
| 415 | int8_t freqNum, | ||
| 416 | size_t receiverType, | ||
| 417 | GnssObs::ObservationType obsType, | ||
| 418 | const Eigen::Vector3d& e_recvPosMarker, | ||
| 419 | const Eigen::Vector3d& e_satPos, | ||
| 420 | double cn0, | ||
| 421 | const std::shared_ptr<const GnssObs>& gnssObs, | ||
| 422 | const std::shared_ptr<const SatNavData>& navData, | ||
| 423 | const std::shared_ptr<const IonosphericCorrections>& ionosphericCorrections, | ||
| 424 | [[maybe_unused]] const std::string& nameId, | ||
| 425 | ObservationDifference obsDiff) const | ||
| 426 | { | ||
| 427 | const SatelliteSystem satSys = freq.getSatSys(); | ||
| 428 | |||
| 429 | Eigen::Vector3d e_recvPosAPC = e_calcRecvPosAPC(freq, receiverType, e_recvPosMarker, gnssObs, nameId); | ||
| 430 | Eigen::Vector3d lla_recvPosAPC = trafo::ecef2lla_WGS84(e_recvPosAPC); | ||
| 431 | |||
| 432 | Eigen::Vector3d e_pLOS = e_calcLineOfSightUnitVector(e_recvPosAPC, e_satPos); | ||
| 433 | Eigen::Vector3d n_lineOfSightUnitVector = trafo::n_Quat_e(lla_recvPosAPC(0), lla_recvPosAPC(1)) * e_pLOS; | ||
| 434 | double elevation = calcSatElevation(n_lineOfSightUnitVector); | ||
| 435 | double azimuth = calcSatAzimuth(n_lineOfSightUnitVector); | ||
| 436 | |||
| 437 | // Estimated troposphere propagation error [m] | ||
| 438 | double dpsr_T_r_s = calculateTroposphericDelay(lla_recvPosAPC, gnssObs, elevation, azimuth, nameId); | ||
| 439 | // Estimated ionosphere propagation error [m] | ||
| 440 | double dpsr_I_r_s = calculateIonosphericDelay(freq, freqNum, lla_recvPosAPC, gnssObs, ionosphericCorrections, elevation, azimuth); | ||
| 441 | |||
| 442 | double variance = 0.0; | ||
| 443 | |||
| 444 | switch (obsType) | ||
| 445 | { | ||
| 446 | case GnssObs::Pseudorange: | ||
| 447 | variance = _gnssMeasurementErrorModel.psrMeasErrorVar(satSys, elevation, cn0); | ||
| 448 | break; | ||
| 449 | case GnssObs::Carrier: | ||
| 450 | variance = _gnssMeasurementErrorModel.carrierMeasErrorVar(satSys, elevation, cn0); | ||
| 451 | break; | ||
| 452 | case GnssObs::Doppler: | ||
| 453 | variance = _gnssMeasurementErrorModel.psrRateMeasErrorVar(freq, freqNum, elevation, cn0); | ||
| 454 | break; | ||
| 455 | case GnssObs::ObservationType_COUNT: | ||
| 456 | break; | ||
| 457 | } | ||
| 458 | |||
| 459 | if (obsDiff == NoDifference) | ||
| 460 | { | ||
| 461 | if (obsType == GnssObs::Pseudorange || obsType == GnssObs::Carrier) | ||
| 462 | { | ||
| 463 | variance += navData->calcSatellitePositionVariance() | ||
| 464 | + ionoErrorVar(dpsr_I_r_s, freq, freqNum) | ||
| 465 | + tropoErrorVar(dpsr_T_r_s, elevation); | ||
| 466 | } | ||
| 467 | if (obsType == GnssObs::Pseudorange) | ||
| 468 | { | ||
| 469 | variance += _gnssMeasurementErrorModel.codeBiasErrorVar(); | ||
| 470 | } | ||
| 471 | } | ||
| 472 | |||
| 473 | return variance; | ||
| 474 | } | ||
| 475 | |||
| 476 | /// @brief Calculates the antenna phase center position for the marker | ||
| 477 | /// @param[in] freq Signal frequency | ||
| 478 | /// @param[in] receiverType Receiver type to select the correct antenna | ||
| 479 | /// @param[in] e_recvPosMarker Receiver marker position in ECEF coordinates [m] | ||
| 480 | /// @param[in] gnssObs GNSS observation | ||
| 481 | /// @param[in] nameId Name and Id of the node used for log messages only | ||
| 482 | template<typename Derived> | ||
| 483 | 34903 | [[nodiscard]] Eigen::Vector3<typename Derived::Scalar> e_calcRecvPosAPC(const Frequency& freq, | |
| 484 | size_t receiverType, | ||
| 485 | const Eigen::MatrixBase<Derived>& e_recvPosMarker, | ||
| 486 | const std::shared_ptr<const GnssObs>& gnssObs, | ||
| 487 | [[maybe_unused]] const std::string& nameId) const | ||
| 488 | { | ||
| 489 | 34903 | const Antenna& antenna = _antenna.at(receiverType); | |
| 490 | |||
| 491 | 34903 | auto e_recvPosAPC = trafo::e_posMarker2ARP(e_recvPosMarker, | |
| 492 | gnssObs, | ||
| 493 | 34903 | antenna.hen_delta); | |
| 494 |
1/2✓ Branch 0 taken 34903 times.
✗ Branch 1 not taken.
|
34903 | if (antenna.correctPCO) |
| 495 | { | ||
| 496 | 34903 | std::string antennaType; | |
| 497 |
3/6✓ Branch 0 taken 34903 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 34903 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 34903 times.
✗ Branch 7 not taken.
|
34903 | if (antenna.autoDetermine && gnssObs->receiverInfo) |
| 498 | { | ||
| 499 |
1/2✓ Branch 4 taken 34903 times.
✗ Branch 5 not taken.
|
34903 | antennaType = gnssObs->receiverInfo->get().antennaType; |
| 500 | } | ||
| 501 | ✗ | else if (!antenna.autoDetermine) | |
| 502 | { | ||
| 503 | ✗ | antennaType = antenna.name; | |
| 504 | } | ||
| 505 |
1/2✓ Branch 1 taken 34903 times.
✗ Branch 2 not taken.
|
34903 | e_recvPosAPC = trafo::e_posARP2APC(e_recvPosAPC, gnssObs, freq, antennaType, nameId); |
| 506 | 34903 | } | |
| 507 | |||
| 508 | 34903 | return e_recvPosAPC; | |
| 509 | } | ||
| 510 | |||
| 511 | /// @brief Calculates the phase center variation | ||
| 512 | /// @param[in] freq Signal frequency | ||
| 513 | /// @param[in] insTime Time to lookup the PCV pattern | ||
| 514 | /// @param[in] elevation Elevation angle in [rad] | ||
| 515 | /// @param[in] azimuth Azimuth in [rad] or nullopt to use the azimuth independent (NOAZI) | ||
| 516 | /// @param[in] receiverType Receiver type to select the correct antenna | ||
| 517 | /// @param[in] gnssObs GNSS observation | ||
| 518 | /// @param[in] nameId Name and Id of the node used for log messages only | ||
| 519 | /// @return The PCV value in [m] | ||
| 520 | template<typename T> | ||
| 521 | 34903 | [[nodiscard]] T calcPCV(const Frequency& freq, | |
| 522 | const InsTime& insTime, | ||
| 523 | const T& elevation, | ||
| 524 | std::optional<double> azimuth, | ||
| 525 | size_t receiverType, | ||
| 526 | const std::shared_ptr<const GnssObs>& gnssObs, | ||
| 527 | [[maybe_unused]] const std::string& nameId) const | ||
| 528 | { | ||
| 529 | 34903 | if (std::isnan(elevation) | |
| 530 |
3/8✓ Branch 0 taken 34903 times.
✗ Branch 1 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 34903 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 34903 times.
|
34903 | || (azimuth.has_value() && std::isnan(*azimuth))) { return T(0.0); } |
| 531 | 34903 | const Antenna& antenna = _antenna.at(receiverType); | |
| 532 |
1/2✓ Branch 0 taken 34903 times.
✗ Branch 1 not taken.
|
34903 | if (antenna.correctPCV) |
| 533 | { | ||
| 534 | 34903 | std::string antennaType; | |
| 535 |
3/6✓ Branch 0 taken 34903 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 34903 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 34903 times.
✗ Branch 7 not taken.
|
34903 | if (antenna.autoDetermine && gnssObs->receiverInfo) |
| 536 | { | ||
| 537 |
1/2✓ Branch 4 taken 34903 times.
✗ Branch 5 not taken.
|
34903 | antennaType = gnssObs->receiverInfo->get().antennaType; |
| 538 | } | ||
| 539 | ✗ | else if (!antenna.autoDetermine) | |
| 540 | { | ||
| 541 | ✗ | antennaType = antenna.name; | |
| 542 | } | ||
| 543 |
1/2✓ Branch 3 taken 34903 times.
✗ Branch 4 not taken.
|
34903 | return AntexReader::Get().getAntennaPhaseCenterVariation(antennaType, |
| 544 | Frequency_(freq), | ||
| 545 | insTime, | ||
| 546 | elevation, | ||
| 547 | azimuth, | ||
| 548 | nameId) | ||
| 549 | 34903 | .value_or(T(0.0)); | |
| 550 | 34903 | } | |
| 551 | |||
| 552 | ✗ | return T(0.0); | |
| 553 | } | ||
| 554 | |||
| 555 | /// @brief Calculate the ionospheric delay with the model selected in the GUI | ||
| 556 | /// @param[in] freq Frequency of the signal | ||
| 557 | /// @param[in] freqNum Frequency number. Only used for GLONASS G1 and G2 | ||
| 558 | /// @param[in] lla_recvPosAPC Receiver antenna phase center position in LLA coordinates [rad, rad, m] | ||
| 559 | /// @param[in] gnssObs GNSS observation | ||
| 560 | /// @param[in] ionosphericCorrections Ionospheric correction parameters collected from the Nav data | ||
| 561 | /// @param[in] elevation Satellite elevation [rad] | ||
| 562 | /// @param[in] azimuth Satellite azimuth [rad] | ||
| 563 | /// @return Estimated ionosphere propagation error [m] | ||
| 564 | double calculateIonosphericDelay(const Frequency& freq, | ||
| 565 | int8_t freqNum, | ||
| 566 | const Eigen::Vector3d& lla_recvPosAPC, | ||
| 567 | const std::shared_ptr<const GnssObs>& gnssObs, | ||
| 568 | const std::shared_ptr<const IonosphericCorrections>& ionosphericCorrections, | ||
| 569 | double elevation, | ||
| 570 | double azimuth) const | ||
| 571 | { | ||
| 572 | return calcIonosphericDelay(static_cast<double>(gnssObs->insTime.toGPSweekTow().tow), | ||
| 573 | freq, freqNum, lla_recvPosAPC, | ||
| 574 | elevation, azimuth, | ||
| 575 | _ionosphereModel, ionosphericCorrections.get()); | ||
| 576 | } | ||
| 577 | |||
| 578 | /// @brief Calculate the tropospheric delay with the model selected in the GUI | ||
| 579 | /// @param[in] lla_recvPosAPC Receiver antenna phase center position in LLA coordinates [rad, rad, m] | ||
| 580 | /// @param[in] gnssObs GNSS observation | ||
| 581 | /// @param[in] elevation Satellite elevation [rad] | ||
| 582 | /// @param[in] azimuth Satellite azimuth [rad] | ||
| 583 | /// @param[in] nameId Name and Id of the node used for log messages only | ||
| 584 | /// @return Estimated troposphere propagation error [m] | ||
| 585 | double calculateTroposphericDelay(const Eigen::Vector3d& lla_recvPosAPC, | ||
| 586 | const std::shared_ptr<const GnssObs>& gnssObs, | ||
| 587 | double elevation, | ||
| 588 | double azimuth, | ||
| 589 | [[maybe_unused]] const std::string& nameId) const | ||
| 590 | { | ||
| 591 | auto tropo_r_s = calcTroposphericDelayAndMapping(gnssObs->insTime, lla_recvPosAPC, | ||
| 592 | elevation, azimuth, _troposphereModels, nameId); | ||
| 593 | |||
| 594 | return tropo_r_s.ZHD * tropo_r_s.zhdMappingFactor + tropo_r_s.ZWD * tropo_r_s.zwdMappingFactor; | ||
| 595 | } | ||
| 596 | |||
| 597 | /// @brief Shows the GUI input to select the options | ||
| 598 | /// @param[in] id Unique id for ImGui. | ||
| 599 | /// @param[in] itemWidth Width of the widgets | ||
| 600 | template<typename ReceiverType> | ||
| 601 | ✗ | bool ShowGuiWidgets(const char* id, float itemWidth) | |
| 602 | { | ||
| 603 | ✗ | bool changed = false; | |
| 604 | |||
| 605 | ✗ | ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); | |
| 606 | ✗ | if (ImGui::TreeNode(fmt::format("Compensation models##{}", id).c_str())) | |
| 607 | { | ||
| 608 | ✗ | ImGui::SetNextItemWidth(itemWidth - ImGui::GetStyle().IndentSpacing); | |
| 609 | ✗ | if (ComboIonosphereModel(fmt::format("Ionosphere Model##{}", id).c_str(), _ionosphereModel)) | |
| 610 | { | ||
| 611 | ✗ | LOG_DEBUG("{}: Ionosphere Model changed to {}", id, NAV::to_string(_ionosphereModel)); | |
| 612 | ✗ | changed = true; | |
| 613 | } | ||
| 614 | ✗ | if (ComboTroposphereModel(fmt::format("Troposphere Model##{}", id).c_str(), _troposphereModels, itemWidth - ImGui::GetStyle().IndentSpacing)) | |
| 615 | { | ||
| 616 | ✗ | changed = true; | |
| 617 | } | ||
| 618 | ✗ | ImGui::TreePop(); | |
| 619 | } | ||
| 620 | |||
| 621 | ✗ | ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); | |
| 622 | ✗ | if (ImGui::TreeNode(fmt::format("GNSS Measurement Error##{}", id).c_str())) | |
| 623 | { | ||
| 624 | ✗ | if (_gnssMeasurementErrorModel.ShowGuiWidgets(id, itemWidth - ImGui::GetStyle().IndentSpacing)) | |
| 625 | { | ||
| 626 | ✗ | LOG_DEBUG("{}: GNSS Measurement Error Model changed.", id); | |
| 627 | ✗ | changed = true; | |
| 628 | } | ||
| 629 | ✗ | ImGui::TreePop(); | |
| 630 | } | ||
| 631 | |||
| 632 | ✗ | if (ImGui::TreeNode(fmt::format("Antenna settings##{}", id).c_str())) | |
| 633 | { | ||
| 634 | ✗ | for (size_t i = 0; i < _receiverCount; ++i) | |
| 635 | { | ||
| 636 | ✗ | if (_receiverCount > 1) | |
| 637 | { | ||
| 638 | ✗ | ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); | |
| 639 | ✗ | if (!ImGui::TreeNode(fmt::format("{}", static_cast<ReceiverType>(i)).c_str())) { continue; } | |
| 640 | } | ||
| 641 | |||
| 642 | ✗ | if (ImGui::Checkbox(fmt::format("Correct Phase-center offset##antenna {} {}", id, i).c_str(), &_antenna.at(i).correctPCO)) | |
| 643 | { | ||
| 644 | ✗ | LOG_DEBUG("{}: Antenna {} correctPCO: {}", id, i, _antenna.at(i).correctPCO); | |
| 645 | ✗ | changed = true; | |
| 646 | } | ||
| 647 | ✗ | ImGui::SameLine(); | |
| 648 | ✗ | if (ImGui::Checkbox(fmt::format("Correct Phase-center variation##antenna {} {}", id, i).c_str(), &_antenna.at(i).correctPCV)) | |
| 649 | { | ||
| 650 | ✗ | LOG_DEBUG("{}: Antenna {} correctPCV: {}", id, i, _antenna.at(i).correctPCV); | |
| 651 | ✗ | changed = true; | |
| 652 | } | ||
| 653 | |||
| 654 | ✗ | if (!_antenna.at(i).correctPCO && !_antenna.at(i).correctPCV) { ImGui::BeginDisabled(); } | |
| 655 | ✗ | if (ImGui::Checkbox(fmt::format("Auto determine##antenna {} {}", id, i).c_str(), &_antenna.at(i).autoDetermine)) | |
| 656 | { | ||
| 657 | ✗ | LOG_DEBUG("{}: Antenna {} auto: {}", id, i, _antenna.at(i).autoDetermine); | |
| 658 | ✗ | changed = true; | |
| 659 | } | ||
| 660 | ✗ | ImGui::SameLine(); | |
| 661 | ✗ | if (_antenna.at(i).autoDetermine) { ImGui::BeginDisabled(); } | |
| 662 | ✗ | ImGui::SetNextItemWidth(itemWidth - (1.0F + static_cast<float>(_receiverCount > 1)) * ImGui::GetStyle().IndentSpacing); | |
| 663 | |||
| 664 | ✗ | if (ImGui::BeginCombo(fmt::format("Type##Combo {} {}", id, i).c_str(), _antenna.at(i).name.c_str())) | |
| 665 | { | ||
| 666 | ✗ | ImGui::PushFont(Application::MonoFont()); | |
| 667 | ✗ | if (ImGui::Selectable(fmt::format("##Selectable {} {}", id, i).c_str(), _antenna.at(i).name.empty())) | |
| 668 | { | ||
| 669 | ✗ | _antenna.at(i).name = ""; | |
| 670 | } | ||
| 671 | |||
| 672 | ✗ | for (const auto& iter : AntexReader::Get().antennas()) | |
| 673 | { | ||
| 674 | ✗ | const bool is_selected = (_antenna.at(i).name == iter); | |
| 675 | ✗ | if (ImGui::Selectable(iter.c_str(), is_selected)) | |
| 676 | { | ||
| 677 | ✗ | _antenna.at(i).name = iter; | |
| 678 | } | ||
| 679 | ✗ | if (is_selected) { ImGui::SetItemDefaultFocus(); } | |
| 680 | } | ||
| 681 | ✗ | ImGui::PopFont(); | |
| 682 | ✗ | ImGui::EndCombo(); | |
| 683 | } | ||
| 684 | ✗ | if (_antenna.at(i).autoDetermine) { ImGui::EndDisabled(); } | |
| 685 | ✗ | if (!_antenna.at(i).correctPCO && !_antenna.at(i).correctPCV) { ImGui::EndDisabled(); } | |
| 686 | ✗ | ImGui::SameLine(); | |
| 687 | ✗ | gui::widgets::HelpMarker("Used for lookup in ANTEX file."); | |
| 688 | |||
| 689 | ✗ | ImGui::SetNextItemWidth(itemWidth - (1.0F + static_cast<float>(_receiverCount > 1)) * ImGui::GetStyle().IndentSpacing); | |
| 690 | ✗ | if (ImGui::InputDouble3(fmt::format("Delta H/E/N [m]##{} {}", id, i).c_str(), _antenna.at(i).hen_delta.data(), "%.3f")) | |
| 691 | { | ||
| 692 | ✗ | LOG_DEBUG("{}: Antenna {} delta: {}", id, i, _antenna.at(i).hen_delta.transpose()); | |
| 693 | ✗ | changed = true; | |
| 694 | } | ||
| 695 | ✗ | ImGui::SameLine(); | |
| 696 | ✗ | gui::widgets::HelpMarker("- Antenna height: Height of the antenna reference point (ARP) above the marker\n" | |
| 697 | "- Horizontal eccentricity of ARP relative to the marker (east/north)"); | ||
| 698 | |||
| 699 | ✗ | if (_receiverCount > 1) { ImGui::TreePop(); } | |
| 700 | } | ||
| 701 | ✗ | ImGui::TreePop(); | |
| 702 | } | ||
| 703 | |||
| 704 | ✗ | return changed; | |
| 705 | } | ||
| 706 | |||
| 707 | /// @brief Get the Ionosphere Model selected | ||
| 708 | [[nodiscard]] const IonosphereModel& getIonosphereModel() const { return _ionosphereModel; } | ||
| 709 | /// @brief Get the Troposphere Model selection | ||
| 710 | [[nodiscard]] const TroposphereModelSelection& getTroposphereModels() const { return _troposphereModels; } | ||
| 711 | /// @brief Get the GNSS measurement error model | ||
| 712 | [[nodiscard]] const GnssMeasurementErrorModel& getGnssMeasurementErrorModel() const { return _gnssMeasurementErrorModel; } | ||
| 713 | |||
| 714 | private: | ||
| 715 | IonosphereModel _ionosphereModel = IonosphereModel::Klobuchar; ///< Ionosphere Model used for the calculation | ||
| 716 | TroposphereModelSelection _troposphereModels; ///< Troposphere Models used for the calculation | ||
| 717 | GnssMeasurementErrorModel _gnssMeasurementErrorModel; ///< GNSS measurement error model to use | ||
| 718 | |||
| 719 | size_t _receiverCount; ///< Number of receivers (used for GUI) | ||
| 720 | |||
| 721 | /// Antenna information | ||
| 722 | struct Antenna | ||
| 723 | { | ||
| 724 | bool correctPCO = true; ///< Correct phase center offset | ||
| 725 | bool correctPCV = true; ///< Correct phase center variation | ||
| 726 | bool autoDetermine = true; ///< Try determine antenna type from e.g. RINEX file | ||
| 727 | std::string name; ///< Type of the antenna for the Antex lookup | ||
| 728 | Eigen::Vector3d hen_delta = Eigen::Vector3d::Zero(); ///< Delta values of marker to antenna base | ||
| 729 | }; | ||
| 730 | |||
| 731 | std::unordered_map<size_t, Antenna> _antenna; ///< User antenna selection. Key is receiver type | ||
| 732 | |||
| 733 | /// @brief Converts the provided object into json | ||
| 734 | /// @param[out] j Json object which gets filled with the info | ||
| 735 | /// @param[in] obj Object to convert into json | ||
| 736 | ✗ | friend void to_json(json& j, const ObservationEstimator::Antenna& obj) | |
| 737 | { | ||
| 738 | ✗ | j = json{ | |
| 739 | ✗ | { "correctPCO", obj.correctPCO }, | |
| 740 | ✗ | { "correctPCV", obj.correctPCV }, | |
| 741 | ✗ | { "autoDetermine", obj.autoDetermine }, | |
| 742 | ✗ | { "name", obj.name }, | |
| 743 | ✗ | { "hen_delta", obj.hen_delta }, | |
| 744 | ✗ | }; | |
| 745 | ✗ | } | |
| 746 | /// @brief Converts the provided json object into a node object | ||
| 747 | /// @param[in] j Json object with the needed values | ||
| 748 | /// @param[out] obj Object to fill from the json | ||
| 749 | 6 | friend void from_json(const json& j, ObservationEstimator::Antenna& obj) | |
| 750 | { | ||
| 751 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
|
6 | if (j.contains("correctPCO")) { j.at("correctPCO").get_to(obj.correctPCO); } |
| 752 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
|
6 | if (j.contains("correctPCV")) { j.at("correctPCV").get_to(obj.correctPCV); } |
| 753 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("autoDetermine")) { j.at("autoDetermine").get_to(obj.autoDetermine); } |
| 754 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("name")) { j.at("name").get_to(obj.name); } |
| 755 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("hen_delta")) { j.at("hen_delta").get_to(obj.hen_delta); } |
| 756 | 6 | } | |
| 757 | |||
| 758 | /// @brief Converts the provided object into json | ||
| 759 | /// @param[out] j Json object which gets filled with the info | ||
| 760 | /// @param[in] obj Object to convert into json | ||
| 761 | ✗ | friend void to_json(json& j, const ObservationEstimator& obj) | |
| 762 | { | ||
| 763 | ✗ | j = json{ | |
| 764 | ✗ | { "ionosphereModel", obj._ionosphereModel }, | |
| 765 | ✗ | { "troposphereModels", obj._troposphereModels }, | |
| 766 | ✗ | { "gnssMeasurementError", obj._gnssMeasurementErrorModel }, | |
| 767 | ✗ | { "antenna", obj._antenna }, | |
| 768 | ✗ | }; | |
| 769 | ✗ | } | |
| 770 | /// @brief Converts the provided json object into a node object | ||
| 771 | /// @param[in] j Json object with the needed values | ||
| 772 | /// @param[out] obj Object to fill from the json | ||
| 773 | 7 | friend void from_json(const json& j, ObservationEstimator& obj) | |
| 774 | { | ||
| 775 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("ionosphereModel")) { j.at("ionosphereModel").get_to(obj._ionosphereModel); } |
| 776 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("troposphereModels")) { j.at("troposphereModels").get_to(obj._troposphereModels); } |
| 777 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if (j.contains("gnssMeasurementError")) { j.at("gnssMeasurementError").get_to(obj._gnssMeasurementErrorModel); } |
| 778 |
2/2✓ Branch 1 taken 6 times.
✓ Branch 2 taken 1 times.
|
7 | if (j.contains("antenna")) { j.at("antenna").get_to(obj._antenna); } |
| 779 | 7 | } | |
| 780 | }; | ||
| 781 | |||
| 782 | } // namespace NAV | ||
| 783 |