0.3.0
Loading...
Searching...
No Matches
SppSolution.hpp
Go to the documentation of this file.
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
13
14#pragma once
15
16#include <cstddef>
17#include <vector>
18#include <optional>
19#include <algorithm>
20
26
28
29#include "util/Assert.h"
32
33namespace NAV
34{
36class SppSolution : public PosVel
37{
38 public:
41 [[nodiscard]] static std::string type()
42 {
43 return "SppSolution";
44 }
45
48 [[nodiscard]] std::string getType() const override { return type(); }
49
52 [[nodiscard]] static std::vector<std::string> parentTypes()
53 {
54 auto parent = PosVel::parentTypes();
55 parent.push_back(PosVel::type());
56 return parent;
57 }
58
60 [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
61 {
63 desc.reserve(GetStaticDescriptorCount());
64 desc.emplace_back("Number satellites");
65 desc.emplace_back("Receiver clock bias GPS [s]");
66 desc.emplace_back("Receiver clock drift GPS [s/s]");
67 desc.emplace_back("Receiver clock bias StDev GPS [s]");
68 desc.emplace_back("Receiver clock drift StDev GPS [s/s]");
69 desc.emplace_back("Receiver clock bias GAL [s]");
70 desc.emplace_back("Receiver clock drift GAL [s/s]");
71 desc.emplace_back("Receiver clock bias StDev GAL [s]");
72 desc.emplace_back("Receiver clock drift StDev GAL [s/s]");
73 desc.emplace_back("Receiver clock bias GLO [s]");
74 desc.emplace_back("Receiver clock drift GLO [s/s]");
75 desc.emplace_back("Receiver clock bias StDev GLO [s]");
76 desc.emplace_back("Receiver clock drift StDev GLO [s/s]");
77 desc.emplace_back("Receiver clock bias BDS [s]");
78 desc.emplace_back("Receiver clock drift BDS [s/s]");
79 desc.emplace_back("Receiver clock bias StDev BDS [s]");
80 desc.emplace_back("Receiver clock drift StDev BDS [s/s]");
81 desc.emplace_back("Receiver clock bias QZSS [s]");
82 desc.emplace_back("Receiver clock drift QZSS [s/s]");
83 desc.emplace_back("Receiver clock bias StDev QZSS [s]");
84 desc.emplace_back("Receiver clock drift StDev QZSS [s/s]");
85 desc.emplace_back("Receiver clock bias IRNSS [s]");
86 desc.emplace_back("Receiver clock drift IRNSS [s/s]");
87 desc.emplace_back("Receiver clock bias StDev IRNSS [s]");
88 desc.emplace_back("Receiver clock drift StDev IRNSS [s/s]");
89 desc.emplace_back("Receiver clock bias SBAS [s]");
90 desc.emplace_back("Receiver clock drift SBAS [s/s]");
91 desc.emplace_back("Receiver clock bias StDev SBAS [s]");
92 desc.emplace_back("Receiver clock drift StDev SBAS [s/s]");
93 desc.emplace_back("HDOP");
94 desc.emplace_back("VDOP");
95 desc.emplace_back("PDOP");
96
97 return desc;
98 }
99
101 [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return PosVel::GetStaticDescriptorCount() + 32; }
102
104 [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); }
105
107 [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); }
108
112 [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
113 {
115 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
224 [[nodiscard]] std::vector<std::string> dynamicDataDescriptors() const override
225 {
226 std::vector<std::string> descriptors;
227 descriptors.reserve(interFrequencyBias.size() * 2 + satData.size() * 2);
228
229 for (const auto& bias : interFrequencyBias)
230 {
231 descriptors.push_back(fmt::format("{} Inter-freq bias [s]", bias.first));
232 descriptors.push_back(fmt::format("{} Inter-freq bias StDev [s]", bias.first));
233 }
234 for (const auto& [satId, satData] : satData)
235 {
236 descriptors.push_back(fmt::format("{} Elevation [deg]", satId));
237 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 }
251 {
252 for (size_t r = 0; r < static_cast<size_t>(_e_sppCovarianceMatrix->rows()); r++)
253 {
254 for (size_t c = r; c < static_cast<size_t>(_e_sppCovarianceMatrix->cols()); c++)
255 {
256 descriptors.push_back(fmt::format("Cov({},{})", _e_sppCovarianceMatrix->rowKeys().at(r), _e_sppCovarianceMatrix->colKeys().at(c)));
257 }
258 }
259 }
260
261 return descriptors;
262 }
263
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 }
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
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 }
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
349 size_t nSatellites = 0;
351 size_t nMeasPsr = 0;
353 size_t nMeasDopp = 0;
355 size_t nParam = 0;
356
358 double HDOP = std::nan("");
360 double VDOP = std::nan("");
362 double PDOP = std::nan("");
363
366
368 std::unordered_map<Frequency, UncertainValue<double>> interFrequencyBias;
369
371 struct SatData
372 {
373 double satElevation = 0.0;
374 double satAzimuth = 0.0;
375 };
376
378 std::vector<std::pair<SatId, SatData>> satData;
379
381 std::optional<KeyedMatrixXd<SPP::States::StateKeyType, SPP::States::StateKeyType>> _e_sppCovarianceMatrix;
382};
383
384} // namespace NAV
Assertion helpers.
#define INS_ASSERT(_EXPR)
Assert function wrapper.
Definition Assert.h:19
Code definitions.
Keys for the SPP algorithm for use inside the KeyedMatrices.
Position and Velocity Storage Class.
Receiver Clock information.
Structs identifying a unique satellite.
GNSS Satellite System.
@ GPS
Global Positioning System.
Definition SatelliteSystem.hpp:33
@ QZSS
Quasi-Zenith Satellite System.
Definition SatelliteSystem.hpp:37
@ GLO
Globalnaja nawigazionnaja sputnikowaja sistema (GLONASS)
Definition SatelliteSystem.hpp:35
@ GAL
Galileo.
Definition SatelliteSystem.hpp:34
@ SBAS
Satellite Based Augmentation System.
Definition SatelliteSystem.hpp:39
@ BDS
Beidou.
Definition SatelliteSystem.hpp:36
@ IRNSS
Indian Regional Navigation Satellite System.
Definition SatelliteSystem.hpp:38
Values with an uncertainty (Standard Deviation)
Position and Velocity Storage Class.
Definition PosVel.hpp:23
static constexpr size_t GetStaticDescriptorCount()
Get the amount of descriptors.
Definition PosVel.hpp:73
static std::vector< std::string > parentTypes()
Returns the parent types of the data class.
Definition PosVel.hpp:38
static std::vector< std::string > GetStaticDataDescriptors()
Returns a vector of data descriptors.
Definition PosVel.hpp:46
static std::string type()
Returns the type of the data class.
Definition PosVel.hpp:27
std::optional< double > getValueAt(size_t idx) const override
Get the value at the index.
Definition PosVel.hpp:84
SPP Algorithm output.
Definition SppSolution.hpp:37
double PDOP
PDOP value.
Definition SppSolution.hpp:362
std::optional< double > getDynamicDataAt(const std::string &descriptor) const override
Get the value for the descriptor.
Definition SppSolution.hpp:266
size_t staticDescriptorCount() const override
Get the amount of descriptors.
Definition SppSolution.hpp:107
size_t nSatellites
Amount of satellites used for the calculation.
Definition SppSolution.hpp:349
std::vector< std::pair< SatId, SatData > > satData
Extended data for each satellite.
Definition SppSolution.hpp:378
static std::string type()
Returns the type of the data class.
Definition SppSolution.hpp:41
static std::vector< std::string > GetStaticDataDescriptors()
Returns a vector of data descriptors.
Definition SppSolution.hpp:60
std::optional< double > getValueAt(size_t idx) const override
Get the value at the index.
Definition SppSolution.hpp:112
size_t nParam
Amount of Parameters estimated in this epoch.
Definition SppSolution.hpp:355
double VDOP
VDOP value.
Definition SppSolution.hpp:360
std::string getType() const override
Returns the type of the data class.
Definition SppSolution.hpp:48
std::vector< std::pair< std::string, double > > getDynamicData() const override
Returns a vector of data descriptors and values for the dynamic data.
Definition SppSolution.hpp:306
std::optional< KeyedMatrixXd< SPP::States::StateKeyType, SPP::States::StateKeyType > > _e_sppCovarianceMatrix
Covariance matrix in ECEF coordinates.
Definition SppSolution.hpp:381
static std::vector< std::string > parentTypes()
Returns the parent types of the data class.
Definition SppSolution.hpp:52
ReceiverClock recvClk
Estimated receiver clock parameter.
Definition SppSolution.hpp:365
double HDOP
HDOP value.
Definition SppSolution.hpp:358
std::unordered_map< Frequency, UncertainValue< double > > interFrequencyBias
Inter-frequency biases.
Definition SppSolution.hpp:368
std::vector< std::string > dynamicDataDescriptors() const override
Returns a vector of data descriptors for the dynamic data.
Definition SppSolution.hpp:224
std::vector< std::string > staticDataDescriptors() const override
Returns a vector of data descriptors.
Definition SppSolution.hpp:104
static constexpr size_t GetStaticDescriptorCount()
Get the amount of descriptors.
Definition SppSolution.hpp:101
size_t nMeasPsr
Amount of pseudorange measurements used to calculate the position solution.
Definition SppSolution.hpp:351
size_t nMeasDopp
Amount of doppler measurements used to calculate the velocity solution.
Definition SppSolution.hpp:353
Receiver Clock information.
Definition ReceiverClock.hpp:32
Satellite specific data.
Definition SppSolution.hpp:372
double satElevation
Satellite Elevation [rad].
Definition SppSolution.hpp:373
double satAzimuth
Satellite Azimuth [rad].
Definition SppSolution.hpp:374
Matrix which can be accessed by keys.
static const internal::all_t all
Used to request all rows or columns in KeyedMatrices.
Definition KeyedMatrix.hpp:1457