INSTINCT Code Coverage Report

Directory: src/
File: Navigation/GNSS/Positioning/SPP/Keys.hpp
Date: 2025-06-02 15:19:59
Exec Total Coverage
Lines: 6 26 23.1%
Functions: 4 8 50.0%
Branches: 2 16 12.5%
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 189219 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 245280 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 840123 size_t operator()(const NAV::SPP::Meas::Psr& psr) const
103 {
104
1/2
✓ Branch 1 taken 840123 times.
✗ Branch 2 not taken.
 840123 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 590232 size_t operator()(const NAV::SPP::Meas::Doppler& doppler) const
114 {
115
1/2
✓ Branch 1 taken 590232 times.
✗ Branch 2 not taken.
 590232 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 ✗ return fmt::formatter<std::string>::format(fmt::format("{}", *s), ctx);
168 }
169 ✗ if (const auto* recvClkErr = std::get_if<RecvClkBias>(&state))
170 {
171 ✗ return fmt::formatter<std::string>::format(fmt::format("{}", *recvClkErr), ctx);
172 }
173 ✗ if (const auto* recvClkDrift = std::get_if<RecvClkDrift>(&state))
174 {
175 ✗ return fmt::formatter<std::string>::format(fmt::format("{}", *recvClkDrift), ctx);
176 }
177 ✗ if (const auto* interFreqBias = std::get_if<NAV::Keys::InterFreqBias>(&state))
178 {
179 ✗ 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