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