INSTINCT Code Coverage Report

Directory: src/
File: NodeData/GNSS/SppSolution.hpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 61 191 31.9%
Functions: 7 11 63.6%
Branches: 55 301 18.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 SppSolution.hpp
10 /// @brief SPP Algorithm output
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2022-05-28
13
14 #pragma once
15
16 #include <cstddef>
17 #include <vector>
18 #include <optional>
19 #include <algorithm>
20
21 #include "Navigation/GNSS/Core/SatelliteIdentifier.hpp"
22 #include "Navigation/GNSS/Core/Code.hpp"
23 #include "Navigation/GNSS/Core/SatelliteSystem.hpp"
24 #include "Navigation/GNSS/Positioning/ReceiverClock.hpp"
25 #include "Navigation/GNSS/Positioning/SPP/Keys.hpp"
26
27 #include "NodeData/State/PosVel.hpp"
28
29 #include "util/Assert.h"
30 #include "util/Container/KeyedMatrix.hpp"
31 #include "util/Container/UncertainValue.hpp"
32
33 namespace NAV
34 {
35 /// SPP Algorithm output
36 class SppSolution : public PosVel
37 {
38 public:
39 /// @brief Returns the type of the data class
40 /// @return The data type
41 15324 [[nodiscard]] static std::string type()
42 {
43
1/2
✓ Branch 1 taken 15324 times.
✗ Branch 2 not taken.
 30648 return "SppSolution";
44 }
45
46 /// @brief Returns the type of the data class
47 /// @return The data type
48 ✗ [[nodiscard]] std::string getType() const override { return type(); }
49
50 /// @brief Returns the parent types of the data class
51 /// @return The parent data types
52 114 [[nodiscard]] static std::vector<std::string> parentTypes()
53 {
54 114 auto parent = PosVel::parentTypes();
55
2/4
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
 114 parent.push_back(PosVel::type());
56 114 return parent;
57 ✗ }
58
59 /// @brief Returns a vector of data descriptors
60 7214 [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
61 {
62 7214 auto desc = PosVel::GetStaticDataDescriptors();
63
1/2
✓ Branch 2 taken 7214 times.
✗ Branch 3 not taken.
 7214 desc.reserve(GetStaticDescriptorCount());
64
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Number satellites");
65
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock bias GPS [s]");
66
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock drift GPS [s/s]");
67
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock bias StDev GPS [s]");
68
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock drift StDev GPS [s/s]");
69
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock bias GAL [s]");
70
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock drift GAL [s/s]");
71
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock bias StDev GAL [s]");
72
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock drift StDev GAL [s/s]");
73
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock bias GLO [s]");
74
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock drift GLO [s/s]");
75
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock bias StDev GLO [s]");
76
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock drift StDev GLO [s/s]");
77
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock bias BDS [s]");
78
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock drift BDS [s/s]");
79
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock bias StDev BDS [s]");
80
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock drift StDev BDS [s/s]");
81
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock bias QZSS [s]");
82
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock drift QZSS [s/s]");
83
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock bias StDev QZSS [s]");
84
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock drift StDev QZSS [s/s]");
85
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock bias IRNSS [s]");
86
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock drift IRNSS [s/s]");
87
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock bias StDev IRNSS [s]");
88
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock drift StDev IRNSS [s/s]");
89
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock bias SBAS [s]");
90
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock drift SBAS [s/s]");
91
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock bias StDev SBAS [s]");
92
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("Receiver clock drift StDev SBAS [s/s]");
93
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("HDOP");
94
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("VDOP");
95
1/2
✓ Branch 1 taken 7214 times.
✗ Branch 2 not taken.
 7214 desc.emplace_back("PDOP");
96
97 7214 return desc;
98 ✗ }
99
100 /// @brief Get the amount of descriptors
101 14426 [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return PosVel::GetStaticDescriptorCount() + 32; }
102
103 /// @brief Returns a vector of data descriptors
104 ✗ [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); }
105
106 /// @brief Get the amount of descriptors
107 3606 [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); }
108
109 /// @brief Get the value at the index
110 /// @param idx Index corresponding to data descriptor order
111 /// @return Value if in the observation
112 3606 [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
113 {
114
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3606 times.
 3606 INS_ASSERT(idx < GetStaticDescriptorCount());
115
1/2
✓ Branch 1 taken 3606 times.
✗ Branch 2 not taken.
 3606 if (idx < PosVel::GetStaticDescriptorCount()) { return PosVel::getValueAt(idx); }
116 ✗ switch (idx)
117 {
118 ✗ case PosVel::GetStaticDescriptorCount() + 0: // Number satellites
119 ✗ return static_cast<double>(nSatellites);
120
121 ✗ case PosVel::GetStaticDescriptorCount() + 1: // Receiver clock bias GPS [s]
122 ✗ if (const double* v = recvClk.biasFor(GPS)) { return *v; }
123 ✗ break;
124 ✗ case PosVel::GetStaticDescriptorCount() + 2: // Receiver clock drift GPS [s/s]
125 ✗ if (const double* v = recvClk.driftFor(GPS)) { return *v; }
126 ✗ break;
127 ✗ case PosVel::GetStaticDescriptorCount() + 3: // Receiver clock bias StDev GPS [s]
128 ✗ if (const double* v = recvClk.biasStdDevFor(GPS)) { return *v; }
129 ✗ break;
130 ✗ case PosVel::GetStaticDescriptorCount() + 4: // Receiver clock drift StDev GPS [s/s]
131 ✗ if (const double* v = recvClk.driftStdDevFor(GPS)) { return *v; }
132 ✗ break;
133
134 ✗ case PosVel::GetStaticDescriptorCount() + 5: // Receiver clock bias GAL [s]
135 ✗ if (const double* v = recvClk.biasFor(GAL)) { return *v; }
136 ✗ break;
137 ✗ case PosVel::GetStaticDescriptorCount() + 6: // Receiver clock drift GAL [s/s]
138 ✗ if (const double* v = recvClk.driftFor(GAL)) { return *v; }
139 ✗ break;
140 ✗ case PosVel::GetStaticDescriptorCount() + 7: // Receiver clock bias StDev GAL [s]
141 ✗ if (const double* v = recvClk.biasStdDevFor(GAL)) { return *v; }
142 ✗ break;
143 ✗ case PosVel::GetStaticDescriptorCount() + 8: // Receiver clock drift StDev GAL [s/s]
144 ✗ if (const double* v = recvClk.driftStdDevFor(GAL)) { return *v; }
145 ✗ break;
146
147 ✗ case PosVel::GetStaticDescriptorCount() + 9: // Receiver clock bias GLO [s]
148 ✗ if (const double* v = recvClk.biasFor(GLO)) { return *v; }
149 ✗ break;
150 ✗ case PosVel::GetStaticDescriptorCount() + 10: // Receiver clock drift GLO [s/s]
151 ✗ if (const double* v = recvClk.driftFor(GLO)) { return *v; }
152 ✗ break;
153 ✗ case PosVel::GetStaticDescriptorCount() + 11: // Receiver clock bias StDev GLO [s]
154 ✗ if (const double* v = recvClk.biasStdDevFor(GLO)) { return *v; }
155 ✗ break;
156 ✗ case PosVel::GetStaticDescriptorCount() + 12: // Receiver clock drift StDev GLO [s/s]
157 ✗ if (const double* v = recvClk.driftStdDevFor(GLO)) { return *v; }
158 ✗ break;
159
160 ✗ case PosVel::GetStaticDescriptorCount() + 13: // Receiver clock bias BDS [s]
161 ✗ if (const double* v = recvClk.biasFor(BDS)) { return *v; }
162 ✗ break;
163 ✗ case PosVel::GetStaticDescriptorCount() + 14: // Receiver clock drift BDS [s/s]
164 ✗ if (const double* v = recvClk.driftFor(BDS)) { return *v; }
165 ✗ break;
166 ✗ case PosVel::GetStaticDescriptorCount() + 15: // Receiver clock bias StDev BDS [s]
167 ✗ if (const double* v = recvClk.biasStdDevFor(BDS)) { return *v; }
168 ✗ break;
169 ✗ case PosVel::GetStaticDescriptorCount() + 16: // Receiver clock drift StDev BDS [s/s]
170 ✗ if (const double* v = recvClk.driftStdDevFor(BDS)) { return *v; }
171 ✗ break;
172
173 ✗ case PosVel::GetStaticDescriptorCount() + 17: // Receiver clock bias QZSS [s]
174 ✗ if (const double* v = recvClk.biasFor(QZSS)) { return *v; }
175 ✗ break;
176 ✗ case PosVel::GetStaticDescriptorCount() + 18: // Receiver clock drift QZSS [s/s]
177 ✗ if (const double* v = recvClk.driftFor(QZSS)) { return *v; }
178 ✗ break;
179 ✗ case PosVel::GetStaticDescriptorCount() + 19: // Receiver clock bias StDev QZSS [s]
180 ✗ if (const double* v = recvClk.biasStdDevFor(QZSS)) { return *v; }
181 ✗ break;
182 ✗ case PosVel::GetStaticDescriptorCount() + 20: // Receiver clock drift StDev QZSS [s/s]
183 ✗ if (const double* v = recvClk.driftStdDevFor(QZSS)) { return *v; }
184 ✗ break;
185
186 ✗ case PosVel::GetStaticDescriptorCount() + 21: // Receiver clock bias IRNSS [s]
187 ✗ if (const double* v = recvClk.biasFor(IRNSS)) { return *v; }
188 ✗ break;
189 ✗ case PosVel::GetStaticDescriptorCount() + 22: // Receiver clock drift IRNSS [s/s]
190 ✗ if (const double* v = recvClk.driftFor(IRNSS)) { return *v; }
191 ✗ break;
192 ✗ case PosVel::GetStaticDescriptorCount() + 23: // Receiver clock bias StDev IRNSS [s]
193 ✗ if (const double* v = recvClk.biasStdDevFor(IRNSS)) { return *v; }
194 ✗ break;
195 ✗ case PosVel::GetStaticDescriptorCount() + 24: // Receiver clock drift StDev IRNSS [s/s]
196 ✗ if (const double* v = recvClk.driftStdDevFor(IRNSS)) { return *v; }
197 ✗ break;
198
199 ✗ case PosVel::GetStaticDescriptorCount() + 25: // Receiver clock bias SBAS [s]
200 ✗ if (const double* v = recvClk.biasFor(SBAS)) { return *v; }
201 ✗ break;
202 ✗ case PosVel::GetStaticDescriptorCount() + 26: // Receiver clock drift SBAS [s/s]
203 ✗ if (const double* v = recvClk.driftFor(SBAS)) { return *v; }
204 ✗ break;
205 ✗ case PosVel::GetStaticDescriptorCount() + 27: // Receiver clock bias StDev SBAS [s]
206 ✗ if (const double* v = recvClk.biasStdDevFor(SBAS)) { return *v; }
207 ✗ break;
208 ✗ case PosVel::GetStaticDescriptorCount() + 28: // Receiver clock drift StDev SBAS [s/s]
209 ✗ if (const double* v = recvClk.driftStdDevFor(SBAS)) { return *v; }
210 ✗ break;
211 ✗ case PosVel::GetStaticDescriptorCount() + 29: // HDOP
212 ✗ return HDOP;
213 ✗ case PosVel::GetStaticDescriptorCount() + 30: // VDOP
214 ✗ return VDOP;
215 ✗ case PosVel::GetStaticDescriptorCount() + 31: // PDOP
216 ✗ return PDOP;
217 ✗ default:
218 ✗ return std::nullopt;
219 }
220 ✗ return std::nullopt;
221 }
222
223 /// @brief Returns a vector of data descriptors for the dynamic data
224 601 [[nodiscard]] std::vector<std::string> dynamicDataDescriptors() const override
225 {
226 601 std::vector<std::string> descriptors;
227
1/2
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
 601 descriptors.reserve(interFrequencyBias.size() * 2 + satData.size() * 2);
228
229
2/2
✓ Branch 4 taken 3005 times.
✓ Branch 5 taken 601 times.
 3606 for (const auto& bias : interFrequencyBias)
230 {
231
1/2
✓ Branch 2 taken 3005 times.
✗ Branch 3 not taken.
 6010 descriptors.push_back(fmt::format("{} Inter-freq bias [s]", bias.first));
232
1/2
✓ Branch 2 taken 3005 times.
✗ Branch 3 not taken.
 6010 descriptors.push_back(fmt::format("{} Inter-freq bias StDev [s]", bias.first));
233 }
234
2/2
✓ Branch 6 taken 8067 times.
✓ Branch 7 taken 601 times.
 8668 for (const auto& [satId, satData] : satData)
235 {
236
1/2
✓ Branch 2 taken 8067 times.
✗ Branch 3 not taken.
 16134 descriptors.push_back(fmt::format("{} Elevation [deg]", satId));
237
1/2
✓ Branch 2 taken 8067 times.
✗ Branch 3 not taken.
 16134 descriptors.push_back(fmt::format("{} Azimuth [deg]", satId));
238 // descriptors.push_back(fmt::format("{} Satellite clock bias [s]", satData.first));
239 // descriptors.push_back(fmt::format("{} Satellite clock drift [s/s]", satData.first));
240 // descriptors.push_back(fmt::format("{} SatPos ECEF X [m]", satData.first));
241 // descriptors.push_back(fmt::format("{} SatPos ECEF Y [m]", satData.first));
242 // descriptors.push_back(fmt::format("{} SatPos ECEF Z [m]", satData.first));
243 // descriptors.push_back(fmt::format("{} SatPos Latitude [deg]", satData.first));
244 // descriptors.push_back(fmt::format("{} SatPos Longitude [deg]", satData.first));
245 // descriptors.push_back(fmt::format("{} SatPos Altitude [m]", satData.first));
246 // descriptors.push_back(fmt::format("{} SatVel ECEF X [m/s]", satData.first));
247 // descriptors.push_back(fmt::format("{} SatVel ECEF Y [m/s]", satData.first));
248 // descriptors.push_back(fmt::format("{} SatVel ECEF Z [m/s]", satData.first));
249 }
250
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 if (_e_sppCovarianceMatrix)
251 {
252
2/2
✓ Branch 2 taken 9015 times.
✓ Branch 3 taken 601 times.
 9616 for (size_t r = 0; r < static_cast<size_t>(_e_sppCovarianceMatrix->rows()); r++)
253 {
254
2/2
✓ Branch 2 taken 72120 times.
✓ Branch 3 taken 9015 times.
 81135 for (size_t c = r; c < static_cast<size_t>(_e_sppCovarianceMatrix->cols()); c++)
255 {
256
3/6
✓ Branch 3 taken 72120 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 72120 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 72120 times.
✗ Branch 13 not taken.
 144240 descriptors.push_back(fmt::format("Cov({},{})", _e_sppCovarianceMatrix->rowKeys().at(r), _e_sppCovarianceMatrix->colKeys().at(c)));
257 }
258 }
259 }
260
261 601 return descriptors;
262 ✗ }
263
264 /// @brief Get the value for the descriptor
265 /// @return Value if in the observation
266 ✗ [[nodiscard]] std::optional<double> getDynamicDataAt(const std::string& descriptor) const override
267 {
268 ✗ for (const auto& bias : interFrequencyBias)
269 {
270 ✗ if (descriptor == fmt::format("{} Inter-freq bias [s]", bias.first)) { return bias.second.value; }
271 ✗ if (descriptor == fmt::format("{} Inter-freq bias StDev [s]", bias.first)) { return bias.second.stdDev; }
272 }
273 ✗ for (const auto& [satId, satData] : satData)
274 {
275 ✗ if (descriptor == fmt::format("{} Elevation [deg]", satId)) { return rad2deg(satData.satElevation); }
276 ✗ if (descriptor == fmt::format("{} Azimuth [deg]", satId)) { return rad2deg(satData.satAzimuth); }
277 // if (descriptor == fmt::format("{} Satellite clock bias [s]", satData.first)) { return satData.second.satClock.bias; }
278 // if (descriptor == fmt::format("{} Satellite clock drift [s/s]", satData.first)) { return satData.second.satClock.drift; }
279 // if (descriptor == fmt::format("{} SatPos ECEF X [m]", satData.first)) { return satData.second.e_satPos.x(); }
280 // if (descriptor == fmt::format("{} SatPos ECEF Y [m]", satData.first)) { return satData.second.e_satPos.y(); }
281 // if (descriptor == fmt::format("{} SatPos ECEF Z [m]", satData.first)) { return satData.second.e_satPos.z(); }
282 // if (descriptor == fmt::format("{} SatPos Latitude [deg]", satData.first)) { return rad2deg(satData.second.lla_satPos.x()); }
283 // if (descriptor == fmt::format("{} SatPos Longitude [deg]", satData.first)) { return rad2deg(satData.second.lla_satPos.y()); }
284 // if (descriptor == fmt::format("{} SatPos Altitude [m]", satData.first)) { return satData.second.lla_satPos.z(); }
285 // if (descriptor == fmt::format("{} SatVel ECEF X [m/s]", satData.first)) { return satData.second.e_satVel.x(); }
286 // if (descriptor == fmt::format("{} SatVel ECEF Y [m/s]", satData.first)) { return satData.second.e_satVel.y(); }
287 // if (descriptor == fmt::format("{} SatVel ECEF Z [m/s]", satData.first)) { return satData.second.e_satVel.z(); }
288 }
289 ✗ if (_e_sppCovarianceMatrix)
290 {
291 ✗ for (size_t r = 0; r < static_cast<size_t>(_e_sppCovarianceMatrix->rows()); r++)
292 {
293 ✗ for (size_t c = r; c < static_cast<size_t>(_e_sppCovarianceMatrix->cols()); c++)
294 {
295 ✗ if (descriptor == fmt::format("Cov({},{})", _e_sppCovarianceMatrix->rowKeys().at(r), _e_sppCovarianceMatrix->colKeys().at(c)))
296 {
297 ✗ return (*_e_sppCovarianceMatrix)(all, all)(static_cast<int>(r), static_cast<int>(c));
298 }
299 }
300 }
301 }
302 ✗ return std::nullopt;
303 }
304
305 /// @brief Returns a vector of data descriptors and values for the dynamic data
306 ✗ [[nodiscard]] std::vector<std::pair<std::string, double>> getDynamicData() const override
307 {
308 ✗ std::vector<std::pair<std::string, double>> dynData;
309 ✗ dynData.reserve(interFrequencyBias.size() * 2 + satData.size() * 2);
310
311 ✗ for (const auto& bias : interFrequencyBias)
312 {
313 ✗ dynData.emplace_back(fmt::format("{} Inter-freq bias [s]", bias.first), bias.second.value);
314 ✗ dynData.emplace_back(fmt::format("{} Inter-freq bias StDev [s]", bias.first), bias.second.stdDev);
315 }
316 ✗ for (const auto& [satId, satData] : satData)
317 {
318 ✗ dynData.emplace_back(fmt::format("{} Elevation [deg]", satId), rad2deg(satData.satElevation));
319 ✗ dynData.emplace_back(fmt::format("{} Azimuth [deg]", satId), rad2deg(satData.satAzimuth));
320 // dynData.emplace_back(fmt::format("{} Satellite clock bias [s]", satData.first), satData.second.satClock.bias);
321 // dynData.emplace_back(fmt::format("{} Satellite clock drift [s/s]", satData.first), satData.second.satClock.drift);
322 // dynData.emplace_back(fmt::format("{} SatPos ECEF X [m]", satData.first), satData.second.e_satPos.x());
323 // dynData.emplace_back(fmt::format("{} SatPos ECEF Y [m]", satData.first), satData.second.e_satPos.y());
324 // dynData.emplace_back(fmt::format("{} SatPos ECEF Z [m]", satData.first), satData.second.e_satPos.z());
325 // dynData.emplace_back(fmt::format("{} SatPos Latitude [deg]", satData.first), rad2deg(satData.second.lla_satPos.x()));
326 // dynData.emplace_back(fmt::format("{} SatPos Longitude [deg]", satData.first), rad2deg(satData.second.lla_satPos.y()));
327 // dynData.emplace_back(fmt::format("{} SatPos Altitude [m]", satData.first), satData.second.lla_satPos.z());
328 // dynData.emplace_back(fmt::format("{} SatVel ECEF X [m/s]", satData.first), satData.second.e_satVel.x());
329 // dynData.emplace_back(fmt::format("{} SatVel ECEF Y [m/s]", satData.first), satData.second.e_satVel.y());
330 // dynData.emplace_back(fmt::format("{} SatVel ECEF Z [m/s]", satData.first), satData.second.e_satVel.z());
331 }
332 ✗ if (_e_sppCovarianceMatrix)
333 {
334 ✗ for (size_t r = 0; r < static_cast<size_t>(_e_sppCovarianceMatrix->rows()); r++)
335 {
336 ✗ for (size_t c = r; c < static_cast<size_t>(_e_sppCovarianceMatrix->cols()); c++)
337 {
338 ✗ dynData.emplace_back(fmt::format("Cov({},{})", _e_sppCovarianceMatrix->rowKeys().at(r), _e_sppCovarianceMatrix->colKeys().at(c)),
339 ✗ (*_e_sppCovarianceMatrix)(all, all)(static_cast<int>(r), static_cast<int>(c)));
340 }
341 }
342 }
343 ✗ return dynData;
344 ✗ }
345
346 // --------------------------------------------------------- Public Members ------------------------------------------------------------
347
348 /// Amount of satellites used for the calculation
349 size_t nSatellites = 0;
350 /// Amount of pseudorange measurements used to calculate the position solution
351 size_t nMeasPsr = 0;
352 /// Amount of doppler measurements used to calculate the velocity solution
353 size_t nMeasDopp = 0;
354 /// Amount of Parameters estimated in this epoch
355 size_t nParam = 0;
356
357 /// HDOP value
358 double HDOP = std::nan("");
359 /// VDOP value
360 double VDOP = std::nan("");
361 /// PDOP value
362 double PDOP = std::nan("");
363
364 /// Estimated receiver clock parameter
365 ReceiverClock recvClk{ {} };
366
367 /// Inter-frequency biases
368 std::unordered_map<Frequency, UncertainValue<double>> interFrequencyBias;
369
370 /// Satellite specific data
371 struct SatData
372 {
373 double satElevation = 0.0; ///< Satellite Elevation [rad]
374 double satAzimuth = 0.0; ///< Satellite Azimuth [rad]
375 };
376
377 /// Extended data for each satellite
378 std::vector<std::pair<SatId, SatData>> satData;
379
380 /// Covariance matrix in ECEF coordinates
381 std::optional<KeyedMatrixXd<SPP::States::StateKeyType, SPP::States::StateKeyType>> _e_sppCovarianceMatrix;
382 };
383
384 } // namespace NAV
385