| 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 Keys.hpp | ||
| 10 | /// @brief Keys for the SPP algorithm for use inside the KeyedMatrices | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2023-12-22 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <vector> | ||
| 17 | #include <variant> | ||
| 18 | #include <fmt/format.h> | ||
| 19 | |||
| 20 | #include "Navigation/GNSS/Core/SatelliteIdentifier.hpp" | ||
| 21 | #include "Navigation/GNSS/SystemModel/MotionModel.hpp" | ||
| 22 | #include "Navigation/GNSS/SystemModel/InterFrequencyBiasModel.hpp" | ||
| 23 | #include "Navigation/GNSS/SystemModel/ReceiverClockModel.hpp" | ||
| 24 | |||
| 25 | namespace NAV::SPP | ||
| 26 | { | ||
| 27 | |||
| 28 | namespace States | ||
| 29 | { | ||
| 30 | |||
| 31 | constexpr size_t POS_STATE_COUNT = 4; ///< Amount of states to estimate for the position | ||
| 32 | constexpr size_t VEL_STATE_COUNT = 4; ///< Amount of states to estimate for the velocity | ||
| 33 | |||
| 34 | constexpr size_t POS_VEL_STATE_COUNT = 6; ///< Amount of states | ||
| 35 | |||
| 36 | /// Alias for the state key type | ||
| 37 | using StateKeyType = std::variant<Keys::MotionModelKey, Keys::RecvClkBias, Keys::RecvClkDrift, Keys::InterFreqBias>; | ||
| 38 | |||
| 39 | } // namespace States | ||
| 40 | |||
| 41 | namespace Meas | ||
| 42 | { | ||
| 43 | |||
| 44 | /// @brief Pseudorange measurement [m] | ||
| 45 | struct Psr | ||
| 46 | { | ||
| 47 | /// @brief Equal comparison operator | ||
| 48 | /// @param rhs Right-hand side | ||
| 49 | 728564 | bool operator==(const Psr& rhs) const { return satSigId == rhs.satSigId; } | |
| 50 | /// @brief Satellite Signal Id | ||
| 51 | SatSigId satSigId; | ||
| 52 | }; | ||
| 53 | /// @brief Range-rate (doppler) measurement [m/s] | ||
| 54 | struct Doppler | ||
| 55 | { | ||
| 56 | /// @brief Equal comparison operator | ||
| 57 | /// @param rhs Right-hand side | ||
| 58 | 945159 | bool operator==(const Doppler& rhs) const { return satSigId == rhs.satSigId; } | |
| 59 | /// @brief Satellite Signal Id | ||
| 60 | SatSigId satSigId; | ||
| 61 | }; | ||
| 62 | |||
| 63 | /// Alias for the measurement key type | ||
| 64 | using MeasKeyTypes = std::variant<Psr, Doppler>; | ||
| 65 | |||
| 66 | } // namespace Meas | ||
| 67 | |||
| 68 | } // namespace NAV::SPP | ||
| 69 | |||
| 70 | /// @brief Stream insertion operator overload | ||
| 71 | /// @param[in, out] os Output stream object to stream the time into | ||
| 72 | /// @param[in] obj Object to print | ||
| 73 | /// @return Returns the output stream object in order to chain stream insertions | ||
| 74 | std::ostream& operator<<(std::ostream& os, const NAV::SPP::Meas::Psr& obj); | ||
| 75 | |||
| 76 | /// @brief Stream insertion operator overload | ||
| 77 | /// @param[in, out] os Output stream object to stream the time into | ||
| 78 | /// @param[in] obj Object to print | ||
| 79 | /// @return Returns the output stream object in order to chain stream insertions | ||
| 80 | std::ostream& operator<<(std::ostream& os, const NAV::SPP::Meas::Doppler& obj); | ||
| 81 | |||
| 82 | /// @brief Stream insertion operator overload | ||
| 83 | /// @param[in, out] os Output stream object to stream the time into | ||
| 84 | /// @param[in] obj Object to print | ||
| 85 | /// @return Returns the output stream object in order to chain stream insertions | ||
| 86 | std::ostream& operator<<(std::ostream& os, const NAV::SPP::States::StateKeyType& obj); | ||
| 87 | |||
| 88 | /// @brief Stream insertion operator overload | ||
| 89 | /// @param[in, out] os Output stream object to stream the time into | ||
| 90 | /// @param[in] obj Object to print | ||
| 91 | /// @return Returns the output stream object in order to chain stream insertions | ||
| 92 | std::ostream& operator<<(std::ostream& os, const NAV::SPP::Meas::MeasKeyTypes& obj); | ||
| 93 | |||
| 94 | namespace std | ||
| 95 | { | ||
| 96 | /// @brief Hash function (needed for unordered_map) | ||
| 97 | template<> | ||
| 98 | struct hash<NAV::SPP::Meas::Psr> | ||
| 99 | { | ||
| 100 | /// @brief Hash function | ||
| 101 | /// @param[in] psr Pseudorange observation | ||
| 102 | 3095921 | size_t operator()(const NAV::SPP::Meas::Psr& psr) const | |
| 103 | { | ||
| 104 |
1/2✓ Branch 1 taken 3095931 times.
✗ Branch 2 not taken.
|
3095921 | return std::hash<NAV::SatSigId>()(psr.satSigId); |
| 105 | } | ||
| 106 | }; | ||
| 107 | /// @brief Hash function (needed for unordered_map) | ||
| 108 | template<> | ||
| 109 | struct hash<NAV::SPP::Meas::Doppler> | ||
| 110 | { | ||
| 111 | /// @brief Hash function | ||
| 112 | /// @param[in] doppler Doppler observation | ||
| 113 | 2184816 | size_t operator()(const NAV::SPP::Meas::Doppler& doppler) const | |
| 114 | { | ||
| 115 |
1/2✓ Branch 1 taken 2184818 times.
✗ Branch 2 not taken.
|
2184816 | return std::hash<NAV::SatSigId>()(doppler.satSigId) << 12; |
| 116 | } | ||
| 117 | }; | ||
| 118 | } // namespace std | ||
| 119 | |||
| 120 | #ifndef DOXYGEN_IGNORE | ||
| 121 | |||
| 122 | /// @brief Formatter | ||
| 123 | template<> | ||
| 124 | struct fmt::formatter<NAV::SPP::Meas::Psr> : fmt::formatter<std::string> | ||
| 125 | { | ||
| 126 | /// @brief Defines how to format structs | ||
| 127 | /// @param[in] psr Struct to format | ||
| 128 | /// @param[in, out] ctx Format context | ||
| 129 | /// @return Output iterator | ||
| 130 | template<typename FormatContext> | ||
| 131 | ✗ | auto format(const NAV::SPP::Meas::Psr& psr, FormatContext& ctx) const | |
| 132 | { | ||
| 133 | ✗ | return fmt::formatter<std::string>::format(fmt::format("psr({})", psr.satSigId), ctx); | |
| 134 | } | ||
| 135 | }; | ||
| 136 | |||
| 137 | /// @brief Formatter | ||
| 138 | template<> | ||
| 139 | struct fmt::formatter<NAV::SPP::Meas::Doppler> : fmt::formatter<std::string> | ||
| 140 | { | ||
| 141 | /// @brief Defines how to format structs | ||
| 142 | /// @param[in] doppler Struct to format | ||
| 143 | /// @param[in, out] ctx Format context | ||
| 144 | /// @return Output iterator | ||
| 145 | template<typename FormatContext> | ||
| 146 | ✗ | auto format(const NAV::SPP::Meas::Doppler& doppler, FormatContext& ctx) const | |
| 147 | { | ||
| 148 | ✗ | return fmt::formatter<std::string>::format(fmt::format("dop({})", doppler.satSigId), ctx); | |
| 149 | } | ||
| 150 | }; | ||
| 151 | |||
| 152 | /// @brief Formatter | ||
| 153 | template<> | ||
| 154 | struct fmt::formatter<NAV::SPP::States::StateKeyType> : fmt::formatter<std::string> | ||
| 155 | { | ||
| 156 | /// @brief Defines how to format structs | ||
| 157 | /// @param[in] state Struct to format | ||
| 158 | /// @param[in, out] ctx Format context | ||
| 159 | /// @return Output iterator | ||
| 160 | template<typename FormatContext> | ||
| 161 | 144240 | auto format(const NAV::SPP::States::StateKeyType& state, FormatContext& ctx) const | |
| 162 | { | ||
| 163 | using namespace NAV::Keys; // NOLINT(google-build-using-namespace) | ||
| 164 | |||
| 165 |
2/2✓ Branch 1 taken 57696 times.
✓ Branch 2 taken 86544 times.
|
144240 | if (const auto* s = std::get_if<MotionModelKey>(&state)) |
| 166 | { | ||
| 167 |
2/4✓ Branch 1 taken 57696 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 57696 times.
✗ Branch 6 not taken.
|
57696 | return fmt::formatter<std::string>::format(fmt::format("{}", *s), ctx); |
| 168 | } | ||
| 169 |
2/2✓ Branch 1 taken 19232 times.
✓ Branch 2 taken 67312 times.
|
86544 | if (const auto* recvClkErr = std::get_if<RecvClkBias>(&state)) |
| 170 | { | ||
| 171 |
2/4✓ Branch 1 taken 19232 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 19232 times.
✗ Branch 6 not taken.
|
19232 | return fmt::formatter<std::string>::format(fmt::format("{}", *recvClkErr), ctx); |
| 172 | } | ||
| 173 |
2/2✓ Branch 1 taken 19232 times.
✓ Branch 2 taken 48080 times.
|
67312 | if (const auto* recvClkDrift = std::get_if<RecvClkDrift>(&state)) |
| 174 | { | ||
| 175 |
2/4✓ Branch 1 taken 19232 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 19232 times.
✗ Branch 6 not taken.
|
19232 | return fmt::formatter<std::string>::format(fmt::format("{}", *recvClkDrift), ctx); |
| 176 | } | ||
| 177 |
1/2✓ Branch 1 taken 48080 times.
✗ Branch 2 not taken.
|
48080 | if (const auto* interFreqBias = std::get_if<NAV::Keys::InterFreqBias>(&state)) |
| 178 | { | ||
| 179 |
2/4✓ Branch 1 taken 48080 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 48080 times.
✗ Branch 6 not taken.
|
48080 | return fmt::formatter<std::string>::format(fmt::format("{}", *interFreqBias), ctx); |
| 180 | } | ||
| 181 | |||
| 182 | ✗ | return fmt::formatter<std::string>::format("ERROR", ctx); | |
| 183 | } | ||
| 184 | }; | ||
| 185 | |||
| 186 | /// @brief Formatter | ||
| 187 | template<> | ||
| 188 | struct fmt::formatter<NAV::SPP::Meas::MeasKeyTypes> : fmt::formatter<std::string> | ||
| 189 | { | ||
| 190 | /// @brief Defines how to format structs | ||
| 191 | /// @param[in] meas Struct to format | ||
| 192 | /// @param[in, out] ctx Format context | ||
| 193 | /// @return Output iterator | ||
| 194 | template<typename FormatContext> | ||
| 195 | ✗ | auto format(const NAV::SPP::Meas::MeasKeyTypes& meas, FormatContext& ctx) const | |
| 196 | { | ||
| 197 | ✗ | if (const auto* psr = std::get_if<NAV::SPP::Meas::Psr>(&meas)) | |
| 198 | { | ||
| 199 | ✗ | return fmt::formatter<std::string>::format(fmt::format("{}", *psr), ctx); | |
| 200 | } | ||
| 201 | ✗ | if (const auto* doppler = std::get_if<NAV::SPP::Meas::Doppler>(&meas)) | |
| 202 | { | ||
| 203 | ✗ | return fmt::formatter<std::string>::format(fmt::format("{}", *doppler), ctx); | |
| 204 | } | ||
| 205 | |||
| 206 | ✗ | return fmt::formatter<std::string>::format("ERROR", ctx); | |
| 207 | } | ||
| 208 | }; | ||
| 209 | |||
| 210 | #endif | ||
| 211 |