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 <vector>
17#include <optional>
18#include <algorithm>
19
25
27
28#include "util/Assert.h"
31
32namespace NAV
33{
35class SppSolution : public PosVel
36{
37 public:
40 [[nodiscard]] static std::string type()
41 {
42 return "SppSolution";
43 }
44
47 [[nodiscard]] std::string getType() const override { return type(); }
48
51 [[nodiscard]] static std::vector<std::string> parentTypes()
52 {
53 auto parent = PosVel::parentTypes();
54 parent.push_back(PosVel::type());
55 return parent;
56 }
57
59 [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
60 {
62 desc.reserve(GetStaticDescriptorCount());
63 desc.emplace_back("Number satellites");
64 desc.emplace_back("Receiver clock bias GPS [s]");
65 desc.emplace_back("Receiver clock drift GPS [s/s]");
66 desc.emplace_back("Receiver clock bias StDev GPS [s]");
67 desc.emplace_back("Receiver clock drift StDev GPS [s/s]");
68 desc.emplace_back("Receiver clock bias GAL [s]");
69 desc.emplace_back("Receiver clock drift GAL [s/s]");
70 desc.emplace_back("Receiver clock bias StDev GAL [s]");
71 desc.emplace_back("Receiver clock drift StDev GAL [s/s]");
72 desc.emplace_back("Receiver clock bias GLO [s]");
73 desc.emplace_back("Receiver clock drift GLO [s/s]");
74 desc.emplace_back("Receiver clock bias StDev GLO [s]");
75 desc.emplace_back("Receiver clock drift StDev GLO [s/s]");
76 desc.emplace_back("Receiver clock bias BDS [s]");
77 desc.emplace_back("Receiver clock drift BDS [s/s]");
78 desc.emplace_back("Receiver clock bias StDev BDS [s]");
79 desc.emplace_back("Receiver clock drift StDev BDS [s/s]");
80 desc.emplace_back("Receiver clock bias QZSS [s]");
81 desc.emplace_back("Receiver clock drift QZSS [s/s]");
82 desc.emplace_back("Receiver clock bias StDev QZSS [s]");
83 desc.emplace_back("Receiver clock drift StDev QZSS [s/s]");
84 desc.emplace_back("Receiver clock bias IRNSS [s]");
85 desc.emplace_back("Receiver clock drift IRNSS [s/s]");
86 desc.emplace_back("Receiver clock bias StDev IRNSS [s]");
87 desc.emplace_back("Receiver clock drift StDev IRNSS [s/s]");
88 desc.emplace_back("Receiver clock bias SBAS [s]");
89 desc.emplace_back("Receiver clock drift SBAS [s/s]");
90 desc.emplace_back("Receiver clock bias StDev SBAS [s]");
91 desc.emplace_back("Receiver clock drift StDev SBAS [s/s]");
92 desc.emplace_back("HDOP");
93 desc.emplace_back("VDOP");
94 desc.emplace_back("PDOP");
95
96 return desc;
97 }
98
100 [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return PosVel::GetStaticDescriptorCount() + 32; }
101
103 [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); }
104
106 [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); }
107
111 [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
112 {
114 if (idx < PosVel::GetStaticDescriptorCount()) { return PosVel::getValueAt(idx); }
115 switch (idx)
116 {
117 case PosVel::GetStaticDescriptorCount() + 0: // Number satellites
118 return static_cast<double>(nSatellites);
119
120 case PosVel::GetStaticDescriptorCount() + 1: // Receiver clock bias GPS [s]
121 if (const double* v = recvClk.biasFor(GPS)) { return *v; }
122 break;
123 case PosVel::GetStaticDescriptorCount() + 2: // Receiver clock drift GPS [s/s]
124 if (const double* v = recvClk.driftFor(GPS)) { return *v; }
125 break;
126 case PosVel::GetStaticDescriptorCount() + 3: // Receiver clock bias StDev GPS [s]
127 if (const double* v = recvClk.biasStdDevFor(GPS)) { return *v; }
128 break;
129 case PosVel::GetStaticDescriptorCount() + 4: // Receiver clock drift StDev GPS [s/s]
130 if (const double* v = recvClk.driftStdDevFor(GPS)) { return *v; }
131 break;
132
133 case PosVel::GetStaticDescriptorCount() + 5: // Receiver clock bias GAL [s]
134 if (const double* v = recvClk.biasFor(GAL)) { return *v; }
135 break;
136 case PosVel::GetStaticDescriptorCount() + 6: // Receiver clock drift GAL [s/s]
137 if (const double* v = recvClk.driftFor(GAL)) { return *v; }
138 break;
139 case PosVel::GetStaticDescriptorCount() + 7: // Receiver clock bias StDev GAL [s]
140 if (const double* v = recvClk.biasStdDevFor(GAL)) { return *v; }
141 break;
142 case PosVel::GetStaticDescriptorCount() + 8: // Receiver clock drift StDev GAL [s/s]
143 if (const double* v = recvClk.driftStdDevFor(GAL)) { return *v; }
144 break;
145
146 case PosVel::GetStaticDescriptorCount() + 9: // Receiver clock bias GLO [s]
147 if (const double* v = recvClk.biasFor(GLO)) { return *v; }
148 break;
149 case PosVel::GetStaticDescriptorCount() + 10: // Receiver clock drift GLO [s/s]
150 if (const double* v = recvClk.driftFor(GLO)) { return *v; }
151 break;
152 case PosVel::GetStaticDescriptorCount() + 11: // Receiver clock bias StDev GLO [s]
153 if (const double* v = recvClk.biasStdDevFor(GLO)) { return *v; }
154 break;
155 case PosVel::GetStaticDescriptorCount() + 12: // Receiver clock drift StDev GLO [s/s]
156 if (const double* v = recvClk.driftStdDevFor(GLO)) { return *v; }
157 break;
158
159 case PosVel::GetStaticDescriptorCount() + 13: // Receiver clock bias BDS [s]
160 if (const double* v = recvClk.biasFor(BDS)) { return *v; }
161 break;
162 case PosVel::GetStaticDescriptorCount() + 14: // Receiver clock drift BDS [s/s]
163 if (const double* v = recvClk.driftFor(BDS)) { return *v; }
164 break;
165 case PosVel::GetStaticDescriptorCount() + 15: // Receiver clock bias StDev BDS [s]
166 if (const double* v = recvClk.biasStdDevFor(BDS)) { return *v; }
167 break;
168 case PosVel::GetStaticDescriptorCount() + 16: // Receiver clock drift StDev BDS [s/s]
169 if (const double* v = recvClk.driftStdDevFor(BDS)) { return *v; }
170 break;
171
172 case PosVel::GetStaticDescriptorCount() + 17: // Receiver clock bias QZSS [s]
173 if (const double* v = recvClk.biasFor(QZSS)) { return *v; }
174 break;
175 case PosVel::GetStaticDescriptorCount() + 18: // Receiver clock drift QZSS [s/s]
176 if (const double* v = recvClk.driftFor(QZSS)) { return *v; }
177 break;
178 case PosVel::GetStaticDescriptorCount() + 19: // Receiver clock bias StDev QZSS [s]
179 if (const double* v = recvClk.biasStdDevFor(QZSS)) { return *v; }
180 break;
181 case PosVel::GetStaticDescriptorCount() + 20: // Receiver clock drift StDev QZSS [s/s]
182 if (const double* v = recvClk.driftStdDevFor(QZSS)) { return *v; }
183 break;
184
185 case PosVel::GetStaticDescriptorCount() + 21: // Receiver clock bias IRNSS [s]
186 if (const double* v = recvClk.biasFor(IRNSS)) { return *v; }
187 break;
188 case PosVel::GetStaticDescriptorCount() + 22: // Receiver clock drift IRNSS [s/s]
189 if (const double* v = recvClk.driftFor(IRNSS)) { return *v; }
190 break;
191 case PosVel::GetStaticDescriptorCount() + 23: // Receiver clock bias StDev IRNSS [s]
192 if (const double* v = recvClk.biasStdDevFor(IRNSS)) { return *v; }
193 break;
194 case PosVel::GetStaticDescriptorCount() + 24: // Receiver clock drift StDev IRNSS [s/s]
195 if (const double* v = recvClk.driftStdDevFor(IRNSS)) { return *v; }
196 break;
197
198 case PosVel::GetStaticDescriptorCount() + 25: // Receiver clock bias SBAS [s]
199 if (const double* v = recvClk.biasFor(SBAS)) { return *v; }
200 break;
201 case PosVel::GetStaticDescriptorCount() + 26: // Receiver clock drift SBAS [s/s]
202 if (const double* v = recvClk.driftFor(SBAS)) { return *v; }
203 break;
204 case PosVel::GetStaticDescriptorCount() + 27: // Receiver clock bias StDev SBAS [s]
205 if (const double* v = recvClk.biasStdDevFor(SBAS)) { return *v; }
206 break;
207 case PosVel::GetStaticDescriptorCount() + 28: // Receiver clock drift StDev SBAS [s/s]
208 if (const double* v = recvClk.driftStdDevFor(SBAS)) { return *v; }
209 break;
210 case PosVel::GetStaticDescriptorCount() + 29: // HDOP
211 return HDOP;
212 case PosVel::GetStaticDescriptorCount() + 30: // VDOP
213 return VDOP;
214 case PosVel::GetStaticDescriptorCount() + 31: // PDOP
215 return PDOP;
216 default:
217 return std::nullopt;
218 }
219 return std::nullopt;
220 }
221
223 [[nodiscard]] std::vector<std::string> dynamicDataDescriptors() const override
224 {
225 std::vector<std::string> descriptors;
226 descriptors.reserve(interFrequencyBias.size() * 2 + satData.size() * 2);
227
228 for (const auto& bias : interFrequencyBias)
229 {
230 descriptors.push_back(fmt::format("{} Inter-freq bias [s]", bias.first));
231 descriptors.push_back(fmt::format("{} Inter-freq bias StDev [s]", bias.first));
232 }
233 for (const auto& [satId, satData] : satData)
234 {
235 descriptors.push_back(fmt::format("{} Elevation [deg]", satId));
236 descriptors.push_back(fmt::format("{} Azimuth [deg]", satId));
237 // descriptors.push_back(fmt::format("{} Satellite clock bias [s]", satData.first));
238 // descriptors.push_back(fmt::format("{} Satellite clock drift [s/s]", satData.first));
239 // descriptors.push_back(fmt::format("{} SatPos ECEF X [m]", satData.first));
240 // descriptors.push_back(fmt::format("{} SatPos ECEF Y [m]", satData.first));
241 // descriptors.push_back(fmt::format("{} SatPos ECEF Z [m]", satData.first));
242 // descriptors.push_back(fmt::format("{} SatPos Latitude [deg]", satData.first));
243 // descriptors.push_back(fmt::format("{} SatPos Longitude [deg]", satData.first));
244 // descriptors.push_back(fmt::format("{} SatPos Altitude [m]", satData.first));
245 // descriptors.push_back(fmt::format("{} SatVel ECEF X [m/s]", satData.first));
246 // descriptors.push_back(fmt::format("{} SatVel ECEF Y [m/s]", satData.first));
247 // descriptors.push_back(fmt::format("{} SatVel ECEF Z [m/s]", satData.first));
248 }
249
250 return descriptors;
251 }
252
255 [[nodiscard]] std::optional<double> getDynamicDataAt(const std::string& descriptor) const override
256 {
257 for (const auto& bias : interFrequencyBias)
258 {
259 if (descriptor == fmt::format("{} Inter-freq bias [s]", bias.first)) { return bias.second.value; }
260 if (descriptor == fmt::format("{} Inter-freq bias StDev [s]", bias.first)) { return bias.second.stdDev; }
261 }
262 for (const auto& [satId, satData] : satData)
263 {
264 if (descriptor == fmt::format("{} Elevation [deg]", satId)) { return rad2deg(satData.satElevation); }
265 if (descriptor == fmt::format("{} Azimuth [deg]", satId)) { return rad2deg(satData.satAzimuth); }
266 // if (descriptor == fmt::format("{} Satellite clock bias [s]", satData.first)) { return satData.second.satClock.bias; }
267 // if (descriptor == fmt::format("{} Satellite clock drift [s/s]", satData.first)) { return satData.second.satClock.drift; }
268 // if (descriptor == fmt::format("{} SatPos ECEF X [m]", satData.first)) { return satData.second.e_satPos.x(); }
269 // if (descriptor == fmt::format("{} SatPos ECEF Y [m]", satData.first)) { return satData.second.e_satPos.y(); }
270 // if (descriptor == fmt::format("{} SatPos ECEF Z [m]", satData.first)) { return satData.second.e_satPos.z(); }
271 // if (descriptor == fmt::format("{} SatPos Latitude [deg]", satData.first)) { return rad2deg(satData.second.lla_satPos.x()); }
272 // if (descriptor == fmt::format("{} SatPos Longitude [deg]", satData.first)) { return rad2deg(satData.second.lla_satPos.y()); }
273 // if (descriptor == fmt::format("{} SatPos Altitude [m]", satData.first)) { return satData.second.lla_satPos.z(); }
274 // if (descriptor == fmt::format("{} SatVel ECEF X [m/s]", satData.first)) { return satData.second.e_satVel.x(); }
275 // if (descriptor == fmt::format("{} SatVel ECEF Y [m/s]", satData.first)) { return satData.second.e_satVel.y(); }
276 // if (descriptor == fmt::format("{} SatVel ECEF Z [m/s]", satData.first)) { return satData.second.e_satVel.z(); }
277 }
278 return std::nullopt;
279 }
280
282 [[nodiscard]] std::vector<std::pair<std::string, double>> getDynamicData() const override
283 {
284 std::vector<std::pair<std::string, double>> dynData;
285 dynData.reserve(interFrequencyBias.size() * 2 + satData.size() * 2);
286
287 for (const auto& bias : interFrequencyBias)
288 {
289 dynData.emplace_back(fmt::format("{} Inter-freq bias [s]", bias.first), bias.second.value);
290 dynData.emplace_back(fmt::format("{} Inter-freq bias StDev [s]", bias.first), bias.second.stdDev);
291 }
292 for (const auto& [satId, satData] : satData)
293 {
294 dynData.emplace_back(fmt::format("{} Elevation [deg]", satId), rad2deg(satData.satElevation));
295 dynData.emplace_back(fmt::format("{} Azimuth [deg]", satId), rad2deg(satData.satAzimuth));
296 // dynData.emplace_back(fmt::format("{} Satellite clock bias [s]", satData.first), satData.second.satClock.bias);
297 // dynData.emplace_back(fmt::format("{} Satellite clock drift [s/s]", satData.first), satData.second.satClock.drift);
298 // dynData.emplace_back(fmt::format("{} SatPos ECEF X [m]", satData.first), satData.second.e_satPos.x());
299 // dynData.emplace_back(fmt::format("{} SatPos ECEF Y [m]", satData.first), satData.second.e_satPos.y());
300 // dynData.emplace_back(fmt::format("{} SatPos ECEF Z [m]", satData.first), satData.second.e_satPos.z());
301 // dynData.emplace_back(fmt::format("{} SatPos Latitude [deg]", satData.first), rad2deg(satData.second.lla_satPos.x()));
302 // dynData.emplace_back(fmt::format("{} SatPos Longitude [deg]", satData.first), rad2deg(satData.second.lla_satPos.y()));
303 // dynData.emplace_back(fmt::format("{} SatPos Altitude [m]", satData.first), satData.second.lla_satPos.z());
304 // dynData.emplace_back(fmt::format("{} SatVel ECEF X [m/s]", satData.first), satData.second.e_satVel.x());
305 // dynData.emplace_back(fmt::format("{} SatVel ECEF Y [m/s]", satData.first), satData.second.e_satVel.y());
306 // dynData.emplace_back(fmt::format("{} SatVel ECEF Z [m/s]", satData.first), satData.second.e_satVel.z());
307 }
308 return dynData;
309 }
310
311 // --------------------------------------------------------- Public Members ------------------------------------------------------------
312
314 size_t nSatellites = 0;
316 size_t nMeasPsr = 0;
318 size_t nMeasDopp = 0;
320 size_t nParam = 0;
321
323 double HDOP = std::nan("");
325 double VDOP = std::nan("");
327 double PDOP = std::nan("");
328
331
333 std::unordered_map<Frequency, UncertainValue<double>> interFrequencyBias;
334
336 struct SatData
337 {
338 double satElevation = 0.0;
339 double satAzimuth = 0.0;
340 };
341
343 std::vector<std::pair<SatId, SatData>> satData;
344
347 void addEvent(const std::string& event) { _events.push_back(event); }
348
349 private:
351 Eigen::Vector3d _e_positionStdev = Eigen::Vector3d::Zero() * std::nan("");
353 Eigen::Vector3d _n_positionStdev = Eigen::Vector3d::Zero() * std::nan("");
354
356 Eigen::Vector3d _e_velocityStdev = Eigen::Vector3d::Zero() * std::nan("");
358 Eigen::Vector3d _n_velocityStdev = Eigen::Vector3d::Zero() * std::nan("");
359
361 std::optional<KeyedMatrixXd<SPP::States::StateKeyType, SPP::States::StateKeyType>> _e_covarianceMatrix;
362
364 std::optional<KeyedMatrixXd<SPP::States::StateKeyType, SPP::States::StateKeyType>> _n_covarianceMatrix;
365};
366
367} // 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, Velocity and Attitude 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)
std::vector< std::string > _events
List of events.
Definition NodeData.hpp:127
Position, Velocity and Attitude 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:36
Eigen::Vector3d _n_positionStdev
Standard deviation of Position in local navigation frame coordinates [m].
Definition SppSolution.hpp:353
double PDOP
PDOP value.
Definition SppSolution.hpp:327
std::optional< double > getDynamicDataAt(const std::string &descriptor) const override
Get the value for the descriptor.
Definition SppSolution.hpp:255
size_t staticDescriptorCount() const override
Get the amount of descriptors.
Definition SppSolution.hpp:106
size_t nSatellites
Amount of satellites used for the calculation.
Definition SppSolution.hpp:314
std::vector< std::pair< SatId, SatData > > satData
Extended data for each satellite.
Definition SppSolution.hpp:343
static std::string type()
Returns the type of the data class.
Definition SppSolution.hpp:40
static std::vector< std::string > GetStaticDataDescriptors()
Returns a vector of data descriptors.
Definition SppSolution.hpp:59
std::optional< double > getValueAt(size_t idx) const override
Get the value at the index.
Definition SppSolution.hpp:111
size_t nParam
Amount of Parameters estimated in this epoch.
Definition SppSolution.hpp:320
double VDOP
VDOP value.
Definition SppSolution.hpp:325
std::string getType() const override
Returns the type of the data class.
Definition SppSolution.hpp:47
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:282
static std::vector< std::string > parentTypes()
Returns the parent types of the data class.
Definition SppSolution.hpp:51
ReceiverClock recvClk
Estimated receiver clock parameter.
Definition SppSolution.hpp:330
void addEvent(const std::string &event)
Adds an event to the event list.
Definition SppSolution.hpp:347
Eigen::Vector3d _e_positionStdev
Standard deviation of Position in ECEF coordinates [m].
Definition SppSolution.hpp:351
double HDOP
HDOP value.
Definition SppSolution.hpp:323
Eigen::Vector3d _e_velocityStdev
Standard deviation of Velocity in earth coordinates [m/s].
Definition SppSolution.hpp:356
std::unordered_map< Frequency, UncertainValue< double > > interFrequencyBias
Inter-frequency biases.
Definition SppSolution.hpp:333
std::optional< KeyedMatrixXd< SPP::States::StateKeyType, SPP::States::StateKeyType > > _n_covarianceMatrix
Covariance matrix in local navigation coordinates.
Definition SppSolution.hpp:364
std::vector< std::string > dynamicDataDescriptors() const override
Returns a vector of data descriptors for the dynamic data.
Definition SppSolution.hpp:223
std::vector< std::string > staticDataDescriptors() const override
Returns a vector of data descriptors.
Definition SppSolution.hpp:103
static constexpr size_t GetStaticDescriptorCount()
Get the amount of descriptors.
Definition SppSolution.hpp:100
Eigen::Vector3d _n_velocityStdev
Standard deviation of Velocity in navigation coordinates [m/s].
Definition SppSolution.hpp:358
size_t nMeasPsr
Amount of pseudorange measurements used to calculate the position solution.
Definition SppSolution.hpp:316
std::optional< KeyedMatrixXd< SPP::States::StateKeyType, SPP::States::StateKeyType > > _e_covarianceMatrix
Covariance matrix in ECEF coordinates.
Definition SppSolution.hpp:361
size_t nMeasDopp
Amount of doppler measurements used to calculate the velocity solution.
Definition SppSolution.hpp:318
Receiver Clock information.
Definition ReceiverClock.hpp:32
const double * biasFor(SatelliteSystem satSys) const
Get the bias for the given satellite system.
Definition ReceiverClock.hpp:81
const double * biasStdDevFor(SatelliteSystem satSys) const
Get the bias StdDev for the given satellite system.
Definition ReceiverClock.hpp:98
const double * driftStdDevFor(SatelliteSystem satSys) const
Get the drift StdDev for the given satellite system.
Definition ReceiverClock.hpp:132
const double * driftFor(SatelliteSystem satSys) const
Get the drift for the given satellite system.
Definition ReceiverClock.hpp:115
Satellite specific data.
Definition SppSolution.hpp:337
double satElevation
Satellite Elevation [rad].
Definition SppSolution.hpp:338
double satAzimuth
Satellite Azimuth [rad].
Definition SppSolution.hpp:339
Matrix which can be accessed by keys.