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