| 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 RtkSolution.hpp | ||
| 10 | /// @brief RTK Node/Algorithm output | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2022-05-28 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <cstddef> | ||
| 17 | #include <string> | ||
| 18 | #include <vector> | ||
| 19 | #include "Navigation/GNSS/Ambiguity/AmbiguityResolution.hpp" | ||
| 20 | #include "Navigation/GNSS/Positioning/ObservationFilter.hpp" | ||
| 21 | #include "util/Assert.h" | ||
| 22 | #include "NodeData/GNSS/GnssObs.hpp" | ||
| 23 | #include "NodeData/State/PosVel.hpp" | ||
| 24 | |||
| 25 | #include "Navigation/GNSS/Core/SatelliteIdentifier.hpp" | ||
| 26 | #include "Navigation/GNSS/Core/Code.hpp" | ||
| 27 | #include "Navigation/GNSS/Positioning/RTK/Keys.hpp" | ||
| 28 | #include "Navigation/GNSS/Ambiguity/CycleSlipDetector.hpp" | ||
| 29 | #include "Navigation/Math/KeyedKalmanFilter.hpp" | ||
| 30 | #include "Navigation/Transformations/Units.hpp" | ||
| 31 | #include "util/Container/UncertainValue.hpp" | ||
| 32 | #include "util/Container/KeyedMatrix.hpp" | ||
| 33 | |||
| 34 | namespace NAV | ||
| 35 | { | ||
| 36 | /// SPP Algorithm output | ||
| 37 | class RtkSolution : public PosVel | ||
| 38 | { | ||
| 39 | public: | ||
| 40 | /// @brief Returns the type of the data class | ||
| 41 | /// @return The data type | ||
| 42 | 22524 | [[nodiscard]] static std::string type() | |
| 43 | { | ||
| 44 |
1/2✓ Branch 1 taken 22524 times.
✗ Branch 2 not taken.
|
45048 | return "RtkSolution"; |
| 45 | } | ||
| 46 | |||
| 47 | /// @brief Returns the type of the data class | ||
| 48 | /// @return The data type | ||
| 49 | ✗ | [[nodiscard]] std::string getType() const override { return type(); } | |
| 50 | |||
| 51 | /// @brief Returns the parent types of the data class | ||
| 52 | /// @return The parent data types | ||
| 53 | 114 | [[nodiscard]] static std::vector<std::string> parentTypes() | |
| 54 | { | ||
| 55 | 114 | auto parent = PosVel::parentTypes(); | |
| 56 |
2/4✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
|
114 | parent.push_back(PosVel::type()); |
| 57 | 114 | return parent; | |
| 58 | ✗ | } | |
| 59 | |||
| 60 | /// @brief Returns a vector of data descriptors | ||
| 61 | 7213 | [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors() | |
| 62 | { | ||
| 63 | 7213 | auto desc = PosVel::GetStaticDataDescriptors(); | |
| 64 |
1/2✓ Branch 2 taken 7213 times.
✗ Branch 3 not taken.
|
7213 | desc.reserve(GetStaticDescriptorCount()); |
| 65 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("Solution Type"); |
| 66 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("Number satellites"); |
| 67 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("Number pseudorange observables"); |
| 68 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("Number carrier observables"); |
| 69 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("Number doppler observables"); |
| 70 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("Number pseudorange observables (unique per satellite)"); |
| 71 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("Number carrier observables (unique per satellite)"); |
| 72 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("Number doppler observables (unique per satellite)"); |
| 73 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("Ambiguity Resolution Failure"); |
| 74 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("Ambiguity Critical Value µ ∈ (0, 1] (R1/R2 ≤ µ)"); |
| 75 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("Number of Ambiguities fixed"); |
| 76 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("NIS Triggered (Initial)"); |
| 77 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("NIS value (Initial)"); |
| 78 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("NIS r2 upper boundary (Initial)"); |
| 79 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("NIS removed observations"); |
| 80 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("NIS Triggered (Final)"); |
| 81 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("NIS value (Final)"); |
| 82 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("NIS r2 upper boundary (Final)"); |
| 83 |
1/2✓ Branch 1 taken 7213 times.
✗ Branch 2 not taken.
|
7213 | desc.emplace_back("Distance Rover-Base [m]"); |
| 84 | 7213 | return desc; | |
| 85 | ✗ | } | |
| 86 | |||
| 87 | /// @brief Get the amount of descriptors | ||
| 88 | 14425 | [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return PosVel::GetStaticDescriptorCount() + 19; } | |
| 89 | |||
| 90 | /// @brief Returns a vector of data descriptors | ||
| 91 | ✗ | [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); } | |
| 92 | |||
| 93 | /// @brief Get the amount of descriptors | ||
| 94 | 3606 | [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); } | |
| 95 | |||
| 96 | /// @brief Get the value at the index | ||
| 97 | /// @param idx Index corresponding to data descriptor order | ||
| 98 | /// @return Value if in the observation | ||
| 99 | 3606 | [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override | |
| 100 | { | ||
| 101 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 3606 times.
|
3606 | INS_ASSERT(idx < GetStaticDescriptorCount()); |
| 102 |
1/2✓ Branch 1 taken 3606 times.
✗ Branch 2 not taken.
|
3606 | if (idx < PosVel::GetStaticDescriptorCount()) { return PosVel::getValueAt(idx); } |
| 103 | ✗ | switch (idx) | |
| 104 | { | ||
| 105 | ✗ | case PosVel::GetStaticDescriptorCount() + 0: // Solution Type | |
| 106 | ✗ | return static_cast<double>(solType); | |
| 107 | ✗ | case PosVel::GetStaticDescriptorCount() + 1: // Number satellites | |
| 108 | ✗ | return static_cast<double>(nSatellites); | |
| 109 | ✗ | case PosVel::GetStaticDescriptorCount() + 2: // Number pseudorange observables | |
| 110 | ✗ | if (nObservations.contains(GnssObs::Pseudorange)) { return static_cast<double>(nObservations.at(GnssObs::Pseudorange)); } | |
| 111 | ✗ | break; | |
| 112 | ✗ | case PosVel::GetStaticDescriptorCount() + 3: // Number carrier observables | |
| 113 | ✗ | if (nObservations.contains(GnssObs::Carrier)) { return static_cast<double>(nObservations.at(GnssObs::Carrier)); } | |
| 114 | ✗ | break; | |
| 115 | ✗ | case PosVel::GetStaticDescriptorCount() + 4: // Number doppler observables | |
| 116 | ✗ | if (nObservations.contains(GnssObs::Doppler)) { return static_cast<double>(nObservations.at(GnssObs::Doppler)); } | |
| 117 | ✗ | break; | |
| 118 | ✗ | case PosVel::GetStaticDescriptorCount() + 5: // Number pseudorange observables unique satellite | |
| 119 | ✗ | if (nObservationsUniqueSatellite.contains(GnssObs::Pseudorange)) { return static_cast<double>(nObservationsUniqueSatellite.at(GnssObs::Pseudorange)); } | |
| 120 | ✗ | break; | |
| 121 | ✗ | case PosVel::GetStaticDescriptorCount() + 6: // Number carrier observables unique satellite | |
| 122 | ✗ | if (nObservationsUniqueSatellite.contains(GnssObs::Carrier)) { return static_cast<double>(nObservationsUniqueSatellite.at(GnssObs::Carrier)); } | |
| 123 | ✗ | break; | |
| 124 | ✗ | case PosVel::GetStaticDescriptorCount() + 7: // Number doppler observables unique satellite | |
| 125 | ✗ | if (nObservationsUniqueSatellite.contains(GnssObs::Doppler)) { return static_cast<double>(nObservationsUniqueSatellite.at(GnssObs::Doppler)); } | |
| 126 | ✗ | break; | |
| 127 | ✗ | case PosVel::GetStaticDescriptorCount() + 8: // Ambiguity Resolution Failure | |
| 128 | ✗ | return static_cast<double>(ambiguityResolutionFailure); | |
| 129 | ✗ | case PosVel::GetStaticDescriptorCount() + 9: // Ambiguity Critical Value µ ∈ (0, 1] (R1/R2 ≤ µ) | |
| 130 | ✗ | return ambiguityCriticalValueRatio; | |
| 131 | ✗ | case PosVel::GetStaticDescriptorCount() + 10: // Number of Ambiguities fixed | |
| 132 | ✗ | if (nAmbiguitiesFixed) { return static_cast<double>(nAmbiguitiesFixed.value()); } | |
| 133 | ✗ | break; | |
| 134 | ✗ | case PosVel::GetStaticDescriptorCount() + 11: // NIS Triggered (Initial) | |
| 135 | ✗ | if (nisResultInitial) { return static_cast<double>(nisResultInitial->triggered); } | |
| 136 | ✗ | break; | |
| 137 | ✗ | case PosVel::GetStaticDescriptorCount() + 12: // NIS value (Initial) | |
| 138 | ✗ | if (nisResultInitial) { return nisResultInitial->NIS; } | |
| 139 | ✗ | break; | |
| 140 | ✗ | case PosVel::GetStaticDescriptorCount() + 13: // NIS r2 upper boundary (Initial) | |
| 141 | ✗ | if (nisResultInitial) { return nisResultInitial->r2; } | |
| 142 | ✗ | break; | |
| 143 | ✗ | case PosVel::GetStaticDescriptorCount() + 14: // NIS removed observations | |
| 144 | ✗ | return static_cast<double>(nisRemovedCnt); | |
| 145 | ✗ | case PosVel::GetStaticDescriptorCount() + 15: // NIS Triggered (Final) | |
| 146 | ✗ | if (nisResultFinal) { return static_cast<double>(nisResultFinal->triggered); } | |
| 147 | ✗ | break; | |
| 148 | ✗ | case PosVel::GetStaticDescriptorCount() + 16: // NIS value (Final) | |
| 149 | ✗ | if (nisResultFinal) { return nisResultFinal->NIS; } | |
| 150 | ✗ | break; | |
| 151 | ✗ | case PosVel::GetStaticDescriptorCount() + 17: // NIS r2 upper boundary (Final) | |
| 152 | ✗ | if (nisResultFinal) { return nisResultFinal->r2; } | |
| 153 | ✗ | break; | |
| 154 | ✗ | case PosVel::GetStaticDescriptorCount() + 18: // Distance Rover-Base [m] | |
| 155 | ✗ | return distanceBaseRover; | |
| 156 | ✗ | default: | |
| 157 | ✗ | return std::nullopt; | |
| 158 | } | ||
| 159 | ✗ | return std::nullopt; | |
| 160 | } | ||
| 161 | |||
| 162 | /// @brief Returns a vector of data descriptors for the dynamic data | ||
| 163 | 601 | [[nodiscard]] std::vector<std::string> dynamicDataDescriptors() const override | |
| 164 | { | ||
| 165 | 601 | std::vector<std::string> descriptors; | |
| 166 |
1/2✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
|
601 | descriptors.reserve(ambiguityDD_br.size() * 2 + static_cast<size_t>(measInnovation.rows())); |
| 167 | |||
| 168 |
2/2✓ Branch 4 taken 23579 times.
✓ Branch 5 taken 601 times.
|
24180 | for (const auto& ambDD : ambiguityDD_br) |
| 169 | { | ||
| 170 |
1/2✓ Branch 2 taken 23579 times.
✗ Branch 3 not taken.
|
47158 | descriptors.push_back(fmt::format("AmbDD {} [cycles]", ambDD.satSigId)); |
| 171 |
1/2✓ Branch 2 taken 23579 times.
✗ Branch 3 not taken.
|
47158 | descriptors.push_back(fmt::format("AmbDD StDev {} [cycles]", ambDD.satSigId)); |
| 172 | } | ||
| 173 | |||
| 174 |
2/2✓ Branch 6 taken 23579 times.
✓ Branch 7 taken 601 times.
|
24180 | for (const auto& key : measInnovation.rowKeys()) |
| 175 | { | ||
| 176 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 23579 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
23579 | if (std::holds_alternative<RTK::Meas::PsrDD>(key)) { descriptors.push_back(fmt::format("Innovation {} [m]", key)); } |
| 177 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 23579 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
23579 | else if (std::holds_alternative<RTK::Meas::CarrierDD>(key)) { descriptors.push_back(fmt::format("Innovation {} [m]", key)); } |
| 178 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 23579 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
23579 | else if (std::holds_alternative<RTK::Meas::DopplerDD>(key)) { descriptors.push_back(fmt::format("Innovation {} [m/s]", key)); } |
| 179 |
2/4✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23579 times.
✗ Branch 6 not taken.
|
47158 | else if (std::holds_alternative<RTK::States::AmbiguityDD>(key)) { descriptors.push_back(fmt::format("Innovation {} [cyc]", key)); } |
| 180 | } | ||
| 181 |
2/2✓ Branch 6 taken 12020 times.
✓ Branch 7 taken 601 times.
|
12621 | for (const auto& [satId, satData] : satData) |
| 182 | { | ||
| 183 |
1/2✓ Branch 2 taken 12020 times.
✗ Branch 3 not taken.
|
24040 | descriptors.push_back(fmt::format("{} Elevation [deg]", satId)); |
| 184 |
1/2✓ Branch 2 taken 12020 times.
✗ Branch 3 not taken.
|
24040 | descriptors.push_back(fmt::format("{} Azimuth [deg]", satId)); |
| 185 | } | ||
| 186 | |||
| 187 | 601 | return descriptors; | |
| 188 | ✗ | } | |
| 189 | |||
| 190 | /// @brief Get the value for the descriptor | ||
| 191 | /// @return Value if in the observation | ||
| 192 | ✗ | [[nodiscard]] std::optional<double> getDynamicDataAt(const std::string& descriptor) const override | |
| 193 | { | ||
| 194 | ✗ | for (const auto& ambDD : ambiguityDD_br) | |
| 195 | { | ||
| 196 | ✗ | if (descriptor == fmt::format("AmbDD {} [cycles]", ambDD.satSigId)) { return ambDD.value.value; } | |
| 197 | ✗ | if (descriptor == fmt::format("AmbDD StDev {} [cycles]", ambDD.satSigId)) { return ambDD.value.stdDev; } | |
| 198 | } | ||
| 199 | ✗ | for (const auto& key : measInnovation.rowKeys()) | |
| 200 | { | ||
| 201 | ✗ | if (descriptor.starts_with(fmt::format("Innovation {}", key))) { return measInnovation(key); } | |
| 202 | } | ||
| 203 | ✗ | for (const auto& [satId, satData] : satData) | |
| 204 | { | ||
| 205 | ✗ | if (descriptor == fmt::format("{} Elevation [deg]", satId)) { return rad2deg(satData.satElevation); } | |
| 206 | ✗ | if (descriptor == fmt::format("{} Azimuth [deg]", satId)) { return rad2deg(satData.satAzimuth); } | |
| 207 | } | ||
| 208 | ✗ | return std::nullopt; | |
| 209 | } | ||
| 210 | |||
| 211 | /// @brief Returns a vector of data descriptors and values for the dynamic data | ||
| 212 | ✗ | [[nodiscard]] std::vector<std::pair<std::string, double>> getDynamicData() const override | |
| 213 | { | ||
| 214 | ✗ | std::vector<std::pair<std::string, double>> dynData; | |
| 215 | ✗ | dynData.reserve(ambiguityDD_br.size() * 2 + static_cast<size_t>(measInnovation.rows())); | |
| 216 | ✗ | for (const auto& ambDD : ambiguityDD_br) | |
| 217 | { | ||
| 218 | ✗ | dynData.emplace_back(fmt::format("AmbDD {} [cycles]", ambDD.satSigId), ambDD.value.value); | |
| 219 | ✗ | dynData.emplace_back(fmt::format("AmbDD StDev {} [cycles]", ambDD.satSigId), ambDD.value.stdDev); | |
| 220 | } | ||
| 221 | ✗ | for (const auto& key : measInnovation.rowKeys()) | |
| 222 | { | ||
| 223 | ✗ | if (std::holds_alternative<RTK::Meas::PsrDD>(key)) { dynData.emplace_back(fmt::format("Innovation {} [m]", key), measInnovation(key)); } | |
| 224 | ✗ | else if (std::holds_alternative<RTK::Meas::CarrierDD>(key)) { dynData.emplace_back(fmt::format("Innovation {} [m]", key), measInnovation(key)); } | |
| 225 | ✗ | else if (std::holds_alternative<RTK::Meas::DopplerDD>(key)) { dynData.emplace_back(fmt::format("Innovation {} [m/s]", key), measInnovation(key)); } | |
| 226 | ✗ | else if (std::holds_alternative<RTK::States::AmbiguityDD>(key)) { dynData.emplace_back(fmt::format("Innovation {} [cyc]", key), measInnovation(key)); } | |
| 227 | } | ||
| 228 | ✗ | for (const auto& [satId, satData] : satData) | |
| 229 | { | ||
| 230 | ✗ | dynData.emplace_back(fmt::format("{} Elevation [deg]", satId), rad2deg(satData.satElevation)); | |
| 231 | ✗ | dynData.emplace_back(fmt::format("{} Azimuth [deg]", satId), rad2deg(satData.satAzimuth)); | |
| 232 | } | ||
| 233 | ✗ | return dynData; | |
| 234 | ✗ | } | |
| 235 | |||
| 236 | /// @brief Shows a GUI tooltip to look into details of the observation | ||
| 237 | /// @param[in] detailView Flag to show the detailed view | ||
| 238 | /// @param[in] firstOpen Flag whether the tooltip is opened once | ||
| 239 | /// @param[in] displayName Data identifier, can be used in dynamic data to identify the correct data | ||
| 240 | /// @param[in] id Unique identifier | ||
| 241 | /// @param[in] rootWindow Pointer to the root window opening the tooltip | ||
| 242 | void guiTooltip(bool detailView, bool firstOpen, const char* displayName, const char* id, int* rootWindow) const override; | ||
| 243 | |||
| 244 | /// @brief Return whether this data has a tooltip | ||
| 245 | ✗ | [[nodiscard]] bool hasTooltip() const override { return true; } | |
| 246 | |||
| 247 | // --------------------------------------------------------- Public Members ------------------------------------------------------------ | ||
| 248 | |||
| 249 | /// Possible types of the RTK solution | ||
| 250 | enum class SolutionType : uint8_t | ||
| 251 | { | ||
| 252 | None, ///< No solution type specified | ||
| 253 | SPP, ///< Solution calculated via SPP algorithm because of missing data for RTK | ||
| 254 | Predicted, ///< Only predicted by Kalman Filter | ||
| 255 | RTK_Float, ///< RTK solution with floating point ambiguities | ||
| 256 | RTK_Fixed, ///< RTK solution with fixed ambiguities to integers | ||
| 257 | }; | ||
| 258 | |||
| 259 | /// Type of th solution | ||
| 260 | SolutionType solType = SolutionType::None; | ||
| 261 | |||
| 262 | /// Amount of satellites used | ||
| 263 | size_t nSatellites = 0; | ||
| 264 | |||
| 265 | /// Distance of Rover to base [m] | ||
| 266 | double distanceBaseRover = 0.0; | ||
| 267 | |||
| 268 | /// Time of the base observation used | ||
| 269 | InsTime baseTime; | ||
| 270 | |||
| 271 | std::unordered_map<GnssObs::ObservationType, size_t> nObservations; ///< Number of utilized observations (including pivot) | ||
| 272 | std::unordered_map<GnssObs::ObservationType, size_t> nObservationsUniqueSatellite; ///< Number of utilized observations (counted once for each satellite) | ||
| 273 | |||
| 274 | AmbiguityResolutionFailure ambiguityResolutionFailure = AmbiguityResolutionFailure::None; ///< Ambiguity resolution failure | ||
| 275 | double ambiguityCriticalValueRatio{}; ///< Ambiguity Critical Value µ ∈ (0, 1] (R1/R2 ≤ µ) | ||
| 276 | std::optional<size_t> nAmbiguitiesFixed; ///< Number of Ambiguities fixed | ||
| 277 | |||
| 278 | /// Cycle slip detector results and name of the receiver | ||
| 279 | std::vector<std::pair<CycleSlipDetector::Result, std::string>> cycleSlipDetectorResult; | ||
| 280 | |||
| 281 | /// Pivot Change information | ||
| 282 | struct PivotChange | ||
| 283 | { | ||
| 284 | /// Possible reasons for a pivot change | ||
| 285 | enum class Reason : uint8_t | ||
| 286 | { | ||
| 287 | None, ///< No reason selected yet | ||
| 288 | NewCode, ///< Code was not observed before | ||
| 289 | PivotNotObservedInEpoch, ///< Old pivot satellite was not observed this epoch | ||
| 290 | PivotCycleSlip, ///< The pivot satellite had a cycle-slip | ||
| 291 | HigherElevationFound, ///< A satellite with higher elevation was observed | ||
| 292 | PivotOutlier, ///< Old pivot satellite was flagged as outlier | ||
| 293 | }; | ||
| 294 | |||
| 295 | Reason reason = Reason::NewCode; ///< Reason | ||
| 296 | GnssObs::ObservationType obsType = GnssObs::ObservationType_COUNT; ///< Observation type | ||
| 297 | |||
| 298 | SatSigId oldPivotSat; ///< Old SatSig identifier | ||
| 299 | double oldPivotElevation = 0.0; ///< Old Satellite elevation [rad] | ||
| 300 | |||
| 301 | SatSigId newPivotSat; ///< New SatSig identifier | ||
| 302 | double newPivotElevation = 0.0; ///< New Satellite elevation [rad] | ||
| 303 | }; | ||
| 304 | |||
| 305 | /// List of pivot satellite changes | ||
| 306 | std::unordered_map<std::pair<Code, GnssObs::ObservationType>, PivotChange> changedPivotSatellites; | ||
| 307 | |||
| 308 | /// Observable | ||
| 309 | struct Observable | ||
| 310 | { | ||
| 311 | /// @brief Constructor | ||
| 312 | /// @param[in] satSigId Satellite Signal Id | ||
| 313 | /// @param[in] obsType Observation Type | ||
| 314 | 301941 | Observable(SatSigId satSigId, GnssObs::ObservationType obsType) | |
| 315 | 301941 | : satSigId(satSigId), obsType(obsType) {} | |
| 316 | |||
| 317 | SatSigId satSigId; ///< Satellite Signal Id | ||
| 318 | GnssObs::ObservationType obsType = GnssObs::ObservationType_COUNT; ///< Observation Type | ||
| 319 | |||
| 320 | /// @brief Less than comparison (needed for map) | ||
| 321 | /// @param[in] rhs Right hand side of the operator | ||
| 322 | /// @return True if lhs < rhs | ||
| 323 | 2492098 | bool operator<(const Observable& rhs) const | |
| 324 | { | ||
| 325 |
2/2✓ Branch 1 taken 416837 times.
✓ Branch 2 taken 2075261 times.
|
2492098 | return satSigId == rhs.satSigId ? obsType < rhs.obsType |
| 326 | 2492098 | : satSigId < rhs.satSigId; | |
| 327 | } | ||
| 328 | }; | ||
| 329 | |||
| 330 | /// List of pivot satellites | ||
| 331 | std::multiset<Observable> pivots; | ||
| 332 | /// Observables available from receivers (only if double diff possible) | ||
| 333 | std::multiset<Observable> observableReceived; | ||
| 334 | /// Observables available from receivers, but filtered by GUI settings | ||
| 335 | std::multiset<Observable> observableFiltered; | ||
| 336 | /// Observables used for the final solution | ||
| 337 | std::multiset<Observable> observableUsed; | ||
| 338 | |||
| 339 | /// Signals filtered by the observation filter | ||
| 340 | ObservationFilter::Filtered filtered; | ||
| 341 | |||
| 342 | /// Outlier information | ||
| 343 | struct Outlier | ||
| 344 | { | ||
| 345 | /// Outlier Type | ||
| 346 | enum class Type : uint8_t | ||
| 347 | { | ||
| 348 | None, ///< None | ||
| 349 | NIS, ///< Normalized Innovation Squared (NIS) | ||
| 350 | }; | ||
| 351 | |||
| 352 | /// @brief Constructor | ||
| 353 | /// @param[in] type Outlier Type | ||
| 354 | /// @param[in] satSigId Satellite Signal Id | ||
| 355 | /// @param[in] obsType Observation Type | ||
| 356 | ✗ | Outlier(const Type& type, const SatSigId& satSigId, const GnssObs::ObservationType& obsType) | |
| 357 | ✗ | : type(type), satSigId(satSigId), obsType(obsType) {} | |
| 358 | |||
| 359 | Type type = Type::None; ///< Outlier Type | ||
| 360 | SatSigId satSigId; ///< Satellite Signal Id | ||
| 361 | GnssObs::ObservationType obsType = GnssObs::ObservationType_COUNT; ///< Observation Type | ||
| 362 | }; | ||
| 363 | |||
| 364 | /// List of found outliers | ||
| 365 | std::vector<Outlier> outliers; | ||
| 366 | /// Normalized Innovation Squared (NIS) test result (before removing anything) | ||
| 367 | std::optional<KeyedKalmanFilter<double, RTK::States::StateKeyType, RTK::Meas::MeasKeyTypes>::NISResult> nisResultInitial; | ||
| 368 | /// Normalized Innovation Squared (NIS) test result (last NIS iteration) | ||
| 369 | std::optional<KeyedKalmanFilter<double, RTK::States::StateKeyType, RTK::Meas::MeasKeyTypes>::NISResult> nisResultFinal; | ||
| 370 | /// Amount of observations removed by NIS | ||
| 371 | size_t nisRemovedCnt = 0; | ||
| 372 | |||
| 373 | /// Ambiguity double differences | ||
| 374 | struct AmbiguityDD | ||
| 375 | { | ||
| 376 | SatSigId pivotSatSigId = SatSigId(Code::None, 0); ///< Pivot satellite Signal Id | ||
| 377 | SatSigId satSigId = SatSigId(Code::None, 0); ///< Satellite Signal id | ||
| 378 | UncertainValue<double> value = UncertainValue<double>{ .value = 0.0, .stdDev = 0.0 }; ///< Value | ||
| 379 | }; | ||
| 380 | |||
| 381 | /// Newly estimated ambiguities | ||
| 382 | std::vector<SatSigId> newEstimatedAmbiguity; | ||
| 383 | /// @brief Double differenced ambiguities | ||
| 384 | std::vector<AmbiguityDD> ambiguityDD_br; | ||
| 385 | |||
| 386 | /// 𝐳 Measurement vector | ||
| 387 | KeyedVectorXd<RTK::Meas::MeasKeyTypes> measInnovation; | ||
| 388 | |||
| 389 | /// Satellite specific data | ||
| 390 | struct SatData | ||
| 391 | { | ||
| 392 | double satElevation = 0.0; ///< Satellite Elevation [rad] | ||
| 393 | double satAzimuth = 0.0; ///< Satellite Azimuth [rad] | ||
| 394 | }; | ||
| 395 | |||
| 396 | /// Extended data for each satellite | ||
| 397 | std::vector<std::pair<SatId, SatData>> satData; | ||
| 398 | |||
| 399 | private: | ||
| 400 | /// @brief Print a table for the satellites | ||
| 401 | /// @param[in] satsReceived List of received satellites | ||
| 402 | /// @param[in] id Unique identifier | ||
| 403 | void guiTooltipSatellites(const std::map<SatelliteSystem, std::unordered_set<SatId>>& satsReceived, const char* id) const; | ||
| 404 | |||
| 405 | /// @brief Print an observation table to the GUI | ||
| 406 | /// @param[in] observables Observables | ||
| 407 | /// @param[in] showSatCounts Whether to show the observable count in the table header | ||
| 408 | /// @param[in] colorPivots Whether to color the pivot satellite | ||
| 409 | /// @param[in] colorNotUsed Whether to color observations not used | ||
| 410 | /// @param[in] colorCycleSlips Whether to color cycle-slips | ||
| 411 | /// @param[in] colorPivotChanges Whether to color pivot changes | ||
| 412 | /// @param[in] id Unique identifier | ||
| 413 | void guiTooltipObservationTable(const std::multiset<RtkSolution::Observable>& observables, | ||
| 414 | bool showSatCounts, | ||
| 415 | bool colorPivots, | ||
| 416 | bool colorNotUsed, | ||
| 417 | bool colorCycleSlips, | ||
| 418 | bool colorPivotChanges, | ||
| 419 | const char* id) const; | ||
| 420 | |||
| 421 | /// @brief Print a table for the ambiguities | ||
| 422 | /// @param[in] id Unique identifier | ||
| 423 | void guiTooltipAmbiguities(const char* id) const; | ||
| 424 | }; | ||
| 425 | |||
| 426 | } // namespace NAV | ||
| 427 | |||
| 428 | #ifndef DOXYGEN_IGNORE | ||
| 429 | |||
| 430 | /// @brief Formatter | ||
| 431 | template<> | ||
| 432 | struct fmt::formatter<NAV::RtkSolution::SolutionType> : fmt::formatter<const char*> | ||
| 433 | { | ||
| 434 | /// @brief Defines how to format structs | ||
| 435 | /// @param[in] solType Struct to format | ||
| 436 | /// @param[in, out] ctx Format context | ||
| 437 | /// @return Output iterator | ||
| 438 | template<typename FormatContext> | ||
| 439 | ✗ | auto format(const NAV::RtkSolution::SolutionType& solType, FormatContext& ctx) const | |
| 440 | { | ||
| 441 | ✗ | switch (solType) | |
| 442 | { | ||
| 443 | ✗ | case NAV::RtkSolution::SolutionType::None: | |
| 444 | ✗ | return fmt::formatter<const char*>::format("None", ctx); | |
| 445 | ✗ | case NAV::RtkSolution::SolutionType::SPP: | |
| 446 | ✗ | return fmt::formatter<const char*>::format("SPP", ctx); | |
| 447 | ✗ | case NAV::RtkSolution::SolutionType::Predicted: | |
| 448 | ✗ | return fmt::formatter<const char*>::format("Predicted", ctx); | |
| 449 | ✗ | case NAV::RtkSolution::SolutionType::RTK_Float: | |
| 450 | ✗ | return fmt::formatter<const char*>::format("Float", ctx); | |
| 451 | ✗ | case NAV::RtkSolution::SolutionType::RTK_Fixed: | |
| 452 | ✗ | return fmt::formatter<const char*>::format("Fixed", ctx); | |
| 453 | } | ||
| 454 | ✗ | return ctx.out(); | |
| 455 | } | ||
| 456 | }; | ||
| 457 | |||
| 458 | /// @brief Formatter | ||
| 459 | template<> | ||
| 460 | struct fmt::formatter<NAV::RtkSolution::Outlier::Type> : fmt::formatter<const char*> | ||
| 461 | { | ||
| 462 | /// @brief Defines how to format structs | ||
| 463 | /// @param[in] outlierType Struct to format | ||
| 464 | /// @param[in, out] ctx Format context | ||
| 465 | /// @return Output iterator | ||
| 466 | template<typename FormatContext> | ||
| 467 | ✗ | auto format(const NAV::RtkSolution::Outlier::Type& outlierType, FormatContext& ctx) const | |
| 468 | { | ||
| 469 | ✗ | switch (outlierType) | |
| 470 | { | ||
| 471 | ✗ | case NAV::RtkSolution::Outlier::Type::None: | |
| 472 | ✗ | return fmt::formatter<const char*>::format("None", ctx); | |
| 473 | ✗ | case NAV::RtkSolution::Outlier::Type::NIS: | |
| 474 | ✗ | return fmt::formatter<const char*>::format("NIS check", ctx); | |
| 475 | } | ||
| 476 | ✗ | return ctx.out(); | |
| 477 | } | ||
| 478 | }; | ||
| 479 | |||
| 480 | /// @brief Formatter | ||
| 481 | template<> | ||
| 482 | struct fmt::formatter<NAV::RtkSolution::PivotChange> : fmt::formatter<std::string> | ||
| 483 | { | ||
| 484 | /// @brief Defines how to format structs | ||
| 485 | /// @param[in] pivot Struct to format | ||
| 486 | /// @param[in, out] ctx Format context | ||
| 487 | /// @return Output iterator | ||
| 488 | template<typename FormatContext> | ||
| 489 | 21 | auto format(const NAV::RtkSolution::PivotChange& pivot, FormatContext& ctx) const | |
| 490 | { | ||
| 491 |
1/7✗ Branch 0 not taken.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
21 | switch (pivot.reason) |
| 492 | { | ||
| 493 | ✗ | case NAV::RtkSolution::PivotChange::Reason::None: | |
| 494 | ✗ | return fmt::formatter<std::string>::format("Pivot change reason is unknown", ctx); | |
| 495 | ✗ | case NAV::RtkSolution::PivotChange::Reason::PivotNotObservedInEpoch: | |
| 496 | ✗ | return fmt::formatter<std::string>::format(fmt::format("Pivot change [{}]: [{}] -> [{}]\n" | |
| 497 | "Old pivot not observed this epoch\n" | ||
| 498 | "New pivot elevation {:.4}°", | ||
| 499 | ✗ | pivot.obsType, pivot.oldPivotSat, pivot.newPivotSat, | |
| 500 | ✗ | NAV::rad2deg(pivot.newPivotElevation)), | |
| 501 | ✗ | ctx); | |
| 502 | ✗ | case NAV::RtkSolution::PivotChange::Reason::PivotCycleSlip: | |
| 503 | ✗ | return fmt::formatter<std::string>::format(fmt::format("Pivot change [{}]: [{}] -> [{}]\n" | |
| 504 | "Old pivot had cycle-slip\n" | ||
| 505 | "Elevation {:.4}° -> {:.4}°", | ||
| 506 | ✗ | pivot.obsType, pivot.oldPivotSat, pivot.newPivotSat, | |
| 507 | ✗ | NAV::rad2deg(pivot.oldPivotElevation), | |
| 508 | ✗ | NAV::rad2deg(pivot.newPivotElevation)), | |
| 509 | ✗ | ctx); | |
| 510 | ✗ | case NAV::RtkSolution::PivotChange::Reason::HigherElevationFound: | |
| 511 | ✗ | return fmt::formatter<std::string>::format(fmt::format("Pivot change [{}]: [{}] -> [{}]\n" | |
| 512 | "Satellite with higher elevation found\n" | ||
| 513 | "Elevation {:.4}° -> {:.4}°", | ||
| 514 | ✗ | pivot.obsType, pivot.oldPivotSat, pivot.newPivotSat, | |
| 515 | ✗ | NAV::rad2deg(pivot.oldPivotElevation), | |
| 516 | ✗ | NAV::rad2deg(pivot.newPivotElevation)), | |
| 517 | ✗ | ctx); | |
| 518 | 21 | case NAV::RtkSolution::PivotChange::Reason::NewCode: | |
| 519 |
1/2✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
|
42 | return fmt::formatter<std::string>::format(fmt::format("New pivot [{}][{}]\n" |
| 520 | "Elevation {:.4}°", | ||
| 521 | 21 | pivot.newPivotSat, pivot.obsType, | |
| 522 | 42 | NAV::rad2deg(pivot.newPivotElevation)), | |
| 523 | 21 | ctx); | |
| 524 | ✗ | case NAV::RtkSolution::PivotChange::Reason::PivotOutlier: | |
| 525 | ✗ | return fmt::formatter<std::string>::format(fmt::format("Pivot change [{}]: [{}] -> [{}]\n" | |
| 526 | "Old pivot flagged as outlier\n" | ||
| 527 | "Elevation {:.4}°", | ||
| 528 | ✗ | pivot.obsType, pivot.oldPivotSat, pivot.newPivotSat, | |
| 529 | ✗ | NAV::rad2deg(pivot.newPivotElevation)), | |
| 530 | ✗ | ctx); | |
| 531 | } | ||
| 532 | ✗ | return ctx.out(); | |
| 533 | } | ||
| 534 | }; | ||
| 535 | |||
| 536 | #endif | ||
| 537 |