INSTINCT Code Coverage Report


Directory: src/
File: Navigation/GNSS/Positioning/RTK/Keys.hpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 36 65 55.4%
Functions: 15 19 78.9%
Branches: 17 76 22.4%

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 RTK algorithm for use inside the KeyedMatrices
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2023-12-21
13
14 #pragma once
15
16 #include <cstdint>
17 #include <vector>
18 #include <variant>
19 #include <fmt/format.h>
20
21 #include "Navigation/GNSS/Core/SatelliteIdentifier.hpp"
22 #include "NodeData/GNSS/GnssObs.hpp"
23
24 namespace NAV::RTK
25 {
26
27 namespace States
28 {
29
30 /// @brief State Keys of the Kalman filter
31 enum KFStates : uint8_t
32 {
33 PosX, ///< Position ECEF_X [m]
34 PosY, ///< Position ECEF_Y [m]
35 PosZ, ///< Position ECEF_Z [m]
36 VelX, ///< Velocity ECEF_X [m/s]
37 VelY, ///< Velocity ECEF_Y [m/s]
38 VelZ, ///< Velocity ECEF_Z [m/s]
39 KFStates_COUNT, ///< Count
40 };
41 /// @brief Double differenced N_br^1s = N_br^s - N_br^1 ambiguity [cycles] (one for each satellite signal, except for the pivot satellites)
42 struct AmbiguityDD
43 {
44 /// @brief Constructor
45 /// @param[in] satSigId Satellite Signal Id
46 280905 explicit AmbiguityDD(const SatSigId& satSigId) : satSigId(satSigId) {}
47 /// @brief Equal comparison operator
48 /// @param rhs Right-hand side
49 464498 bool operator==(const AmbiguityDD& rhs) const { return satSigId == rhs.satSigId; }
50 /// @brief Satellite Signal Id
51 SatSigId satSigId;
52 };
53
54 /// Alias for the state key type
55 using StateKeyType = std::variant<KFStates, AmbiguityDD>;
56 /// @brief Vector with all position and velocity state keys
57 inline static const std::vector<StateKeyType> PosVel = { KFStates::PosX, KFStates::PosY, KFStates::PosZ,
58 KFStates::VelX, KFStates::VelY, KFStates::VelZ };
59 /// @brief All position keys
60 inline static const std::vector<StateKeyType> Pos = { KFStates::PosX, KFStates::PosY, KFStates::PosZ };
61 /// @brief All velocity keys
62 inline static const std::vector<StateKeyType> Vel = { KFStates::VelX, KFStates::VelY, KFStates::VelZ };
63
64 } // namespace States
65
66 namespace Meas
67 {
68
69 /// @brief Double differenced pseudorange measurement psr_br^1s [m] (one for each satellite signal, referenced to the pivot satellite)
70 struct PsrDD
71 {
72 /// @brief Equal comparison operator
73 /// @param rhs Right-hand side
74 235790 bool operator==(const PsrDD& rhs) const { return satSigId == rhs.satSigId; }
75 /// @brief Satellite Signal Id
76 SatSigId satSigId;
77 };
78 /// @brief Double differenced carrier-phase measurement phi_br^1s [m] (one for each satellite signal, referenced to the pivot satellite)
79 struct CarrierDD
80 {
81 /// @brief Equal comparison operator
82 /// @param rhs Right-hand side
83 259369 bool operator==(const CarrierDD& rhs) const { return satSigId == rhs.satSigId; }
84 /// @brief Satellite Signal Id
85 SatSigId satSigId;
86 };
87 /// @brief Double differenced range-rate (doppler) measurement d_br^1s [m/s] (one for each satellite signal, referenced to the pivot satellite)
88 struct DopplerDD
89 {
90 /// @brief Equal comparison operator
91 /// @param rhs Right-hand side
92 306527 bool operator==(const DopplerDD& rhs) const { return satSigId == rhs.satSigId; }
93 /// @brief Satellite Signal Id
94 SatSigId satSigId;
95 };
96
97 /// Alias for the measurement key type
98 using MeasKeyTypes = std::variant<PsrDD, CarrierDD, DopplerDD, States::AmbiguityDD>;
99
100 /// @brief Single Observation key
101 template<typename ReceiverType>
102 struct SingleObs
103 {
104 /// @brief Constructor
105 /// @param[in] satSigId Signal id
106 /// @param[in] recvType Receiver Type
107 /// @param[in] obsType Observation Type
108 616380 SingleObs(const SatSigId& satSigId, ReceiverType recvType, GnssObs::ObservationType obsType)
109 616380 : satSigId(satSigId), recvType(recvType), obsType(obsType) {}
110
111 SatSigId satSigId; ///< Signal id
112 ReceiverType recvType; ///< Receiver Type
113 GnssObs::ObservationType obsType; ///< Observation Type
114
115 /// @brief Equal comparison operator
116 /// @param rhs Right-hand side
117 616380 bool operator==(const SingleObs<ReceiverType>& rhs) const
118 {
119 616380 return satSigId == rhs.satSigId
120
1/2
✓ Branch 0 taken 616380 times.
✗ Branch 1 not taken.
616380 && recvType == rhs.recvType
121
2/4
✓ Branch 0 taken 616380 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 616380 times.
✗ Branch 3 not taken.
1232760 && obsType == rhs.obsType;
122 }
123 };
124
125 /// @brief Ambiguity Observation key
126 template<typename ReceiverType>
127 struct AmbObs
128 {
129 /// @brief Constructor
130 /// @param[in] satSigId Signal id
131 /// @param[in] recvType Receiver Type
132 AmbObs(const SatSigId& satSigId, ReceiverType recvType)
133 : satSigId(satSigId), recvType(recvType) {}
134
135 SatSigId satSigId; ///< Signal id
136 ReceiverType recvType; ///< Receiver Type
137
138 /// @brief Equal comparison operator
139 /// @param rhs Right-hand side
140 bool operator==(const AmbObs<ReceiverType>& rhs) const
141 {
142 return satSigId == rhs.satSigId && recvType == rhs.recvType;
143 }
144 };
145
146 } // namespace Meas
147
148 } // namespace NAV::RTK
149
150 namespace std
151 {
152 /// @brief Hash function (needed for unordered_map)
153 template<>
154 struct hash<NAV::RTK::States::AmbiguityDD>
155 {
156 /// @brief Hash function
157 /// @param[in] ambDD Double differenced ambiguity
158 1063558 size_t operator()(const NAV::RTK::States::AmbiguityDD& ambDD) const
159 {
160
1/2
✓ Branch 1 taken 1063558 times.
✗ Branch 2 not taken.
1063558 return NAV::RTK::States::KFStates_COUNT + std::hash<NAV::SatSigId>()(ambDD.satSigId);
161 }
162 };
163 /// @brief Hash function (needed for unordered_map)
164 template<>
165 struct hash<NAV::RTK::Meas::PsrDD>
166 {
167 /// @brief Hash function
168 /// @param[in] psrDD Double differenced pseudorange
169 679433 size_t operator()(const NAV::RTK::Meas::PsrDD& psrDD) const
170 {
171
1/2
✓ Branch 1 taken 679433 times.
✗ Branch 2 not taken.
679433 return std::hash<NAV::SatSigId>()(psrDD.satSigId);
172 }
173 };
174 /// @brief Hash function (needed for unordered_map)
175 template<>
176 struct hash<NAV::RTK::Meas::CarrierDD>
177 {
178 /// @brief Hash function
179 /// @param[in] cpDD Double differenced carrier-phase
180 703012 size_t operator()(const NAV::RTK::Meas::CarrierDD& cpDD) const
181 {
182
1/2
✓ Branch 1 taken 703012 times.
✗ Branch 2 not taken.
703012 return std::hash<NAV::SatSigId>()(cpDD.satSigId) << 12;
183 }
184 };
185 /// @brief Hash function (needed for unordered_map)
186 template<>
187 struct hash<NAV::RTK::Meas::DopplerDD>
188 {
189 /// @brief Hash function
190 /// @param[in] dDD Double differenced doppler
191 750170 size_t operator()(const NAV::RTK::Meas::DopplerDD& dDD) const
192 {
193
1/2
✓ Branch 1 taken 750170 times.
✗ Branch 2 not taken.
750170 return std::hash<NAV::SatSigId>()(dDD.satSigId) << 24;
194 }
195 };
196 /// @brief Hash function (needed for unordered_map)
197 template<typename ReceiverType>
198 struct hash<NAV::RTK::Meas::SingleObs<ReceiverType>>
199 {
200 /// @brief Hash function
201 /// @param[in] obs Single Observation
202 1616676 size_t operator()(const NAV::RTK::Meas::SingleObs<ReceiverType>& obs) const
203 {
204
1/2
✓ Branch 1 taken 1616676 times.
✗ Branch 2 not taken.
1616676 auto hash1 = std::hash<NAV::SatSigId>()(obs.satSigId);
205 1616676 auto hash2 = static_cast<size_t>(obs.obsType);
206 1616676 auto hash3 = static_cast<size_t>(obs.recvType);
207
208 1616676 return (hash1 << 4) | (hash2 << 2) | hash3;
209 }
210 };
211 /// @brief Hash function (needed for unordered_map)
212 template<typename ReceiverType>
213 struct hash<NAV::RTK::Meas::AmbObs<ReceiverType>>
214 {
215 /// @brief Hash function
216 /// @param[in] obs Single Ambiguity Observation
217 size_t operator()(const NAV::RTK::Meas::AmbObs<ReceiverType>& obs) const
218 {
219 auto hash1 = std::hash<NAV::SatSigId>()(obs.satSigId);
220 auto hash2 = static_cast<size_t>(obs.recvType);
221
222 return (hash1 << 2) | hash2;
223 }
224 };
225 } // namespace std
226
227 #ifndef DOXYGEN_IGNORE
228
229 /// @brief Formatter
230 template<>
231 struct fmt::formatter<NAV::RTK::States::KFStates> : fmt::formatter<const char*>
232 {
233 /// @brief Defines how to format structs
234 /// @param[in] state Struct to format
235 /// @param[in, out] ctx Format context
236 /// @return Output iterator
237 template<typename FormatContext>
238 auto format(const NAV::RTK::States::KFStates& state, FormatContext& ctx) const
239 {
240 using namespace NAV::RTK::States; // NOLINT(google-build-using-namespace)
241
242 switch (state)
243 {
244 case PosX:
245 return fmt::formatter<const char*>::format("PosX", ctx);
246 case PosY:
247 return fmt::formatter<const char*>::format("PosY", ctx);
248 case PosZ:
249 return fmt::formatter<const char*>::format("PosZ", ctx);
250 case VelX:
251 return fmt::formatter<const char*>::format("VelX", ctx);
252 case VelY:
253 return fmt::formatter<const char*>::format("VelY", ctx);
254 case VelZ:
255 return fmt::formatter<const char*>::format("VelZ", ctx);
256 case KFStates_COUNT:
257 return fmt::formatter<const char*>::format("KFStates_COUNT", ctx);
258 }
259
260 return fmt::formatter<const char*>::format("ERROR", ctx);
261 }
262 };
263
264 /// @brief Formatter
265 template<>
266 struct fmt::formatter<NAV::RTK::States::AmbiguityDD> : fmt::formatter<std::string>
267 {
268 /// @brief Defines how to format structs
269 /// @param[in] amb Struct to format
270 /// @param[in, out] ctx Format context
271 /// @return Output iterator
272 template<typename FormatContext>
273 23626 auto format(const NAV::RTK::States::AmbiguityDD& amb, FormatContext& ctx) const
274 {
275
1/2
✓ Branch 3 taken 23626 times.
✗ Branch 4 not taken.
47252 return fmt::formatter<std::string>::format(fmt::format("Amb({})", amb.satSigId), ctx);
276 }
277 };
278
279 /// @brief Formatter
280 template<>
281 struct fmt::formatter<NAV::RTK::Meas::PsrDD> : fmt::formatter<std::string>
282 {
283 /// @brief Defines how to format structs
284 /// @param[in] psrDD Struct to format
285 /// @param[in, out] ctx Format context
286 /// @return Output iterator
287 template<typename FormatContext>
288 auto format(const NAV::RTK::Meas::PsrDD& psrDD, FormatContext& ctx) const
289 {
290 return fmt::formatter<std::string>::format(fmt::format("psrDD({})", psrDD.satSigId), ctx);
291 }
292 };
293
294 /// @brief Formatter
295 template<>
296 struct fmt::formatter<NAV::RTK::Meas::CarrierDD> : fmt::formatter<std::string>
297 {
298 /// @brief Defines how to format structs
299 /// @param[in] phiDD Struct to format
300 /// @param[in, out] ctx Format context
301 /// @return Output iterator
302 template<typename FormatContext>
303 auto format(const NAV::RTK::Meas::CarrierDD& phiDD, FormatContext& ctx) const
304 {
305 return fmt::formatter<std::string>::format(fmt::format("phiDD({})", phiDD.satSigId), ctx);
306 }
307 };
308
309 /// @brief Formatter
310 template<>
311 struct fmt::formatter<NAV::RTK::Meas::DopplerDD> : fmt::formatter<std::string>
312 {
313 /// @brief Defines how to format structs
314 /// @param[in] dDD Struct to format
315 /// @param[in, out] ctx Format context
316 /// @return Output iterator
317 template<typename FormatContext>
318 auto format(const NAV::RTK::Meas::DopplerDD& dDD, FormatContext& ctx) const
319 {
320 return fmt::formatter<std::string>::format(fmt::format("dopDD({})", dDD.satSigId), ctx);
321 }
322 };
323
324 /// @brief Formatter
325 template<>
326 struct fmt::formatter<NAV::RTK::States::StateKeyType> : fmt::formatter<std::string>
327 {
328 /// @brief Defines how to format structs
329 /// @param[in] state Struct to format
330 /// @param[in, out] ctx Format context
331 /// @return Output iterator
332 template<typename FormatContext>
333 3 auto format(const NAV::RTK::States::StateKeyType& state, FormatContext& ctx) const
334 {
335 using namespace NAV::RTK::States; // NOLINT(google-build-using-namespace)
336
337
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
3 if (const auto* s = std::get_if<NAV::RTK::States::KFStates>(&state))
338 {
339 return fmt::formatter<std::string>::format(fmt::format("{}", *s), ctx);
340 }
341
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
3 if (const auto* amb = std::get_if<NAV::RTK::States::AmbiguityDD>(&state))
342 {
343
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
6 return fmt::formatter<std::string>::format(fmt::format("{}", *amb), ctx);
344 }
345
346 return fmt::formatter<std::string>::format("ERROR", ctx);
347 }
348 };
349
350 /// @brief Formatter
351 template<>
352 struct fmt::formatter<NAV::RTK::Meas::MeasKeyTypes> : fmt::formatter<std::string>
353 {
354 /// @brief Defines how to format structs
355 /// @param[in] meas Struct to format
356 /// @param[in, out] ctx Format context
357 /// @return Output iterator
358 template<typename FormatContext>
359 23579 auto format(const NAV::RTK::Meas::MeasKeyTypes& meas, FormatContext& ctx) const
360 {
361
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 23579 times.
23579 if (const auto* psrDD = std::get_if<NAV::RTK::Meas::PsrDD>(&meas))
362 {
363 return fmt::formatter<std::string>::format(fmt::format("{}", *psrDD), ctx);
364 }
365
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 23579 times.
23579 if (const auto* phiDD = std::get_if<NAV::RTK::Meas::CarrierDD>(&meas))
366 {
367 return fmt::formatter<std::string>::format(fmt::format("{}", *phiDD), ctx);
368 }
369
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 23579 times.
23579 if (const auto* dDD = std::get_if<NAV::RTK::Meas::DopplerDD>(&meas))
370 {
371 return fmt::formatter<std::string>::format(fmt::format("{}", *dDD), ctx);
372 }
373
1/2
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
23579 if (const auto* ambDD = std::get_if<NAV::RTK::States::AmbiguityDD>(&meas))
374 {
375
1/2
✓ Branch 3 taken 23579 times.
✗ Branch 4 not taken.
47158 return fmt::formatter<std::string>::format(fmt::format("{}", *ambDD), ctx);
376 }
377
378 return fmt::formatter<std::string>::format("ERROR", ctx);
379 }
380 };
381
382 /// @brief Formatter
383 template<typename ReceiverType>
384 struct fmt::formatter<NAV::RTK::Meas::SingleObs<ReceiverType>> : fmt::formatter<std::string>
385 {
386 /// @brief Defines how to format structs
387 /// @param[in] obs Struct to format
388 /// @param[in, out] ctx Format context
389 /// @return Output iterator
390 template<typename FormatContext>
391 auto format(const NAV::RTK::Meas::SingleObs<ReceiverType>& obs, FormatContext& ctx) const
392 {
393 return fmt::formatter<std::string>::format(fmt::format("obs({}_{:5}_{})", obs.obsType, obs.recvType, obs.satSigId), ctx);
394 }
395 };
396
397 /// @brief Formatter
398 template<typename ReceiverType>
399 struct fmt::formatter<NAV::RTK::Meas::AmbObs<ReceiverType>> : fmt::formatter<std::string>
400 {
401 /// @brief Defines how to format structs
402 /// @param[in] obs Struct to format
403 /// @param[in, out] ctx Format context
404 /// @return Output iterator
405 template<typename FormatContext>
406 auto format(const NAV::RTK::Meas::AmbObs<ReceiverType>& obs, FormatContext& ctx) const
407 {
408 return fmt::formatter<std::string>::format(fmt::format("Amb({:5}_{})", obs.recvType, obs.satSigId), ctx);
409 }
410 };
411
412 #endif
413