| 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 CycleSlipDetector.hpp | ||
| 10 | /// @brief Combination of different cycle-slip detection algorithms | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2023-11-10 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <variant> | ||
| 17 | #include <array> | ||
| 18 | #include "NodeData/GNSS/GnssObs.hpp" | ||
| 19 | #include "Navigation/GNSS/Core/SatelliteIdentifier.hpp" | ||
| 20 | #include "CycleSlipDetector/PolynomialCycleSlipDetector.hpp" | ||
| 21 | |||
| 22 | namespace NAV | ||
| 23 | { | ||
| 24 | |||
| 25 | /// Dual frequency combination | ||
| 26 | struct DualFrequencyCombination | ||
| 27 | { | ||
| 28 | /// @brief Equal comparison (needed for unordered_map) | ||
| 29 | /// @param[in] rhs Right hand side of the operator | ||
| 30 | /// @return True if the elements are equal | ||
| 31 | ✗ | constexpr bool operator==(const DualFrequencyCombination& rhs) const { return satId == rhs.satId && sig1 == rhs.sig1 && sig2 == rhs.sig2; } | |
| 32 | |||
| 33 | /// @brief Less than comparison (needed for map) | ||
| 34 | /// @param[in] rhs Right hand side of the operator | ||
| 35 | /// @return True if lhs < rhs | ||
| 36 | constexpr bool operator<(const DualFrequencyCombination& rhs) const | ||
| 37 | { | ||
| 38 | return satId == rhs.satId ? (sig1 == rhs.sig1 | ||
| 39 | ? sig2 < rhs.sig2 | ||
| 40 | : sig1 < rhs.sig1) | ||
| 41 | : satId < rhs.satId; | ||
| 42 | } | ||
| 43 | |||
| 44 | SatId satId; ///< Satellite Identifier | ||
| 45 | Code sig1; ///< Signal code/frequency (f(sig1) > f(sig2), e.g. L1 if L1-L2) | ||
| 46 | Code sig2; ///< Signal code/frequency (f(sig2) < f(sig1), e.g. L2 if L1-L2) | ||
| 47 | }; | ||
| 48 | } // namespace NAV | ||
| 49 | |||
| 50 | namespace std | ||
| 51 | { | ||
| 52 | /// @brief Hash function for DualFrequencyCombination (needed for unordered_map) | ||
| 53 | template<> | ||
| 54 | struct hash<NAV::DualFrequencyCombination> | ||
| 55 | { | ||
| 56 | /// @brief Hash function for SatId | ||
| 57 | /// @param[in] c Dual frequency combination | ||
| 58 | 564800 | std::size_t operator()(const NAV::DualFrequencyCombination& c) const | |
| 59 | { | ||
| 60 | 564800 | auto hash1 = std::hash<NAV::SatId>{}(c.satId); | |
| 61 |
1/2✓ Branch 1 taken 564800 times.
✗ Branch 2 not taken.
|
564800 | auto hash2 = std::hash<NAV::Code>{}(c.sig1); |
| 62 |
1/2✓ Branch 1 taken 564800 times.
✗ Branch 2 not taken.
|
564800 | auto hash3 = std::hash<NAV::Code>{}(c.sig2); |
| 63 | |||
| 64 | 564800 | return hash1 | (hash2 << 24) | (hash3 << 48); | |
| 65 | } | ||
| 66 | }; | ||
| 67 | } // namespace std | ||
| 68 | |||
| 69 | namespace NAV | ||
| 70 | { | ||
| 71 | |||
| 72 | /// @brief Cycle-slip detector | ||
| 73 | class CycleSlipDetector | ||
| 74 | { | ||
| 75 | public: | ||
| 76 | /// @brief Detectors in use | ||
| 77 | enum class Detector : uint8_t | ||
| 78 | { | ||
| 79 | LLI, ///< Loss-of-Lock Indicator check | ||
| 80 | SingleFrequency, ///< Single frequency detector | ||
| 81 | DualFrequency, ///< Dual frequency detector | ||
| 82 | }; | ||
| 83 | |||
| 84 | /// @brief Is the cycle-slip detector enabled? | ||
| 85 | /// @param[in] detector Detector to request data for | ||
| 86 | [[nodiscard]] bool isEnabled(const Detector& detector) const | ||
| 87 | { | ||
| 88 | switch (detector) | ||
| 89 | { | ||
| 90 | case Detector::LLI: | ||
| 91 | return _enableLLICheck; | ||
| 92 | case Detector::SingleFrequency: | ||
| 93 | return _singleFrequencyDetector.isEnabled(); | ||
| 94 | case Detector::DualFrequency: | ||
| 95 | return _dualFrequencyDetector.isEnabled(); | ||
| 96 | }; | ||
| 97 | return false; | ||
| 98 | } | ||
| 99 | /// @brief Sets the enabled state | ||
| 100 | /// @param[in] enabled Whether to enabled or not | ||
| 101 | /// @param[in] detector Detector to modify | ||
| 102 | void setEnabled(bool enabled, const Detector& detector) | ||
| 103 | { | ||
| 104 | switch (detector) | ||
| 105 | { | ||
| 106 | case Detector::LLI: | ||
| 107 | _enableLLICheck = enabled; | ||
| 108 | break; | ||
| 109 | case Detector::SingleFrequency: | ||
| 110 | _singleFrequencyDetector.setEnabled(enabled); | ||
| 111 | break; | ||
| 112 | case Detector::DualFrequency: | ||
| 113 | _dualFrequencyDetector.setEnabled(enabled); | ||
| 114 | break; | ||
| 115 | }; | ||
| 116 | } | ||
| 117 | |||
| 118 | /// @brief Get the window size for the polynomial fit | ||
| 119 | /// @param[in] detector Detector to request data for | ||
| 120 | [[nodiscard]] size_t getWindowSize(const Detector& detector) const | ||
| 121 | { | ||
| 122 | return detector == Detector::SingleFrequency ? _singleFrequencyDetector.getWindowSize() : _dualFrequencyDetector.getWindowSize(); | ||
| 123 | } | ||
| 124 | /// @brief Sets the amount of points used for the fit (sliding window) | ||
| 125 | /// @param[in] windowSize Amount of points to use for the fit | ||
| 126 | /// @param[in] detector Detector to modify | ||
| 127 | void setWindowSize(size_t windowSize, const Detector& detector) | ||
| 128 | { | ||
| 129 | if (detector == Detector::SingleFrequency) { _singleFrequencyDetector.setWindowSize(windowSize); } | ||
| 130 | else { _dualFrequencyDetector.setWindowSize(windowSize); } | ||
| 131 | } | ||
| 132 | |||
| 133 | /// @brief Get the threshold to categorize a measurement as cycle slip [% of smallest wavelength] | ||
| 134 | /// @param[in] detector Detector to request data for | ||
| 135 | [[nodiscard]] double getThreshold(const Detector& detector) const | ||
| 136 | { | ||
| 137 | return detector == Detector::SingleFrequency ? _singleFrequencyThresholdPercentage : _dualFrequencyThresholdPercentage; | ||
| 138 | } | ||
| 139 | /// @brief Sets the threshold to categorize a measurement as cycle slip | ||
| 140 | /// @param[in] threshold Threshold value in [% of smallest wavelength] | ||
| 141 | /// @param[in] detector Detector to modify | ||
| 142 | void setThreshold(double threshold, const Detector& detector) | ||
| 143 | { | ||
| 144 | if (detector == Detector::SingleFrequency) { _singleFrequencyThresholdPercentage = threshold; } | ||
| 145 | else { _dualFrequencyThresholdPercentage = threshold; } | ||
| 146 | } | ||
| 147 | |||
| 148 | /// @brief Get the degree of the polynomial which is used for fitting | ||
| 149 | /// @param[in] detector Detector to request data for | ||
| 150 | [[nodiscard]] size_t getPolynomialDegree(const Detector& detector) const | ||
| 151 | { | ||
| 152 | return detector == Detector::SingleFrequency ? _singleFrequencyDetector.getPolynomialDegree() : _dualFrequencyDetector.getPolynomialDegree(); | ||
| 153 | } | ||
| 154 | /// @brief Sets the degree of the polynomial which is used for fitting | ||
| 155 | /// @param[in] polyDegree Polynomial degree to fit | ||
| 156 | /// @param[in] detector Detector to modify | ||
| 157 | void setPolynomialDegree(size_t polyDegree, const Detector& detector) | ||
| 158 | { | ||
| 159 | if (detector == Detector::SingleFrequency) { _singleFrequencyDetector.setPolynomialDegree(polyDegree); } | ||
| 160 | else { _dualFrequencyDetector.setPolynomialDegree(polyDegree); } | ||
| 161 | } | ||
| 162 | |||
| 163 | /// Strategies for fitting | ||
| 164 | using Strategy = PolynomialRegressor<>::Strategy; | ||
| 165 | |||
| 166 | /// @brief Get the strategy used for fitting | ||
| 167 | /// @param[in] detector Detector to request data for | ||
| 168 | [[nodiscard]] Strategy getFitStrategy(const Detector& detector) const | ||
| 169 | { | ||
| 170 | return detector == Detector::SingleFrequency ? _singleFrequencyDetector.getFitStrategy() : _dualFrequencyDetector.getFitStrategy(); | ||
| 171 | } | ||
| 172 | /// @brief Sets the strategy used for fitting | ||
| 173 | /// @param[in] strategy Strategy for fitting | ||
| 174 | /// @param[in] detector Detector to modify | ||
| 175 | void setFitStrategy(Strategy strategy, const Detector& detector) | ||
| 176 | { | ||
| 177 | if (detector == Detector::SingleFrequency) { _singleFrequencyDetector.setFitStrategy(strategy); } | ||
| 178 | else { _dualFrequencyDetector.setFitStrategy(strategy); } | ||
| 179 | } | ||
| 180 | |||
| 181 | /// @brief Cycle-slip because LLI was set | ||
| 182 | struct CycleSlipLossOfLockIndicator | ||
| 183 | { | ||
| 184 | SatSigId signal; ///< Signal identifier where the cycle-slip occurred | ||
| 185 | }; | ||
| 186 | /// @brief Cycle-slip found in single frequency carrier-phase observation | ||
| 187 | struct CycleSlipSingleFrequency | ||
| 188 | { | ||
| 189 | SatSigId signal; ///< Signal identifier where the cycle-slip occurred | ||
| 190 | }; | ||
| 191 | /// @brief Cycle-slip found in dual frequency combination | ||
| 192 | struct CycleSlipDualFrequency | ||
| 193 | { | ||
| 194 | std::array<SatSigId, 2> signals; ///< Signal identifiers where the cycle-slip occurred | ||
| 195 | }; | ||
| 196 | |||
| 197 | /// @brief Result of the cycle-slip detection test | ||
| 198 | using Result = std::variant<CycleSlipLossOfLockIndicator, CycleSlipDualFrequency, CycleSlipSingleFrequency>; | ||
| 199 | |||
| 200 | /// Satellite observations ordered per satellite | ||
| 201 | struct SatelliteObservation | ||
| 202 | { | ||
| 203 | /// @brief Signal for a code | ||
| 204 | struct Signal | ||
| 205 | { | ||
| 206 | Code code; ///< Code | ||
| 207 | GnssObs::ObservationData::CarrierPhase measurement; ///< Carrier-phase measurement and LLI flag | ||
| 208 | }; | ||
| 209 | |||
| 210 | SatId satId; ///< Satellite identifier | ||
| 211 | std::vector<Signal> signals; ///< List of signals | ||
| 212 | int8_t freqNum = -128; ///< Frequency number. Only used for GLONASS G1 and G2 | ||
| 213 | }; | ||
| 214 | |||
| 215 | /// @brief Checks for a cycle slip | ||
| 216 | /// @param[in] insTime Time of the measurement | ||
| 217 | /// @param[in] satObs Satellite observations | ||
| 218 | /// @param[in] nameId Node nameId for log messages | ||
| 219 | /// @return Cycle-slip result | ||
| 220 | [[nodiscard]] std::vector<Result> checkForCycleSlip(InsTime insTime, const std::vector<SatelliteObservation>& satObs, const std::string& nameId); | ||
| 221 | |||
| 222 | /// @brief Resets all data related to the provided signal | ||
| 223 | /// @param satSigId Satellite signal identifier | ||
| 224 | void resetSignal(const SatSigId& satSigId); | ||
| 225 | |||
| 226 | /// @brief Resets all data | ||
| 227 | void reset(); | ||
| 228 | |||
| 229 | private: | ||
| 230 | /// @brief Whether to check for LLI flag | ||
| 231 | bool _enableLLICheck = true; | ||
| 232 | |||
| 233 | double _singleFrequencyThresholdPercentage = 11.0; ///< Threshold to detect a cycle-slip in [% of smallest wavelength] | ||
| 234 | double _dualFrequencyThresholdPercentage = 0.5; ///< Threshold to detect a cycle-slip in [% of smallest wavelength] | ||
| 235 | |||
| 236 | /// Single Frequency carrier-phase cycle-slip detector using polynomial fits | ||
| 237 | PolynomialCycleSlipDetector<SatSigId> _singleFrequencyDetector{ 4, 2, false }; | ||
| 238 | |||
| 239 | /// Dual Frequency cycle-slip detector using polynomial fits | ||
| 240 | PolynomialCycleSlipDetector<DualFrequencyCombination> _dualFrequencyDetector{ 2, 1 }; | ||
| 241 | |||
| 242 | friend bool CycleSlipDetectorGui(const char* label, CycleSlipDetector& cycleSlipDetector, float width, bool dualFrequencyAvailable); | ||
| 243 | friend void to_json(json& j, const CycleSlipDetector& data); | ||
| 244 | friend void from_json(const json& j, CycleSlipDetector& data); | ||
| 245 | }; | ||
| 246 | |||
| 247 | /// @brief Shows a GUI for advanced configuration of the CycleSlipDetector | ||
| 248 | /// @param[in] label Label to show beside the combo box. This has to be a unique id for ImGui. | ||
| 249 | /// @param[in] cycleSlipDetector Reference to the cycle-slip detector to configure | ||
| 250 | /// @param[in] width Width of the widget | ||
| 251 | /// @param[in] dualFrequencyAvailable Whether dual frequency is available | ||
| 252 | bool CycleSlipDetectorGui(const char* label, CycleSlipDetector& cycleSlipDetector, float width = 0.0F, bool dualFrequencyAvailable = true); | ||
| 253 | |||
| 254 | /// @brief Write info to a json object | ||
| 255 | /// @param[out] j Json output | ||
| 256 | /// @param[in] data Object to read info from | ||
| 257 | void to_json(json& j, const CycleSlipDetector& data); | ||
| 258 | /// @brief Read info from a json object | ||
| 259 | /// @param[in] j Json variable to read info from | ||
| 260 | /// @param[out] data Output object | ||
| 261 | void from_json(const json& j, CycleSlipDetector& data); | ||
| 262 | |||
| 263 | /// @brief Converts the detector result into a string | ||
| 264 | /// @param cycleSlip Cycle-slip | ||
| 265 | [[nodiscard]] std::string to_string(const CycleSlipDetector::Result& cycleSlip); | ||
| 266 | |||
| 267 | } // namespace NAV | ||
| 268 | |||
| 269 | /// @brief Stream insertion operator overload | ||
| 270 | /// @param[in, out] os Output stream object to stream the time into | ||
| 271 | /// @param[in] obj Object to print | ||
| 272 | /// @return Returns the output stream object in order to chain stream insertions | ||
| 273 | std::ostream& operator<<(std::ostream& os, const NAV::CycleSlipDetector::Result& obj); | ||
| 274 | |||
| 275 | #ifndef DOXYGEN_IGNORE | ||
| 276 | |||
| 277 | /// @brief Formatter | ||
| 278 | template<> | ||
| 279 | struct fmt::formatter<NAV::CycleSlipDetector::Result> : fmt::formatter<std::string> | ||
| 280 | { | ||
| 281 | /// @brief Defines how to format structs | ||
| 282 | /// @param[in] cycleSlip Struct to format | ||
| 283 | /// @param[in, out] ctx Format context | ||
| 284 | /// @return Output iterator | ||
| 285 | template<typename FormatContext> | ||
| 286 | ✗ | auto format(const NAV::CycleSlipDetector::Result& cycleSlip, FormatContext& ctx) const | |
| 287 | { | ||
| 288 | ✗ | return fmt::formatter<std::string>::format(to_string(cycleSlip), ctx); | |
| 289 | } | ||
| 290 | }; | ||
| 291 | |||
| 292 | #endif | ||
| 293 |