| 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 RealTimeKinematic.hpp | ||
| 10 | /// @brief Real-Time Kinematic (RTK) carrier-phase DGNSS | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2023-04-13 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <cstddef> | ||
| 17 | #include <cstdint> | ||
| 18 | #include <memory> | ||
| 19 | #include "internal/Node/Node.hpp" | ||
| 20 | #include "internal/gui/widgets/DynamicInputPins.hpp" | ||
| 21 | |||
| 22 | #include "Navigation/GNSS/Core/Frequency.hpp" | ||
| 23 | #include "Navigation/GNSS/Core/Code.hpp" | ||
| 24 | #include "Navigation/GNSS/Core/SatelliteIdentifier.hpp" | ||
| 25 | #include "Navigation/GNSS/Positioning/SPP/Algorithm.hpp" | ||
| 26 | #include "Navigation/GNSS/Positioning/ReceiverClock.hpp" | ||
| 27 | #include "Navigation/GNSS/Positioning/RTK/Keys.hpp" | ||
| 28 | #include "Navigation/GNSS/Positioning/Observation.hpp" | ||
| 29 | #include "Navigation/GNSS/Positioning/ObservationEstimator.hpp" | ||
| 30 | #include "Navigation/GNSS/Positioning/ObservationFilter.hpp" | ||
| 31 | #include "Navigation/GNSS/Positioning/Receiver.hpp" | ||
| 32 | #include "Navigation/GNSS/Ambiguity/AmbiguityResolution.hpp" | ||
| 33 | #include "Navigation/GNSS/Ambiguity/CycleSlipDetector.hpp" | ||
| 34 | #include "Navigation/GNSS/SNRMask.hpp" | ||
| 35 | #include "Navigation/GNSS/Satellite/internal/SatNavData.hpp" | ||
| 36 | #include "Navigation/Transformations/Units.hpp" | ||
| 37 | #include "Navigation/Math/KalmanFilter.hpp" | ||
| 38 | #include "Navigation/Math/KeyedKalmanFilter.hpp" | ||
| 39 | |||
| 40 | #include "NodeData/GNSS/SppSolution.hpp" | ||
| 41 | #include "NodeData/GNSS/RtkSolution.hpp" | ||
| 42 | #include "NodeData/GNSS/GnssObs.hpp" | ||
| 43 | #include "NodeData/GNSS/GnssNavInfo.hpp" | ||
| 44 | |||
| 45 | #include "util/Eigen.hpp" | ||
| 46 | #include "util/Container/Unordered_map.hpp" | ||
| 47 | |||
| 48 | namespace NAV | ||
| 49 | { | ||
| 50 | |||
| 51 | /// @brief Numerically integrates Imu data | ||
| 52 | class RealTimeKinematic : public Node | ||
| 53 | { | ||
| 54 | public: | ||
| 55 | /// @brief Receiver Types | ||
| 56 | enum ReceiverType : uint8_t | ||
| 57 | { | ||
| 58 | Rover, ///< Rover | ||
| 59 | Base, ///< Base | ||
| 60 | ReceiverType_COUNT, ///< Amount of receiver types | ||
| 61 | }; | ||
| 62 | |||
| 63 | /// @brief Default constructor | ||
| 64 | RealTimeKinematic(); | ||
| 65 | /// @brief Destructor | ||
| 66 | ~RealTimeKinematic() override; | ||
| 67 | /// @brief Copy constructor | ||
| 68 | RealTimeKinematic(const RealTimeKinematic&) = delete; | ||
| 69 | /// @brief Move constructor | ||
| 70 | RealTimeKinematic(RealTimeKinematic&&) = delete; | ||
| 71 | /// @brief Copy assignment operator | ||
| 72 | RealTimeKinematic& operator=(const RealTimeKinematic&) = delete; | ||
| 73 | /// @brief Move assignment operator | ||
| 74 | RealTimeKinematic& operator=(RealTimeKinematic&&) = delete; | ||
| 75 | |||
| 76 | /// @brief String representation of the Class Type | ||
| 77 | [[nodiscard]] static std::string typeStatic(); | ||
| 78 | |||
| 79 | /// @brief String representation of the Class Type | ||
| 80 | [[nodiscard]] std::string type() const override; | ||
| 81 | |||
| 82 | /// @brief String representation of the Class Category | ||
| 83 | [[nodiscard]] static std::string category(); | ||
| 84 | |||
| 85 | /// @brief ImGui config window which is shown on double click | ||
| 86 | /// @attention Don't forget to set _hasConfig to true in the constructor of the node | ||
| 87 | void guiConfig() override; | ||
| 88 | |||
| 89 | /// @brief Saves the node into a json object | ||
| 90 | [[nodiscard]] json save() const override; | ||
| 91 | |||
| 92 | /// @brief Restores the node from a json object | ||
| 93 | /// @param[in] j Json object with the node state | ||
| 94 | void restore(const json& j) override; | ||
| 95 | |||
| 96 | private: | ||
| 97 | constexpr static size_t INPUT_PORT_INDEX_BASE_POS = 0; ///< @brief Pos | ||
| 98 | constexpr static size_t INPUT_PORT_INDEX_BASE_GNSS_OBS = 1; ///< @brief GnssObs | ||
| 99 | constexpr static size_t INPUT_PORT_INDEX_ROVER_GNSS_OBS = 2; ///< @brief GnssObs | ||
| 100 | constexpr static size_t INPUT_PORT_INDEX_GNSS_NAV_INFO = 3; ///< @brief GnssNavInfo | ||
| 101 | |||
| 102 | constexpr static size_t OUTPUT_PORT_INDEX_RTKSOL = 0; ///< @brief Flow (RtkSol) | ||
| 103 | |||
| 104 | // --------------------------------------------------------------- Gui ----------------------------------------------------------------- | ||
| 105 | |||
| 106 | /// Possible Units for the Standard deviation of the ambiguities | ||
| 107 | enum class StdevAmbiguityUnits : uint8_t | ||
| 108 | { | ||
| 109 | Cycle, ///< [cycle] | ||
| 110 | }; | ||
| 111 | |||
| 112 | /// @brief Initialize the node | ||
| 113 | bool initialize() override; | ||
| 114 | |||
| 115 | /// @brief Deinitialize the node | ||
| 116 | void deinitialize() override; | ||
| 117 | |||
| 118 | /// @brief Function to call to add a new pin | ||
| 119 | /// @param[in, out] node Pointer to this node | ||
| 120 | static void pinAddCallback(Node* node); | ||
| 121 | /// @brief Function to call to delete a pin | ||
| 122 | /// @param[in, out] node Pointer to this node | ||
| 123 | /// @param[in] pinIdx Input pin index to delete | ||
| 124 | static void pinDeleteCallback(Node* node, size_t pinIdx); | ||
| 125 | |||
| 126 | /// Observation Filter | ||
| 127 | ObservationFilter _obsFilter{ ReceiverType::ReceiverType_COUNT }; | ||
| 128 | |||
| 129 | /// Observation Estimator | ||
| 130 | ObservationEstimator _obsEstimator{ ReceiverType::ReceiverType_COUNT }; | ||
| 131 | /// Maximum amount of outliers to remove per epoch | ||
| 132 | size_t _maxRemoveOutlier = 4; | ||
| 133 | /// Amount of epochs to remove an outlier after being found (1 = only current) | ||
| 134 | size_t _outlierRemoveEpochs = 1; | ||
| 135 | /// Minumum amount of satellites for doing a NIS check | ||
| 136 | size_t _outlierMinSat = 4; | ||
| 137 | /// Minumum amount of pseudorange observables to leave when doing a NIS check | ||
| 138 | size_t _outlierMinPsrObsKeep = 4; | ||
| 139 | |||
| 140 | /// Do not attempt to remove outlier if the position variance is above the threshold in the startup phase [m^2] | ||
| 141 | double _outlierMaxPosVarStartup = 0.1; | ||
| 142 | |||
| 143 | /// @brief Wether to output a SPP solution if no base observations are available | ||
| 144 | bool _calcSPPIfNoBase = false; | ||
| 145 | /// Maximum time between base and rover observations to calculate the RTK solution | ||
| 146 | double _maxTimeBetweenBaseRoverForRTK = 5.0; | ||
| 147 | |||
| 148 | /// Ambiguity resolution algorithms and parameters to use | ||
| 149 | AmbiguityResolutionParameters _ambiguityResolutionParameters; | ||
| 150 | |||
| 151 | /// Ambiguity resolution strategy | ||
| 152 | AmbiguityResolutionStrategy _ambiguityResolutionStrategy = AmbiguityResolutionStrategy::Continuous; | ||
| 153 | |||
| 154 | /// Minimum amount of satellites with carrier observations to try fixing the solution | ||
| 155 | size_t _nMinSatForAmbFix = 4; | ||
| 156 | /// Minimum amount of satellites with carrier observations for holding the ambiguities | ||
| 157 | size_t _nMinSatForAmbHold = 5; | ||
| 158 | /// Do not attempt to fix if the position variance is above the threshold | ||
| 159 | double _maxPosVar = 0.4; | ||
| 160 | |||
| 161 | /// Make an update with the fixed ambiguities when true. | ||
| 162 | /// Otherwise apply via $$a = a_fix$$ and $$b = b_float - Q_ba * Q_aa^-1 (a_fix - a_float)$$ | ||
| 163 | bool _applyFixedAmbiguitiesWithUpdate = true; | ||
| 164 | |||
| 165 | /// Measurement noise standard deviation used when fixing ambiguities in [cycles] (GUI value) | ||
| 166 | double _gui_ambFixUpdateStdDev = 1e-2; | ||
| 167 | /// Gui selection for the Unit of the input for the StDev of the ambiguities (while float solution) | ||
| 168 | StdevAmbiguityUnits _gui_ambFixUpdateStdDevUnits = StdevAmbiguityUnits::Cycle; | ||
| 169 | /// Measurement noise variance used when fixing ambiguities in [cycles^2] (Value used for calculation) | ||
| 170 | double _ambFixUpdateVariance = _gui_ambFixUpdateStdDev * _gui_ambFixUpdateStdDev; | ||
| 171 | |||
| 172 | bool _maxPosVarTriggered = false; ///< Trigger state of the check | ||
| 173 | bool _outlierMaxPosVarStartupTriggered = false; ///< Trigger state of the check | ||
| 174 | bool _nMinSatForAmbFixTriggered = false; ///< Trigger state of the check | ||
| 175 | bool _nMinSatForAmbHoldTriggered = false; ///< Trigger state of the check | ||
| 176 | |||
| 177 | // ##################################################################################### | ||
| 178 | |||
| 179 | /// Possible Units for the Standard deviation of the acceleration due to user motion | ||
| 180 | enum class StdevAccelUnits : uint8_t | ||
| 181 | { | ||
| 182 | m_sqrts3, ///< [ m / √(s^3) ] | ||
| 183 | }; | ||
| 184 | /// Gui selection for the Unit of the input stdev_accel parameter for the StDev due to acceleration due to user motion | ||
| 185 | StdevAccelUnits _gui_stdevAccelUnits = StdevAccelUnits::m_sqrts3; | ||
| 186 | |||
| 187 | /// @brief GUI selection for the Standard deviation of the acceleration 𝜎_a due to user motion in horizontal and vertical component | ||
| 188 | /// @note See Groves (2013) eq. (9.156) | ||
| 189 | std::array<double, 2> _gui_stdevAccel = { { 3.0, 1.5 } } /* [ m / √(s^3) ] */; | ||
| 190 | |||
| 191 | /// Gui selection for the Unit of the input for the StDev of the ambiguities (while float solution) | ||
| 192 | StdevAmbiguityUnits _gui_stdevAmbiguityFloatUnits = StdevAmbiguityUnits::Cycle; | ||
| 193 | /// Process noise (standard deviation) for the ambiguities (while float solution) (Selection in GUI) | ||
| 194 | double _gui_ambiguityFloatProcessNoiseStDev = 0.5; | ||
| 195 | /// Process noise (variance) for the ambiguities (while float solution) in [cycles^2] (Value used for calculation) | ||
| 196 | double _ambiguityFloatProcessNoiseVariance = _gui_ambiguityFloatProcessNoiseStDev * _gui_ambiguityFloatProcessNoiseStDev; | ||
| 197 | |||
| 198 | /// Gui selection for the Unit of the input for the StDev of the ambiguities (while fix solution) | ||
| 199 | StdevAmbiguityUnits _gui_stdevAmbiguityFixUnits = StdevAmbiguityUnits::Cycle; | ||
| 200 | /// Process noise (standard deviation) for the ambiguities (while fix solution) (Selection in GUI) | ||
| 201 | double _gui_ambiguityFixProcessNoiseStDev = 1e-4; | ||
| 202 | /// Process noise (standard deviation) for the ambiguities (while fix solution) in [cycles^2] (Value used for calculation) | ||
| 203 | double _ambiguityFixProcessNoiseVariance = _gui_ambiguityFixProcessNoiseStDev * _gui_ambiguityFixProcessNoiseStDev; | ||
| 204 | |||
| 205 | // ------------------------------------------------------------ Algorithm -------------------------------------------------------------- | ||
| 206 | |||
| 207 | using Receiver = NAV::Receiver<ReceiverType>; ///< Receiver | ||
| 208 | |||
| 209 | /// @brief SPP algorithm | ||
| 210 | SPP::Algorithm _sppAlgorithm; | ||
| 211 | |||
| 212 | /// @brief Receivers | ||
| 213 | std::array<Receiver, ReceiverType::ReceiverType_COUNT> _receiver = { { Receiver(Base, {}), Receiver(Rover, {}) } }; | ||
| 214 | |||
| 215 | /// Flag, whether the observation was received this epoch | ||
| 216 | bool _baseObsReceivedThisEpoch = true; | ||
| 217 | |||
| 218 | /// Solution type of last epoch | ||
| 219 | RtkSolution::SolutionType _lastSolutionStatus = RtkSolution::SolutionType::RTK_Float; | ||
| 220 | |||
| 221 | /// Differences (single or double) | ||
| 222 | struct Difference | ||
| 223 | { | ||
| 224 | double estimate = 0.0; ///< Estimate | ||
| 225 | double measurement = 0.0; ///< Measurement | ||
| 226 | }; | ||
| 227 | |||
| 228 | /// @brief Difference storage type | ||
| 229 | using Differences = unordered_map<SatSigId, | ||
| 230 | unordered_map<GnssObs::ObservationType, | ||
| 231 | Difference>>; | ||
| 232 | |||
| 233 | /// @brief Pivot satellite information | ||
| 234 | struct PivotSatellite | ||
| 235 | { | ||
| 236 | /// @brief Constructor | ||
| 237 | /// @param[in] satSigId Satellite Signal identifier | ||
| 238 | 57 | explicit PivotSatellite(const SatSigId& satSigId) : satSigId(satSigId) {} | |
| 239 | |||
| 240 | SatSigId satSigId; ///< Satellite Signal identifier | ||
| 241 | }; | ||
| 242 | |||
| 243 | /// Pivot satellites for each constellation | ||
| 244 | unordered_map<std::pair<Code, GnssObs::ObservationType>, PivotSatellite> _pivotSatellites; | ||
| 245 | |||
| 246 | /// Last update time | ||
| 247 | InsTime _lastUpdate; | ||
| 248 | /// Data interval [s] | ||
| 249 | double _dataInterval = 1; | ||
| 250 | |||
| 251 | /// Kalman Filter representation | ||
| 252 | KeyedKalmanFilterD<RTK::States::StateKeyType, RTK::Meas::MeasKeyTypes> _kalmanFilter; | ||
| 253 | |||
| 254 | /// Cycle-slip detector | ||
| 255 | std::array<CycleSlipDetector, ReceiverType_COUNT> _cycleSlipDetector; | ||
| 256 | |||
| 257 | /// List of event texts (pivot changes, cycle-slips) | ||
| 258 | std::vector<std::pair<InsTime, std::vector<std::string>>> _events; | ||
| 259 | |||
| 260 | /// Regex for filtering events | ||
| 261 | std::string _eventFilterRegex; | ||
| 262 | |||
| 263 | /// Whether to output state change events | ||
| 264 | bool _outputStateEvents = false; | ||
| 265 | |||
| 266 | /// Ambiguities which are fixed and hold. Values in [cycles] | ||
| 267 | std::unordered_map<SatSigId, double> _ambiguitiesHold; | ||
| 268 | |||
| 269 | /// Percentage of fix solutions | ||
| 270 | size_t nFixSolutions = 0; | ||
| 271 | /// Percentage of float solutions | ||
| 272 | size_t nFloatSolutions = 0; | ||
| 273 | /// Percentage of float solutions | ||
| 274 | size_t nSingleSolutions = 0; | ||
| 275 | |||
| 276 | size_t _nPivotChange = 0; ///< Amount of pivot changes performed | ||
| 277 | size_t _nCycleSlipsLLI = 0; ///< Cycle-slip detection count (LLI) | ||
| 278 | size_t _nCycleSlipsSingle = 0; ///< Cycle-slip detection count (Single) | ||
| 279 | size_t _nCycleSlipsDual = 0; ///< Cycle-slip detection count (Dual) | ||
| 280 | size_t _nMeasExcludedNIS = 0; ///< Measurement excluded due to NIS test count | ||
| 281 | |||
| 282 | /// @brief Dynamic input pins | ||
| 283 | /// @attention This should always be the last variable in the header, because it accesses others through the function callbacks | ||
| 284 | gui::widgets::DynamicInputPins _dynamicInputPins{ INPUT_PORT_INDEX_GNSS_NAV_INFO, this, pinAddCallback, pinDeleteCallback }; | ||
| 285 | |||
| 286 | /// @brief Adds the event to the GUI list | ||
| 287 | /// @param[in, out] rtkSol RtkSolution to update | ||
| 288 | /// @param[in] text Text to display | ||
| 289 | void addEventToGui(const std::shared_ptr<RtkSolution>& rtkSol, const std::string& text); | ||
| 290 | |||
| 291 | /// @brief Prints the observations for logging | ||
| 292 | /// @param[in] observations List of GNSS observation data used for the calculation of this epoch | ||
| 293 | void printObservations(const Observations& observations); | ||
| 294 | |||
| 295 | /// @brief Receive Function for the Base Position | ||
| 296 | /// @param[in] queue Queue with all the received data messages | ||
| 297 | /// @param[in] pinIdx Index of the pin the data is received on | ||
| 298 | void recvBasePos(InputPin::NodeDataQueue& queue, size_t pinIdx); | ||
| 299 | |||
| 300 | /// @brief Receive Function for the Base GNSS Observations | ||
| 301 | /// @param[in] queue Queue with all the received data messages | ||
| 302 | /// @param[in] pinIdx Index of the pin the data is received on | ||
| 303 | void recvBaseGnssObs(InputPin::NodeDataQueue& queue, size_t pinIdx); | ||
| 304 | |||
| 305 | /// @brief Receive Function for the RoverGNSS Observations | ||
| 306 | /// @param[in] queue Queue with all the received data messages | ||
| 307 | /// @param[in] pinIdx Index of the pin the data is received on | ||
| 308 | void recvRoverGnssObs(InputPin::NodeDataQueue& queue, size_t pinIdx); | ||
| 309 | |||
| 310 | /// @brief Assign the SPP solution to the RTK filter | ||
| 311 | /// @param sppSol SPP solution | ||
| 312 | void assignSolutionToFilter(const std::shared_ptr<NAV::SppSolution>& sppSol); | ||
| 313 | |||
| 314 | /// @brief Calculates the RTK solution | ||
| 315 | void calcRealTimeKinematicSolution(); | ||
| 316 | |||
| 317 | /// @brief Calculates a SPP solution as fallback in case no base data is available | ||
| 318 | std::shared_ptr<RtkSolution> calcFallbackSppSolution(); | ||
| 319 | |||
| 320 | /// @brief Does the Kalman Filter prediction | ||
| 321 | void kalmanFilterPrediction(); | ||
| 322 | |||
| 323 | /// @brief Checks for cycle-slips | ||
| 324 | /// @param[in] observations List of GNSS observation data used for the calculation of this epoch | ||
| 325 | /// @param[in, out] rtkSol RtkSolution to update | ||
| 326 | void checkForCycleSlip(Observations& observations, const std::shared_ptr<RtkSolution>& rtkSol); | ||
| 327 | |||
| 328 | /// @brief Removes observations which do not have a second one to do double differences | ||
| 329 | /// @param[in] observations List of GNSS observation data used for the calculation of this epoch | ||
| 330 | /// @param[in] filtered Filtered observations | ||
| 331 | /// @param[in, out] rtkSol RtkSolution to update | ||
| 332 | void removeSingleObservations(Observations& observations, ObservationFilter::Filtered* filtered, const std::shared_ptr<RtkSolution>& rtkSol); | ||
| 333 | |||
| 334 | /// @brief Update the specified pivot satellite and also does a pivot change in the Kalman filter state if necessary | ||
| 335 | /// @param code Code for the pivot | ||
| 336 | /// @param obsType Observation type of the pivot | ||
| 337 | /// @param observations List of GNSS observation data used for the calculation of this epoch | ||
| 338 | /// @param rtkSol RtkSolution to update | ||
| 339 | /// @param reason Reason for changing the pivot | ||
| 340 | /// @return The final pivot change result after the update. Empty if no pivot was selected | ||
| 341 | std::optional<RtkSolution::PivotChange> updatePivotSatellite(Code code, GnssObs::ObservationType obsType, | ||
| 342 | Observations& observations, const std::shared_ptr<RtkSolution>& rtkSol, | ||
| 343 | const RtkSolution::PivotChange::Reason& reason); | ||
| 344 | |||
| 345 | /// Prints all pivot satellites | ||
| 346 | void printPivotSatellites(); | ||
| 347 | |||
| 348 | /// @brief Update the pivot satellites for each constellation | ||
| 349 | /// @param[in] observations List of GNSS observation data used for the calculation of this epoch | ||
| 350 | /// @param[in, out] rtkSol RtkSolution to update | ||
| 351 | void updatePivotSatellites(Observations& observations, const std::shared_ptr<RtkSolution>& rtkSol); | ||
| 352 | |||
| 353 | /// @brief Adapts the Kalman Filter if a pivot satellite signal was changed | ||
| 354 | /// @param[in] newPivotSatSigId Newly added pivot satellite signal | ||
| 355 | /// @param[in] oldPivotSatSigId Old pivot satellite signal | ||
| 356 | /// @param[in] oldPivotObservedInEpoch Indicates if the old pivot satellite is still observed, or is | ||
| 357 | /// @param[in, out] rtkSol RtkSolution to update | ||
| 358 | void updateKalmanFilterAmbiguitiesForPivotChange(const SatSigId& newPivotSatSigId, const SatSigId& oldPivotSatSigId, bool oldPivotObservedInEpoch, const std::shared_ptr<RtkSolution>& rtkSol); | ||
| 359 | |||
| 360 | /// @brief Adds or remove Ambiguities to/from the Kalman Filter state depending on the received observations | ||
| 361 | /// @param[in] observations List of GNSS observation data used for the calculation of this epoch | ||
| 362 | /// @param[in, out] rtkSol RtkSolution to update | ||
| 363 | void addOrRemoveKalmanFilterAmbiguities(const Observations& observations, const std::shared_ptr<RtkSolution>& rtkSol); | ||
| 364 | |||
| 365 | /// @brief Calculates the single difference of the measurements and estimates | ||
| 366 | /// @param[in] observations List of GNSS observation data used for the calculation of this epoch | ||
| 367 | [[nodiscard]] Differences calcSingleDifferences(const Observations& observations) const; | ||
| 368 | |||
| 369 | /// @brief Calculates the double difference of the measurements and estimates | ||
| 370 | /// @param[in] observations List of GNSS observation data used for the calculation of this epoch | ||
| 371 | /// @param[in] singleDifferences List of single differences | ||
| 372 | [[nodiscard]] Differences calcDoubleDifferences(const Observations& observations, const Differences& singleDifferences) const; | ||
| 373 | |||
| 374 | /// @brief Calculate the measurement noise matrices for each observation type | ||
| 375 | /// @param observations List of GNSS observation data used for the calculation of this epoch | ||
| 376 | [[nodiscard]] unordered_map<GnssObs::ObservationType, KeyedMatrixXd<RTK::Meas::SingleObs<ReceiverType>>> | ||
| 377 | calcSingleObsMeasurementNoiseMatrices(const Observations& observations) const; | ||
| 378 | |||
| 379 | /// @brief Calculates the Measurement vector 𝐳, Measurement sensitivity matrix 𝐇 and Measurement noise covariance matrix 𝐑 for the KF update | ||
| 380 | /// @param[in] observations List of GNSS observation data used for the calculation of this epoch | ||
| 381 | /// @param[in] doubleDifferences List of double differences | ||
| 382 | /// @param[in] Rtilde Single observation Measurement Noise Covariance Matrix R for each observation type | ||
| 383 | void calcKalmanUpdateMatrices(const Observations& observations, const Differences& doubleDifferences, | ||
| 384 | const unordered_map<GnssObs::ObservationType, KeyedMatrixXd<RTK::Meas::SingleObs<ReceiverType>>>& Rtilde); | ||
| 385 | |||
| 386 | /// @brief Outlier removal info | ||
| 387 | struct OutlierInfo | ||
| 388 | { | ||
| 389 | /// @brief Defaults Constructor | ||
| 390 | ✗ | OutlierInfo() = default; | |
| 391 | /// @brief Constructor | ||
| 392 | /// @param key Measurement key | ||
| 393 | explicit OutlierInfo(const RTK::Meas::MeasKeyTypes& key) : key(key) {} | ||
| 394 | |||
| 395 | std::optional<RtkSolution::PivotChange::Reason> pivot; ///< Pivot change reason, if it is a pivot | ||
| 396 | GnssObs::ObservationType obsType = GnssObs::ObservationType_COUNT; ///< Observation type | ||
| 397 | SatSigId satSigId; ///< Satellit signal id | ||
| 398 | RTK::Meas::MeasKeyTypes key = RTK::Meas::PsrDD{}; ///< Measurement key of the outlier | ||
| 399 | }; | ||
| 400 | |||
| 401 | /// @brief Performs the NIS check | ||
| 402 | /// @param[in, out] observations List of GNSS observation data. Elements can be removed | ||
| 403 | /// @param[in, out] rtkSol RtkSolution to update | ||
| 404 | /// @return Info of the removed outliers | ||
| 405 | std::vector<OutlierInfo> removeOutlier(Observations& observations, const std::shared_ptr<RtkSolution>& rtkSol); | ||
| 406 | |||
| 407 | /// @brief Update Valid information | ||
| 408 | struct UpdateStatus | ||
| 409 | { | ||
| 410 | bool valid = true; ///< Flag if the solution is valid | ||
| 411 | double dx = 0.0; ///< Position change due to the update | ||
| 412 | double threshold = 0.0; ///< Allowed position change | ||
| 413 | }; | ||
| 414 | |||
| 415 | /// @brief Does the Kalman Filter update | ||
| 416 | /// @param[in, out] rtkSol RtkSolution to update | ||
| 417 | /// @return Update valid information | ||
| 418 | UpdateStatus kalmanFilterUpdate(const std::shared_ptr<RtkSolution>& rtkSol); | ||
| 419 | |||
| 420 | /// @brief Resolves the ambiguities in the Kalman filter and updates the state and covariance matrix | ||
| 421 | /// @param[in] nCarrierMeasUniqueSatellite Amount of used | ||
| 422 | /// @param[in, out] rtkSol RtkSolution to update | ||
| 423 | /// @return True if the ambiguities could be fixed | ||
| 424 | bool resolveAmbiguities(size_t nCarrierMeasUniqueSatellite, const std::shared_ptr<RtkSolution>& rtkSol); | ||
| 425 | |||
| 426 | /// @brief Apply the fixed ambiguities to the kalman filter state | ||
| 427 | /// @param[in] fixedAmb Fixed ambiguities | ||
| 428 | /// @param[in] ambKeys Ambiguity state keys | ||
| 429 | /// @param[in] ambMeasKeys Ambiguity measurement keys | ||
| 430 | void applyFixedAmbiguities(const Eigen::VectorXd& fixedAmb, const std::vector<RTK::States::StateKeyType>& ambKeys, const std::vector<RTK::Meas::MeasKeyTypes>& ambMeasKeys); | ||
| 431 | }; | ||
| 432 | |||
| 433 | /// @brief Converts the enum to a string | ||
| 434 | /// @param[in] receiver Enum value to convert into text | ||
| 435 | /// @return String representation of the enum | ||
| 436 | [[nodiscard]] const char* to_string(const RealTimeKinematic::ReceiverType& receiver); | ||
| 437 | |||
| 438 | } // namespace NAV | ||
| 439 | |||
| 440 | #ifndef DOXYGEN_IGNORE | ||
| 441 | |||
| 442 | /// @brief Formatter | ||
| 443 | template<> | ||
| 444 | struct fmt::formatter<NAV::RealTimeKinematic::ReceiverType> : fmt::formatter<const char*> | ||
| 445 | { | ||
| 446 | /// @brief Defines how to format structs | ||
| 447 | /// @param[in] type Struct to format | ||
| 448 | /// @param[in, out] ctx Format context | ||
| 449 | /// @return Output iterator | ||
| 450 | template<typename FormatContext> | ||
| 451 | 1202 | auto format(const NAV::RealTimeKinematic::ReceiverType& type, FormatContext& ctx) const | |
| 452 | { | ||
| 453 |
1/2✓ Branch 2 taken 1202 times.
✗ Branch 3 not taken.
|
1202 | return fmt::formatter<const char*>::format(to_string(type), ctx); |
| 454 | } | ||
| 455 | }; | ||
| 456 | |||
| 457 | #endif | ||
| 458 |