INSTINCT Code Coverage Report

Directory: src/
File: Navigation/GNSS/Positioning/SPP/Keys.hpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 6 40 15.0%
Functions: 4 8 50.0%
Branches: 2 24 8.3%
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 241621 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 314230 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 1076719 size_t operator()(const NAV::SPP::Meas::Psr& psr) const
103 {
104
1/2
✓ Branch 1 taken 1076719 times.
✗ Branch 2 not taken.
 1076719 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 762910 size_t operator()(const NAV::SPP::Meas::Doppler& doppler) const
114 {
115
1/2
✓ Branch 1 taken 762910 times.
✗ Branch 2 not taken.
 762910 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 ✗ auto format(const NAV::SPP::States::StateKeyType& state, FormatContext& ctx) const
162 {
163 using namespace NAV::Keys; // NOLINT(google-build-using-namespace)
164
165 ✗ if (const auto* s = std::get_if<MotionModelKey>(&state))
166 {
167 ✗ switch (*s)
168 {
169 ✗ case PosX:
170 ✗ return fmt::formatter<std::string>::format("PosX", ctx);
171 ✗ case PosY:
172 ✗ return fmt::formatter<std::string>::format("PosY", ctx);
173 ✗ case PosZ:
174 ✗ return fmt::formatter<std::string>::format("PosZ", ctx);
175 ✗ case VelX:
176 ✗ return fmt::formatter<std::string>::format("VelX", ctx);
177 ✗ case VelY:
178 ✗ return fmt::formatter<std::string>::format("VelY", ctx);
179 ✗ case VelZ:
180 ✗ return fmt::formatter<std::string>::format("VelZ", ctx);
181 ✗ case MotionModelKey_COUNT:
182 ✗ return fmt::formatter<std::string>::format("MotionModelKey_COUNT", ctx);
183 }
184 }
185 ✗ if (const auto* recvClkErr = std::get_if<RecvClkBias>(&state))
186 {
187 ✗ return fmt::formatter<std::string>::format(fmt::format("RecvClkBias({})", recvClkErr->satSys), ctx);
188 }
189 ✗ if (const auto* recvClkDrift = std::get_if<RecvClkDrift>(&state))
190 {
191 ✗ return fmt::formatter<std::string>::format(fmt::format("RecvClkDrift({})", recvClkDrift->satSys), ctx);
192 }
193 ✗ if (const auto* interFreqBias = std::get_if<NAV::Keys::InterFreqBias>(&state))
194 {
195 ✗ return fmt::formatter<std::string>::format(fmt::format("InterFreqBias({})", interFreqBias->freq), ctx);
196 }
197
198 ✗ return fmt::formatter<std::string>::format("ERROR", ctx);
199 }
200 };
201
202 /// @brief Formatter
203 template<>
204 struct fmt::formatter<NAV::SPP::Meas::MeasKeyTypes> : fmt::formatter<std::string>
205 {
206 /// @brief Defines how to format structs
207 /// @param[in] meas Struct to format
208 /// @param[in, out] ctx Format context
209 /// @return Output iterator
210 template<typename FormatContext>
211 ✗ auto format(const NAV::SPP::Meas::MeasKeyTypes& meas, FormatContext& ctx) const
212 {
213 ✗ if (const auto* psr = std::get_if<NAV::SPP::Meas::Psr>(&meas))
214 {
215 ✗ return fmt::formatter<std::string>::format(fmt::format("psr({})", psr->satSigId), ctx);
216 }
217 ✗ if (const auto* doppler = std::get_if<NAV::SPP::Meas::Doppler>(&meas))
218 {
219 ✗ return fmt::formatter<std::string>::format(fmt::format("doppler({})", doppler->satSigId), ctx);
220 }
221
222 ✗ return fmt::formatter<std::string>::format("ERROR", ctx);
223 }
224 };
225
226 #endif
227