INSTINCT Code Coverage Report

Directory: src/
File: Navigation/GNSS/Positioning/SPP/Algorithm.cpp
Date: 2025-07-19 10:51:51
Exec Total Coverage
Lines: 219 372 58.9%
Functions: 14 23 60.9%
Branches: 269 840 32.0%
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 Algorithm.cpp
10 /// @brief Single Point Positioning Algorithm
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2023-12-20
13
14 #include "Algorithm.hpp"
15 #include <algorithm>
16 #include <spdlog/common.h>
17 #include <spdlog/spdlog.h>
18
19 #include "Navigation/Constants.hpp"
20 #include "Navigation/GNSS/Core/Frequency.hpp"
21 #include "Navigation/GNSS/Positioning/Observation.hpp"
22 #include "Navigation/GNSS/Positioning/SPP/Keys.hpp"
23 #include "Navigation/Transformations/CoordinateFrames.hpp"
24 #include "util/Logger.hpp"
25 #include <fmt/format.h>
26
27 #include "internal/gui/widgets/EnumCombo.hpp"
28 #include "internal/gui/widgets/HelpMarker.hpp"
29
30 #include "Navigation/Atmosphere/Ionosphere/IonosphericCorrections.hpp"
31 #include "Navigation/Math/KeyedLeastSquares.hpp"
32
33 #include "util/Container/STL.hpp"
34
35 namespace NAV
36 {
37 namespace SPP
38 {
39
40 ✗ bool Algorithm::ShowGuiWidgets(const char* id, float itemWidth, float unitWidth)
41 {
42 ✗ bool changed = false;
43
44 ✗ changed |= _obsFilter.ShowGuiWidgets<ReceiverType>(id, itemWidth);
45
46 ✗ ImGui::SetNextItemWidth(itemWidth);
47 ✗ changed |= gui::widgets::EnumCombo(fmt::format("Estimation algorithm##{}", id).c_str(), _estimatorType);
48
49 ✗ if (!canEstimateInterFrequencyBias()) { ImGui::BeginDisabled(); }
50 ✗ changed |= ImGui::Checkbox(fmt::format("Estimate inter-frequency biases##{}", id).c_str(), &_estimateInterFreqBiases);
51 ✗ if (!canEstimateInterFrequencyBias()) { ImGui::EndDisabled(); }
52
53 ✗ changed |= _obsEstimator.ShowGuiWidgets<ReceiverType>(id, itemWidth);
54
55 ✗ if (_estimatorType == EstimatorType::KalmanFilter)
56 {
57 ✗ changed |= _kalmanFilter.ShowGuiWidgets(id, _obsFilter.isObsTypeUsed(GnssObs::Doppler),
58 ✗ _obsFilter.getSystemFilter().toVector().size() != 1,
59 ✗ _estimateInterFreqBiases && canEstimateInterFrequencyBias(), itemWidth, unitWidth);
60 }
61
62 ✗ return changed;
63 }
64
65 27 void Algorithm::reset()
66 {
67
3/6
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
 27 _receiver = Receiver(Rover, _obsFilter.getSystemFilter().toVector());
68 27 _obsFilter.reset();
69
3/6
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
 27 _kalmanFilter.reset(_obsFilter.getSystemFilter().toVector());
70 27 _lastUpdate.reset();
71 27 }
72
73 945 std::shared_ptr<SppSolution> Algorithm::calcSppSolution(const std::shared_ptr<const GnssObs>& gnssObs,
74 const std::vector<const GnssNavInfo*>& gnssNavInfos,
75 const std::string& nameId)
76 {
77 945 _receiver.gnssObs = gnssObs;
78
79
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 945 times.
 945 if (gnssNavInfos.empty())
80 {
81 ✗ LOG_ERROR("{}: [{}] SPP cannot calculate position because no navigation data provided.", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST));
82 ✗ return nullptr;
83 }
84 LOG_DATA("{}: [{}] Calculating SPP", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST));
85
86 // Collection of all connected Ionospheric Corrections
87
1/2
✓ Branch 1 taken 945 times.
✗ Branch 2 not taken.
 945 auto ionosphericCorrections = std::make_shared<const IonosphericCorrections>(gnssNavInfos);
88
89
3/4
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 936 times.
✓ Branch 5 taken 936 times.
✗ Branch 6 not taken.
 945 double dt = _lastUpdate.empty() ? 0.0 : static_cast<double>((_receiver.gnssObs->insTime - _lastUpdate).count());
90 LOG_DATA("{}: dt = {}s", nameId, dt);
91 945 _lastUpdate = _receiver.gnssObs->insTime;
92
93
1/2
✓ Branch 1 taken 945 times.
✗ Branch 2 not taken.
 945 auto sppSol = std::make_shared<SppSolution>();
94 945 sppSol->insTime = _receiver.gnssObs->insTime;
95
96
2/6
✗ Branch 0 not taken.
✓ Branch 1 taken 945 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 945 times.
 945 if (_estimatorType == EstimatorType::KalmanFilter && _kalmanFilter.isInitialized())
97 {
98 ✗ _kalmanFilter.predict(dt, _receiver.lla_posMarker, nameId);
99 ✗ assignKalmanFilterResult(_kalmanFilter.getState(), _kalmanFilter.getErrorCovarianceMatrix(), nameId);
100 }
101
102 945 constexpr size_t N_ITER_MAX_LSQ = 10;
103
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 945 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 945 size_t nIter = _estimatorType == EstimatorType::KalmanFilter && _kalmanFilter.isInitialized() ? 1 : N_ITER_MAX_LSQ;
104
1/2
✓ Branch 1 taken 945 times.
✗ Branch 2 not taken.
 945 Eigen::Vector3d e_oldPos = _receiver.e_posMarker;
105 945 bool accuracyAchieved = false;
106
107
1/2
✓ Branch 1 taken 945 times.
✗ Branch 2 not taken.
 945 KeyedMatrixXd<Meas::MeasKeyTypes, States::StateKeyType> H;
108
109
1/2
✓ Branch 0 taken 2245 times.
✗ Branch 1 not taken.
 2245 for (size_t iteration = 0; iteration < nIter; iteration++)
110 {
111 LOG_DATA("{}: [{}] iteration {}/{}", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), iteration + 1, nIter);
112 LOG_DATA("{}: [{}] e_pos = {}", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), _receiver.e_posMarker.transpose());
113 LOG_DATA("{}: [{}] e_vel = {}", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), _receiver.e_vel.transpose());
114 LOG_DATA("{}: [{}] lla_pos = {:.6f}°, {:.6f}°, {:.3f}m", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST),
115 rad2deg(_receiver.lla_posMarker.x()), rad2deg(_receiver.lla_posMarker.y()), _receiver.lla_posMarker.z());
116
2/2
✓ Branch 5 taken 3753 times.
✓ Branch 6 taken 2245 times.
 5998 for ([[maybe_unused]] const auto& satSys : _receiver.recvClk.satelliteSystems)
117 {
118 LOG_DATA("{}: [{}] {}", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), satSys);
119 LOG_DATA("{}: [{}] clkBias = {} [s] (StdDev = {})", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), *_receiver.recvClk.biasFor(satSys), *_receiver.recvClk.biasStdDevFor(satSys));
120 LOG_DATA("{}: [{}] clkDrift = {} [s/s] (StdDev = {})", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), *_receiver.recvClk.driftFor(satSys), *_receiver.recvClk.driftStdDevFor(satSys));
121 }
122
2/2
✓ Branch 5 taken 8354 times.
✓ Branch 6 taken 2245 times.
 10599 for ([[maybe_unused]] const auto& freq : _receiver.interFrequencyBias)
123 {
124 LOG_DATA("{}: [{}] IFBBias [{:3}] = {} [s] (StdDev = {})", nameId,
125 _receiver.gnssObs->insTime.toYMDHMS(GPST), freq.first, freq.second.value, freq.second.stdDev);
126 }
127
128
1/2
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
 2245 auto posNorm = e_oldPos.norm();
129
3/4
✓ Branch 0 taken 2188 times.
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2188 times.
 2245 bool ignoreElevationMask = posNorm < 0.5 * InsConst::WGS84::a || posNorm > 1.5 * InsConst::WGS84::a;
130 LOG_DATA("{}: [{}] ignoreElevationMask = {} (posNorm = {} [m])", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), ignoreElevationMask, posNorm);
131
1/2
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
 2245 Observations observations;
132 2245 _obsFilter.selectObservationsForCalculation(Rover,
133 2245 _receiver.e_posMarker,
134 2245 _receiver.lla_posMarker,
135
1/2
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
 2245 _receiver.gnssObs,
136 gnssNavInfos,
137 observations,
138 nullptr,
139 nameId,
140 ignoreElevationMask);
141
142
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2245 times.
 2245 if (observations.signals.empty())
143 {
144 LOG_TRACE("{}: [{}] SPP cannot calculate position because no valid observations. Try changing filter settings or reposition your antenna.",
145 nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST));
146 ✗ return nullptr;
147 }
148
149 2245 size_t nParams = SPP::States::POS_STATE_COUNT + observations.systems.size() - 1;
150
151 2245 sppSol->nSatellites = observations.satellites.size();
152 2245 sppSol->nMeasPsr = observations.nObservables[GnssObs::Pseudorange];
153 2245 sppSol->nMeasDopp = observations.nObservables[GnssObs::Doppler];
154 LOG_DATA("{}: nParams = {}", nameId, nParams);
155 LOG_DATA("{}: nSatellites = {}", nameId, sppSol->nSatellites);
156 LOG_DATA("{}: nMeasPsr = {}", nameId, sppSol->nMeasPsr);
157 LOG_DATA("{}: nMeasDopp = {}", nameId, sppSol->nMeasDopp);
158
2/2
✓ Branch 4 taken 3753 times.
✓ Branch 5 taken 2245 times.
 5998 for (const auto& satSys : observations.systems)
159 {
160
1/2
✓ Branch 1 taken 3753 times.
✗ Branch 2 not taken.
 3753 [[maybe_unused]] auto satCount = std::ranges::count_if(observations.satellites, [&](const SatId& satId) {
161 47123 return satId.satSys == satSys;
162 });
163
164 LOG_DATA("{}: nSat {:5} = {}", nameId, satSys, satCount);
165 }
166
167 2245 if (size_t nPsrMeas = observations.nObservablesUniqueSatellite[GnssObs::Pseudorange];
168
3/8
✗ Branch 0 not taken.
✓ Branch 1 taken 2245 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 2245 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 2245 times.
 2245 (_estimatorType != EstimatorType::KalmanFilter || !_kalmanFilter.isInitialized()) && nPsrMeas <= nParams)
169 {
170 LOG_TRACE("{}: [{}] SPP cannot calculate position because only {} satellites with pseudorange observations ({} needed). Try changing filter settings or reposition your antenna.",
171 nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), nPsrMeas, nParams + 1);
172 ✗ return nullptr;
173 }
174
175
1/2
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
 2245 updateInterFrequencyBiases(observations, nameId);
176
177
1/2
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
 2245 _obsEstimator.calcObservationEstimates(observations, _receiver, ionosphericCorrections, nameId, ObservationEstimator::NoDifference);
178
179
1/2
✓ Branch 2 taken 2245 times.
✗ Branch 3 not taken.
 2245 auto stateKeys = determineStateKeys(observations.systems, observations.nObservables[GnssObs::Doppler], nameId);
180
1/2
✓ Branch 3 taken 2245 times.
✗ Branch 4 not taken.
 2245 auto measKeys = determineMeasKeys(observations, sppSol->nMeasPsr, sppSol->nMeasDopp, nameId);
181
182 2245 sppSol->nParam = stateKeys.size();
183
184
1/2
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
 2245 H = calcMatrixH(stateKeys, measKeys, observations, nameId);
185
1/2
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
 2245 auto R = calcMatrixR(measKeys, observations, nameId);
186
1/2
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
 2245 auto dz = calcMeasInnovation(measKeys, observations, nameId);
187
188
2/6
✗ Branch 0 not taken.
✓ Branch 1 taken 2245 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 2245 times.
 2245 if (_estimatorType == EstimatorType::KalmanFilter && _kalmanFilter.isInitialized())
189 {
190 ✗ std::string highInnovation;
191 ✗ size_t highInnovationCounter = 0;
192 ✗ for (const auto& key : dz.rowKeys())
193 {
194 ✗ if (double val = dz(key); std::abs(val) > 1000)
195 {
196 ✗ highInnovation += fmt::format("{}[{} {:.0f}], ", highInnovationCounter++ % 3 == 0 ? "\n" : "", key, val);
197 }
198 }
199 ✗ if (highInnovationCounter != 0)
200 {
201 std::string msg = fmt::format("Potential clock jump detected. Adjusting KF clock error covariance.\n"
202 "Too large innovations: {}",
203 ✗ highInnovation.substr(0, highInnovation.length() - 2));
204 ✗ LOG_WARN("{}: [{}] {}", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), msg);
205 ✗ _kalmanFilter.setClockBiasErrorCovariance(1e1);
206 ✗ sppSol->addEvent(msg);
207 ✗ }
208 ✗ }
209
210
2/6
✗ Branch 0 not taken.
✓ Branch 1 taken 2245 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2245 times.
✗ Branch 6 not taken.
 2245 if (_estimatorType != EstimatorType::KalmanFilter || !_kalmanFilter.isInitialized())
211 {
212
1/2
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
 2245 KeyedLeastSquaresResult<double, States::StateKeyType> lsq;
213
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2245 times.
 2245 if (_estimatorType == EstimatorType::LeastSquares)
214 {
215 ✗ lsq = solveLinearLeastSquaresUncertainties(H, dz);
216 }
217 else /* if (_estimatorType == EstimatorType::WeightedLeastSquares) */
218 {
219
5/10
✓ Branch 6 taken 2245 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2245 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2245 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 2245 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2245 times.
✗ Branch 19 not taken.
 2245 auto W = KeyedMatrixXd<Meas::MeasKeyTypes, Meas::MeasKeyTypes>(Eigen::MatrixXd(R(all, all).diagonal().cwiseInverse().asDiagonal()), R.colKeys(), R.rowKeys());
220 LOG_DATA("{}: W =\n{}", nameId, W);
221
1/2
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
 2245 lsq = solveWeightedLinearLeastSquaresUncertainties(H, W, dz);
222 2245 }
223 LOG_DATA("{}: LSQ sol (dx) =\n{}", nameId, lsq.solution.transposed());
224 LOG_DATA("{}: LSQ var =\n{}", nameId, lsq.variance.transposed());
225
226
1/2
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
 2245 assignLeastSquaresResult(lsq.solution, lsq.variance, e_oldPos,
227 2245 nParams, observations.nObservablesUniqueSatellite[GnssObs::Doppler], dt, nameId);
228
229
1/2
✓ Branch 2 taken 2245 times.
✗ Branch 3 not taken.
 2245 accuracyAchieved = lsq.solution(all).norm() < 1e-4;
230 if (accuracyAchieved) { LOG_DATA("{}: [{}] Accuracy achieved on iteration {}", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), iteration + 1); }
231 else { LOG_DATA("{}: [{}] Bad accuracy on iteration {}: {}", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), iteration + 1, lsq.solution(all).norm()); }
232
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2245 times.
 2245 if (iteration == nIter - 1) { return nullptr; }
233
3/4
✓ Branch 0 taken 1300 times.
✓ Branch 1 taken 945 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1300 times.
 2245 if (accuracyAchieved || iteration == nIter - 1)
234 {
235 ✗ if (_estimatorType == EstimatorType::KalmanFilter && !_kalmanFilter.isInitialized()
236 ✗ && sppSol->nMeasPsr > nParams // Variance can only be calculated if more measurements than parameters
237
2/10
✗ Branch 0 not taken.
✓ Branch 1 taken 945 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 945 times.
 945 && (!_obsFilter.isObsTypeUsed(GnssObs::Doppler) || sppSol->nMeasDopp > nParams))
238 {
239 LOG_TRACE("{}: [{}] Initializing KF with LSQ solution", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST));
240 ✗ KeyedVectorXd<States::StateKeyType> x(Eigen::VectorXd::Zero(lsq.solution.rows()), lsq.solution.rowKeys());
241 ✗ x.segment<3>(PosKey) = _receiver.e_posMarker;
242 ✗ if (x.hasAnyRows(VelKey)) { x.segment<3>(VelKey) = _receiver.e_vel; }
243 ✗ for (size_t i = 0; i < _receiver.recvClk.satelliteSystems.size(); i++)
244 {
245 ✗ auto satSys = _receiver.recvClk.satelliteSystems.at(i);
246 ✗ x(Keys::RecvClkBias{ satSys }) = _receiver.recvClk.bias.at(i) * InsConst::C;
247 ✗ if (x.hasRow(Keys::RecvClkDrift{ satSys }))
248 {
249 ✗ x(Keys::RecvClkDrift{ satSys }) = _receiver.recvClk.drift.at(i) * InsConst::C;
250 }
251 }
252 ✗ for (const auto& state : x.rowKeys())
253 {
254 ✗ if (const auto* bias = std::get_if<Keys::InterFreqBias>(&state))
255 {
256 ✗ x(*bias) = _receiver.interFrequencyBias.at(bias->freq).value * InsConst::C;
257 }
258 }
259
260 ✗ _kalmanFilter.initialize(x, lsq.variance);
261 LOG_DATA("{}: x =\n{}", nameId, _kalmanFilter.getState().transposed());
262 LOG_DATA("{}: P =\n{}", nameId, _kalmanFilter.getErrorCovarianceMatrix());
263 ✗ }
264
2/4
✓ Branch 2 taken 945 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 945 times.
✗ Branch 5 not taken.
 945 if (lsq.variance.hasRows(VelKey))
265 {
266
2/4
✓ Branch 4 taken 945 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 945 times.
✗ Branch 8 not taken.
 945 sppSol->setPosVelAndCov_e(_receiver.e_posMarker, _receiver.e_vel, lsq.variance(PosVelKey, PosVelKey));
267 }
268 else
269 {
270 ✗ sppSol->setPositionAndCov_e(_receiver.e_posMarker, lsq.variance.block<3>(PosKey, PosKey));
271 }
272
273
1/2
✓ Branch 3 taken 945 times.
✗ Branch 4 not taken.
 945 sppSol->satData.reserve(observations.satellites.size());
274
2/2
✓ Branch 7 taken 39534 times.
✓ Branch 8 taken 945 times.
 40479 for (const auto& [satSigId, signalObs] : observations.signals)
275 {
276
1/2
✓ Branch 2 taken 39534 times.
✗ Branch 3 not taken.
 39534 if (std::ranges::find_if(sppSol->satData,
277
1/2
✓ Branch 1 taken 261968 times.
✗ Branch 2 not taken.
 261968 [&satSigId = satSigId](const auto& satIdData) { return satIdData.first == satSigId.toSatId(); })
278
2/2
✓ Branch 3 taken 11128 times.
✓ Branch 4 taken 28406 times.
 79068 == sppSol->satData.end())
279 {
280
1/2
✓ Branch 2 taken 11128 times.
✗ Branch 3 not taken.
 22256 sppSol->satData.emplace_back(satSigId.toSatId(),
281
2/4
✓ Branch 1 taken 11128 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 11128 times.
✗ Branch 6 not taken.
 11128 SppSolution::SatData{ .satElevation = signalObs.recvObs.at(Rover)->satElevation(_receiver.e_posMarker, _receiver.lla_posMarker),
282
3/6
✓ Branch 1 taken 11128 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 11128 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 11128 times.
✗ Branch 9 not taken.
 22256 .satAzimuth = signalObs.recvObs.at(Rover)->satAzimuth(_receiver.e_posMarker, _receiver.lla_posMarker) });
283 }
284 }
285
286
1/2
✓ Branch 2 taken 945 times.
✗ Branch 3 not taken.
 945 sppSol->_e_sppCovarianceMatrix = lsq.variance;
287
288 945 break;
289 }
290 2245 }
291 else // if (_estimatorType == EstimatorType::KalmanFilter)
292 {
293 ✗ _kalmanFilter.update(measKeys, H, R, dz, nameId);
294
295 ✗ if (double posDiff = (_kalmanFilter.getState()(PosKey) - _receiver.e_posMarker).norm();
296 ✗ posDiff > 100)
297 {
298 ✗ std::string msg = fmt::format("Potential clock jump detected. Reinitializing KF with WLSQ.\nPosition difference to previous epoch {:.1f}m", posDiff);
299 ✗ LOG_WARN("{}: [{}] {}", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), msg);
300 ✗ sppSol->addEvent(msg);
301 // _kalmanFilter.deinitialize();
302 // nIter = N_ITER_MAX_LSQ + 1;
303 // continue;
304 ✗ }
305
306 ✗ assignKalmanFilterResult(_kalmanFilter.getState(), _kalmanFilter.getErrorCovarianceMatrix(), nameId);
307 ✗ sppSol->setPosVelAndCov_e(_receiver.e_posMarker, _receiver.e_vel, _kalmanFilter.getErrorCovarianceMatrix()(PosVelKey, PosVelKey));
308
309 ✗ sppSol->satData.reserve(observations.satellites.size());
310 ✗ for (const auto& [satSigId, signalObs] : observations.signals)
311 {
312 ✗ if (std::ranges::find_if(sppSol->satData,
313 ✗ [&satSigId = satSigId](const auto& satIdData) { return satIdData.first == satSigId.toSatId(); })
314 ✗ == sppSol->satData.end())
315 {
316 ✗ sppSol->satData.emplace_back(satSigId.toSatId(),
317 ✗ SppSolution::SatData{ .satElevation = signalObs.recvObs.at(Rover)->satElevation(_receiver.e_posMarker, _receiver.lla_posMarker),
318 ✗ .satAzimuth = signalObs.recvObs.at(Rover)->satAzimuth(_receiver.e_posMarker, _receiver.lla_posMarker) });
319 }
320 }
321 ✗ sppSol->_e_sppCovarianceMatrix = _kalmanFilter.getErrorCovarianceMatrix();
322 }
323 6025 }
324 // use H matrix to calculate DOPs
325
1/2
✓ Branch 1 taken 945 times.
✗ Branch 2 not taken.
 945 computeDOPs(sppSol, H, nameId);
326
327
1/2
✓ Branch 2 taken 945 times.
✗ Branch 3 not taken.
 945 sppSol->recvClk = _receiver.recvClk;
328
1/2
✓ Branch 2 taken 945 times.
✗ Branch 3 not taken.
 945 sppSol->interFrequencyBias = _receiver.interFrequencyBias;
329
330 #if LOG_LEVEL <= LOG_LEVEL_DATA
331 if (sppSol->e_position() != _receiver.e_posMarker)
332 {
333 LOG_DATA("{}: [{}] Receiver: e_pos = {:.6f}m, {:.6f}m, {:.6f}m", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST),
334 _receiver.e_posMarker(0), _receiver.e_posMarker(1), _receiver.e_posMarker(2));
335 LOG_DATA("{}: [{}] Receiver: lla_pos = {:.6f}°, {:.6f}°, {:.3f}m", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST),
336 rad2deg(_receiver.lla_posMarker.x()), rad2deg(_receiver.lla_posMarker.y()), _receiver.lla_posMarker.z());
337 }
338 if (sppSol->e_velocity() != _receiver.e_vel)
339 {
340 LOG_DATA("{}: [{}] Receiver: e_vel = {}", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), _receiver.e_vel.transpose());
341 }
342 for (const auto& satSys : _receiver.recvClk.satelliteSystems)
343 {
344 if (*sppSol->recvClk.biasFor(satSys) != *_receiver.recvClk.biasFor(satSys))
345 {
346 LOG_DATA("{}: [{}] Receiver: clkBias = {} s", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), *_receiver.recvClk.biasFor(satSys));
347 }
348 if (*sppSol->recvClk.driftFor(satSys) != *_receiver.recvClk.driftFor(satSys))
349 {
350 LOG_DATA("{}: [{}] Receiver: clkDrift = {} s/s", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), *_receiver.recvClk.driftFor(satSys));
351 }
352 }
353 #endif
354
355 LOG_DATA("{}: [{}] Solution: e_pos = {:.6f}m, {:.6f}m, {:.6f}m", nameId, sppSol->insTime.toYMDHMS(GPST),
356 sppSol->e_position()(0), sppSol->e_position()(1), sppSol->e_position()(2));
357 LOG_DATA("{}: [{}] Solution: lla_pos = {:.6f}°, {:.6f}°, {:.3f}m", nameId, sppSol->insTime.toYMDHMS(GPST),
358 rad2deg(sppSol->latitude()), rad2deg(sppSol->longitude()), sppSol->altitude());
359 LOG_DATA("{}: [{}] Solution: e_vel = {}", nameId, sppSol->insTime.toYMDHMS(GPST), sppSol->e_velocity().transpose());
360
2/2
✓ Branch 2 taken 1547 times.
✓ Branch 3 taken 945 times.
 2492 for (size_t i = 0; i < sppSol->recvClk.satelliteSystems.size(); i++) // NOLINT
361 {
362 LOG_DATA("{}: [{}] Solution: clkBias [{:5}] = {} s", nameId, sppSol->insTime.toYMDHMS(GPST),
363 sppSol->recvClk.satelliteSystems.at(i), sppSol->recvClk.bias.at(i));
364 }
365
2/2
✓ Branch 2 taken 1547 times.
✓ Branch 3 taken 945 times.
 2492 for (size_t i = 0; i < sppSol->recvClk.satelliteSystems.size(); i++) // NOLINT
366 {
367 LOG_DATA("{}: [{}] Solution: clkDrift [{:5}] = {} s/s", nameId, sppSol->insTime.toYMDHMS(GPST),
368 sppSol->recvClk.satelliteSystems.at(i), sppSol->recvClk.drift.at(i));
369 }
370
2/2
✓ Branch 6 taken 3397 times.
✓ Branch 7 taken 945 times.
 4342 for ([[maybe_unused]] const auto& freq : sppSol->interFrequencyBias)
371 {
372 LOG_DATA("{}: [{}] Solution: IFBBias [{:5}] = {} s", nameId, sppSol->insTime.toYMDHMS(GPST), freq.first, freq.second.value);
373 }
374
375 945 return sppSol;
376 945 }
377
378 10488 bool Algorithm::canCalculateVelocity(size_t nDoppMeas) const
379 {
380
2/6
✗ Branch 0 not taken.
✓ Branch 1 taken 10488 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 10488 times.
 10488 if (_estimatorType == EstimatorType::KalmanFilter && _kalmanFilter.isInitialized()) { return true; }
381
382
2/4
✓ Branch 1 taken 10488 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10488 times.
✗ Branch 4 not taken.
 10488 return _obsFilter.isObsTypeUsed(GnssObs::Doppler) && nDoppMeas >= 4;
383 }
384
385 ✗ bool Algorithm::canEstimateInterFrequencyBias() const
386 {
387 ✗ for (const auto& satSys : SatelliteSystem::GetAll())
388 {
389 ✗ auto filter = Frequency(_obsFilter.getFrequencyFilter() & satSys);
390 ✗ if (filter.count() > 1) { return true; }
391 ✗ }
392
393 ✗ return false;
394 }
395
396 2245 void Algorithm::updateInterFrequencyBiases(const Observations& observations, [[maybe_unused]] const std::string& nameId)
397 {
398
2/2
✓ Branch 0 taken 2238 times.
✓ Branch 1 taken 7 times.
 2245 if (_estimateInterFreqBiases)
399 {
400 2238 std::set<Frequency> observedFrequencies;
401 2238 Frequency allObservedFrequencies;
402
2/2
✓ Branch 4 taken 94916 times.
✓ Branch 5 taken 2238 times.
 97154 for (const auto& obs : observations.signals)
403 {
404
2/4
✓ Branch 1 taken 94916 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 94916 times.
✗ Branch 5 not taken.
 94916 observedFrequencies.insert(obs.first.freq());
405
1/2
✓ Branch 1 taken 94916 times.
✗ Branch 2 not taken.
 94916 allObservedFrequencies |= obs.first.freq();
406 }
407 LOG_DATA("{}: Observed frequencies {}", nameId, fmt::join(observedFrequencies, ", "));
408
409
2/2
✓ Branch 5 taken 12106 times.
✓ Branch 6 taken 2238 times.
 14344 for (const auto& freq : observedFrequencies)
410 {
411
8/10
✓ Branch 1 taken 12106 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8367 times.
✓ Branch 4 taken 3739 times.
✓ Branch 6 taken 8367 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 13 times.
✓ Branch 9 taken 8354 times.
✓ Branch 10 taken 13 times.
✓ Branch 11 taken 12093 times.
 12106 if (!freq.isFirstFrequency(allObservedFrequencies) && !_receiver.interFrequencyBias.contains(freq))
412 {
413 LOG_TRACE("{}: Estimating Inter-Frequency bias for {}", nameId, freq);
414
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 _receiver.interFrequencyBias.emplace(freq, UncertainValue<double>{});
415
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 _kalmanFilter.addInterFrequencyBias(freq);
416 }
417 }
418 2238 }
419 2245 }
420
421 2245 std::vector<States::StateKeyType> Algorithm::determineStateKeys(const std::set<SatelliteSystem>& usedSatSystems, size_t nDoppMeas,
422 [[maybe_unused]] const std::string& nameId) const
423 {
424
2/6
✗ Branch 0 not taken.
✓ Branch 1 taken 2245 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 2245 times.
 2245 if (_estimatorType == EstimatorType::KalmanFilter && _kalmanFilter.isInitialized())
425 {
426 LOG_DATA("{}: stateKeys = [{}]", nameId, joinToString(_kalmanFilter.getStateKeys()));
427 ✗ return _kalmanFilter.getStateKeys();
428 }
429
430 2245 std::vector<States::StateKeyType> stateKeys;
431
1/2
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
 4490 stateKeys.reserve(PosKey.size());
432
2/4
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2245 times.
✗ Branch 5 not taken.
 2245 std::ranges::copy(PosKey, std::back_inserter(stateKeys));
433 2245 stateKeys.reserve(stateKeys.size() + 1
434 2245 + canCalculateVelocity(nDoppMeas) * (VelKey.size() + 1)
435
3/6
✓ Branch 2 taken 2245 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2245 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2245 times.
✗ Branch 9 not taken.
 4490 + usedSatSystems.size() * (1 + canCalculateVelocity(nDoppMeas)));
436
2/4
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2245 times.
✗ Branch 4 not taken.
 2245 if (canCalculateVelocity(nDoppMeas))
437 {
438
2/4
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2245 times.
✗ Branch 5 not taken.
 2245 std::ranges::copy(VelKey, std::back_inserter(stateKeys));
439 }
440
441
2/2
✓ Branch 5 taken 3753 times.
✓ Branch 6 taken 2245 times.
 5998 for (const auto& satSys : usedSatSystems)
442 {
443
1/2
✓ Branch 1 taken 3753 times.
✗ Branch 2 not taken.
 3753 stateKeys.emplace_back(Keys::RecvClkBias{ satSys });
444
3/6
✓ Branch 1 taken 3753 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3753 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3753 times.
✗ Branch 7 not taken.
 3753 if (canCalculateVelocity(nDoppMeas)) { stateKeys.emplace_back(Keys::RecvClkDrift{ satSys }); }
445 }
446
2/2
✓ Branch 4 taken 8367 times.
✓ Branch 5 taken 2245 times.
 10612 for (const auto& freq : _receiver.interFrequencyBias)
447 {
448
1/2
✓ Branch 1 taken 8367 times.
✗ Branch 2 not taken.
 8367 stateKeys.emplace_back(Keys::InterFreqBias{ freq.first });
449 }
450
451 LOG_DATA("{}: stateKeys = [{}]", nameId, joinToString(stateKeys));
452 2245 return stateKeys;
453 2245 }
454
455 2245 std::vector<Meas::MeasKeyTypes> Algorithm::determineMeasKeys(const Observations& observations, size_t nPsrMeas, size_t nDoppMeas, [[maybe_unused]] const std::string& nameId) const
456 {
457 2245 std::vector<Meas::MeasKeyTypes> measKeys;
458
1/2
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
 2245 measKeys.reserve(nPsrMeas + nDoppMeas);
459
460
2/4
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2245 times.
✗ Branch 4 not taken.
 2245 if (_obsFilter.isObsTypeUsed(GnssObs::Pseudorange))
461 {
462
2/2
✓ Branch 5 taken 95392 times.
✓ Branch 6 taken 2245 times.
 97637 for (const auto& signalObs : observations.signals)
463 {
464
3/6
✓ Branch 1 taken 95392 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 95392 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 95392 times.
✗ Branch 8 not taken.
 95392 if (signalObs.second.recvObs.at(Rover)->obs.contains(GnssObs::Pseudorange))
465 {
466
1/2
✓ Branch 1 taken 95392 times.
✗ Branch 2 not taken.
 95392 measKeys.emplace_back(Meas::Psr{ signalObs.first });
467 }
468 }
469 }
470
2/4
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2245 times.
✗ Branch 4 not taken.
 2245 if (_obsFilter.isObsTypeUsed(GnssObs::Doppler))
471 {
472
2/2
✓ Branch 5 taken 95392 times.
✓ Branch 6 taken 2245 times.
 97637 for (const auto& signalObs : observations.signals)
473 {
474
4/6
✓ Branch 1 taken 95392 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 95392 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 94516 times.
✓ Branch 8 taken 876 times.
 95392 if (signalObs.second.recvObs.at(Rover)->obs.contains(GnssObs::Doppler))
475 {
476
1/2
✓ Branch 1 taken 94516 times.
✗ Branch 2 not taken.
 94516 measKeys.emplace_back(Meas::Doppler{ signalObs.first });
477 }
478 }
479 }
480
481 LOG_DATA("{}: measKeys = [{}]", nameId, joinToString(measKeys));
482 2245 return measKeys;
483 ✗ }
484
485 2245 KeyedMatrixXd<Meas::MeasKeyTypes, States::StateKeyType> Algorithm::calcMatrixH(const std::vector<States::StateKeyType>& stateKeys,
486 const std::vector<Meas::MeasKeyTypes>& measKeys,
487 const Observations& observations,
488 const std::string& nameId) const
489 {
490
1/2
✓ Branch 2 taken 2245 times.
✗ Branch 3 not taken.
 2245 KeyedMatrixXd<Meas::MeasKeyTypes, States::StateKeyType> H(Eigen::MatrixXd::Zero(static_cast<int>(measKeys.size()),
491 2245 static_cast<int>(stateKeys.size())),
492
1/2
✓ Branch 3 taken 2245 times.
✗ Branch 4 not taken.
 4490 measKeys, stateKeys);
493
494
2/2
✓ Branch 7 taken 95392 times.
✓ Branch 8 taken 2245 times.
 97637 for (const auto& [satSigId, signalObs] : observations.signals)
495 {
496
1/2
✓ Branch 1 taken 95392 times.
✗ Branch 2 not taken.
 95392 auto satId = satSigId.toSatId();
497
498
1/2
✓ Branch 1 taken 95392 times.
✗ Branch 2 not taken.
 95392 const auto& roverObs = signalObs.recvObs.at(Rover);
499
2/2
✓ Branch 8 taken 189908 times.
✓ Branch 9 taken 95392 times.
 285300 for (const auto& [obsType, obsData] : roverObs->obs)
500 {
501
2/4
✓ Branch 0 taken 95392 times.
✓ Branch 1 taken 94516 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 189908 switch (obsType)
502 {
503 95392 case GnssObs::Pseudorange:
504
5/10
✓ Branch 2 taken 95392 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 95392 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 95392 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 95392 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 95392 times.
✗ Branch 17 not taken.
 95392 H.block<3>(Meas::Psr{ satSigId }, PosKey) = -roverObs->e_pLOS(_receiver.e_posMarker).transpose();
505
1/2
✓ Branch 3 taken 95392 times.
✗ Branch 4 not taken.
 95392 H(Meas::Psr{ satSigId }, Keys::RecvClkBias{ satId.satSys }) = 1;
506
4/6
✓ Branch 1 taken 95392 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 95392 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 60739 times.
✓ Branch 7 taken 34653 times.
 95392 if (_receiver.interFrequencyBias.contains(satSigId.freq()))
507 {
508
2/4
✓ Branch 1 taken 60739 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 60739 times.
✗ Branch 7 not taken.
 60739 H(Meas::Psr{ satSigId }, Keys::InterFreqBias{ satSigId.freq() }) = 1;
509 }
510 95392 break;
511 94516 case GnssObs::Doppler:
512
5/10
✓ Branch 2 taken 94516 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 94516 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 94516 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 94516 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 94516 times.
✗ Branch 17 not taken.
 94516 H.block<3>(Meas::Doppler{ satSigId }, PosKey) = -roverObs->e_vLOS(_receiver.e_posMarker, _receiver.e_vel).transpose();
513
5/10
✓ Branch 2 taken 94516 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 94516 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 94516 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 94516 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 94516 times.
✗ Branch 17 not taken.
 94516 H.block<3>(Meas::Doppler{ satSigId }, VelKey) = -roverObs->e_pLOS(_receiver.e_posMarker).transpose();
514
1/2
✓ Branch 3 taken 94516 times.
✗ Branch 4 not taken.
 94516 H(Meas::Doppler{ satSigId }, Keys::RecvClkDrift{ satId.satSys }) = 1;
515 94516 break;
516 ✗ case GnssObs::Carrier:
517 case GnssObs::ObservationType_COUNT:
518 ✗ LOG_CRITICAL("{}: [{}] is not supported by the SPP algorithm as observation type.", nameId, obsType);
519 break;
520 }
521 }
522 }
523
524 LOG_DATA("{}: H =\n{}", nameId, H);
525
526 2245 return H;
527 ✗ }
528
529 2245 KeyedMatrixXd<Meas::MeasKeyTypes, Meas::MeasKeyTypes> Algorithm::calcMatrixR(const std::vector<Meas::MeasKeyTypes>& measKeys,
530 const Observations& observations,
531 const std::string& nameId)
532 {
533
1/2
✓ Branch 2 taken 2245 times.
✗ Branch 3 not taken.
 2245 KeyedMatrixXd<Meas::MeasKeyTypes, Meas::MeasKeyTypes> R(Eigen::MatrixXd::Zero(static_cast<int>(measKeys.size()),
534 2245 static_cast<int>(measKeys.size())),
535
1/2
✓ Branch 2 taken 2245 times.
✗ Branch 3 not taken.
 4490 measKeys);
536
537
2/2
✓ Branch 7 taken 95392 times.
✓ Branch 8 taken 2245 times.
 97637 for (const auto& [satSigId, signalObs] : observations.signals)
538 {
539
3/4
✓ Branch 1 taken 95392 times.
✗ Branch 2 not taken.
✓ Branch 11 taken 189908 times.
✓ Branch 12 taken 95392 times.
 285300 for (const auto& [obsType, obsData] : signalObs.recvObs.at(Rover)->obs)
540 {
541
2/4
✓ Branch 0 taken 95392 times.
✓ Branch 1 taken 94516 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 189908 switch (obsType)
542 {
543 95392 case GnssObs::Pseudorange:
544
1/2
✓ Branch 3 taken 95392 times.
✗ Branch 4 not taken.
 95392 R(Meas::Psr{ satSigId }, Meas::Psr{ satSigId }) = obsData.measVar;
545 95392 break;
546 94516 case GnssObs::Doppler:
547
1/2
✓ Branch 3 taken 94516 times.
✗ Branch 4 not taken.
 94516 R(Meas::Doppler{ satSigId }, Meas::Doppler{ satSigId }) = obsData.measVar;
548 94516 break;
549 ✗ case GnssObs::Carrier:
550 case GnssObs::ObservationType_COUNT:
551 ✗ LOG_CRITICAL("{}: These observation types are not supported by the SPP algorithm", nameId);
552 break;
553 }
554 }
555 }
556
557 LOG_DATA("{}: R =\n{}", nameId, R);
558
559 2245 return R;
560 ✗ }
561
562 2245 KeyedVectorXd<Meas::MeasKeyTypes> Algorithm::calcMeasInnovation(const std::vector<Meas::MeasKeyTypes>& measKeys,
563 const Observations& observations,
564 const std::string& nameId)
565 {
566
2/4
✓ Branch 3 taken 2245 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2245 times.
✗ Branch 7 not taken.
 2245 KeyedVectorXd<Meas::MeasKeyTypes> dz(Eigen::VectorXd::Zero(static_cast<int>(measKeys.size())), measKeys);
567
568
2/2
✓ Branch 7 taken 95392 times.
✓ Branch 8 taken 2245 times.
 97637 for (const auto& [satSigId, signalObs] : observations.signals)
569 {
570
3/4
✓ Branch 1 taken 95392 times.
✗ Branch 2 not taken.
✓ Branch 11 taken 189908 times.
✓ Branch 12 taken 95392 times.
 285300 for (const auto& [obsType, obsData] : signalObs.recvObs.at(Rover)->obs)
571 {
572
2/4
✓ Branch 0 taken 95392 times.
✓ Branch 1 taken 94516 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 189908 switch (obsType)
573 {
574 95392 case GnssObs::Pseudorange:
575
1/2
✓ Branch 2 taken 95392 times.
✗ Branch 3 not taken.
 95392 dz(Meas::Psr{ satSigId }) = obsData.measurement - obsData.estimate;
576 95392 break;
577 94516 case GnssObs::Doppler:
578
1/2
✓ Branch 2 taken 94516 times.
✗ Branch 3 not taken.
 94516 dz(Meas::Doppler{ satSigId }) = obsData.measurement - obsData.estimate;
579 94516 break;
580 ✗ case GnssObs::Carrier:
581 case GnssObs::ObservationType_COUNT:
582 ✗ LOG_CRITICAL("{}: These observation types are not supported by the SPP algorithm", nameId);
583 break;
584 }
585 }
586 }
587
588 LOG_DATA("{}: dz =\n{}", nameId, dz);
589
590 2245 return dz;
591 ✗ }
592
593 2245 void Algorithm::assignLeastSquaresResult(const KeyedVectorXd<States::StateKeyType>& state,
594 const KeyedMatrixXd<States::StateKeyType, States::StateKeyType>& variance,
595 const Eigen::Vector3d& e_oldPos,
596 size_t nParams, size_t nUniqueDopplerMeas, double dt,
597 [[maybe_unused]] const std::string& nameId)
598 {
599
2/4
✓ Branch 2 taken 2245 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2245 times.
✗ Branch 6 not taken.
 2245 _receiver.e_posMarker += state.segment<3>(PosKey);
600
1/2
✓ Branch 1 taken 2245 times.
✗ Branch 2 not taken.
 2245 _receiver.lla_posMarker = trafo::ecef2lla_WGS84(_receiver.e_posMarker);
601
2/2
✓ Branch 6 taken 29343 times.
✓ Branch 7 taken 2245 times.
 31588 for (const auto& s : state.rowKeys())
602 {
603
2/2
✓ Branch 1 taken 3753 times.
✓ Branch 2 taken 25590 times.
 29343 if (const auto* bias = std::get_if<Keys::RecvClkBias>(&s))
604 {
605
2/4
✓ Branch 1 taken 3753 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3753 times.
✗ Branch 5 not taken.
 3753 size_t idx = _receiver.recvClk.getIdx(bias->satSys).value();
606
2/4
✓ Branch 2 taken 3753 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3753 times.
✗ Branch 6 not taken.
 3753 _receiver.recvClk.bias.at(idx) += state(*bias) / InsConst::C;
607
2/4
✓ Branch 3 taken 3753 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 3753 times.
 3753 if (variance(*bias, *bias) < 0)
608 {
609 ✗ _receiver.recvClk.biasStdDev.at(idx) = 1000 / InsConst::C;
610 ✗ LOG_WARN("{}: Negative variance for {}. Defauting to {:.0f} [m]", nameId, *bias, _receiver.recvClk.biasStdDev.at(idx) * InsConst::C);
611 }
612 else
613 {
614
2/4
✓ Branch 3 taken 3753 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3753 times.
✗ Branch 7 not taken.
 3753 _receiver.recvClk.biasStdDev.at(idx) = std::sqrt(variance(*bias, *bias)) / InsConst::C;
615 }
616 LOG_DATA("{}: Setting Clk Bias [{}] = {} [s] (StdDev = {})", nameId, bias->satSys, _receiver.recvClk.bias.at(idx), _receiver.recvClk.biasStdDev.at(idx));
617 }
618
2/2
✓ Branch 1 taken 8367 times.
✓ Branch 2 taken 17223 times.
 25590 else if (const auto* bias = std::get_if<Keys::InterFreqBias>(&s))
619 {
620
1/2
✓ Branch 1 taken 8367 times.
✗ Branch 2 not taken.
 8367 auto& freqDiff = _receiver.interFrequencyBias.at(bias->freq);
621
1/2
✓ Branch 2 taken 8367 times.
✗ Branch 3 not taken.
 8367 freqDiff.value += state(*bias) / InsConst::C;
622
2/4
✓ Branch 3 taken 8367 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 8367 times.
 8367 if (variance(*bias, *bias) < 0)
623 {
624 ✗ freqDiff.stdDev = 1000 / InsConst::C;
625 ✗ LOG_WARN("{}: Negative variance for {}. Defauting to {:.0f} [m]", nameId, *bias, freqDiff.stdDev * InsConst::C);
626 }
627 else
628 {
629
1/2
✓ Branch 3 taken 8367 times.
✗ Branch 4 not taken.
 8367 freqDiff.stdDev = std::sqrt(variance(*bias, *bias)) / InsConst::C;
630 }
631 LOG_DATA("{}: Setting IFB Bias [{}] = {} [s] (StdDev = {})", nameId, bias->freq, freqDiff.value, freqDiff.stdDev);
632 }
633 }
634
635
1/2
✓ Branch 0 taken 2245 times.
✗ Branch 1 not taken.
 2245 if (nUniqueDopplerMeas >= nParams)
636 {
637
2/4
✓ Branch 2 taken 2245 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2245 times.
✗ Branch 6 not taken.
 2245 _receiver.e_vel += state.segment<3>(VelKey);
638
2/2
✓ Branch 6 taken 29343 times.
✓ Branch 7 taken 2245 times.
 31588 for (const auto& s : state.rowKeys())
639 {
640
2/2
✓ Branch 1 taken 3753 times.
✓ Branch 2 taken 25590 times.
 29343 if (const auto* drift = std::get_if<Keys::RecvClkDrift>(&s))
641 {
642
2/4
✓ Branch 1 taken 3753 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3753 times.
✗ Branch 5 not taken.
 3753 size_t idx = _receiver.recvClk.getIdx(drift->satSys).value();
643
2/4
✓ Branch 2 taken 3753 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3753 times.
✗ Branch 6 not taken.
 3753 _receiver.recvClk.drift.at(idx) += state(*drift) / InsConst::C;
644
2/4
✓ Branch 3 taken 3753 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 3753 times.
 3753 if (variance(*drift, *drift) < 0)
645 {
646 ✗ _receiver.recvClk.driftStdDev.at(idx) = 1000 / InsConst::C;
647 ✗ LOG_WARN("{}: Negative variance for {}. Defauting to {:.0f} [m/s]", nameId, *drift, _receiver.recvClk.driftStdDev.at(idx) * InsConst::C);
648 }
649 else
650 {
651
2/4
✓ Branch 3 taken 3753 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3753 times.
✗ Branch 7 not taken.
 3753 _receiver.recvClk.driftStdDev.at(idx) = std::sqrt(variance(*drift, *drift)) / InsConst::C;
652 }
653 LOG_DATA("{}: Setting Clk Drift [{}] = {} [s/s] (StdDev = {})", nameId, drift->satSys,
654 _receiver.recvClk.drift.at(idx), _receiver.recvClk.driftStdDev.at(idx));
655 }
656 }
657 }
658 else
659 {
660 ✗ if (dt > 1e-6 && !e_oldPos.isZero())
661 {
662 ✗ if (_obsFilter.isObsTypeUsed(GnssObs::Doppler))
663 {
664 LOG_TRACE("{}: [{}] SPP has only {} satellites with doppler observations ({} needed). Calculating velocity as position difference.",
665 nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), nUniqueDopplerMeas, nParams);
666 }
667 LOG_DATA("{}: [{}] e_oldPos = {}", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), e_oldPos.transpose());
668 LOG_DATA("{}: [{}] e_newPos = {}", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), _receiver.e_posMarker.transpose());
669 LOG_DATA("{}: [{}] dt = {}s", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), dt);
670 ✗ _receiver.e_vel = (_receiver.e_posMarker - e_oldPos) / dt;
671 LOG_DATA("{}: [{}] e_vel = {}", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), _receiver.e_vel.transpose());
672 }
673 }
674
675 LOG_DATA("{}: [{}] Assigning solution to _receiver", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST));
676 LOG_DATA("{}: [{}] e_pos = {}", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), _receiver.e_posMarker.transpose());
677 LOG_DATA("{}: [{}] e_vel = {}", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), _receiver.e_vel.transpose());
678 LOG_DATA("{}: [{}] lla_pos = {:.6f}°, {:.6f}°, {:.3f}m", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST),
679 rad2deg(_receiver.lla_posMarker.x()), rad2deg(_receiver.lla_posMarker.y()), _receiver.lla_posMarker.z());
680
2/2
✓ Branch 5 taken 3753 times.
✓ Branch 6 taken 2245 times.
 5998 for ([[maybe_unused]] const auto& satSys : _receiver.recvClk.satelliteSystems)
681 {
682 LOG_DATA("{}: [{}] clkBias = {} [s] (StdDev = {})", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST),
683 *_receiver.recvClk.biasFor(satSys), *_receiver.recvClk.biasStdDevFor(satSys));
684 LOG_DATA("{}: [{}] clkDrift = {} [s/s] (StdDev = {})", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST),
685 *_receiver.recvClk.driftFor(satSys), *_receiver.recvClk.driftStdDevFor(satSys));
686 }
687
688
2/2
✓ Branch 5 taken 8367 times.
✓ Branch 6 taken 2245 times.
 10612 for ([[maybe_unused]] const auto& freq : _receiver.interFrequencyBias)
689 {
690 LOG_DATA("{}: [{}] IFBBias [{:3}] = {} [s] (StdDev = {})", nameId,
691 _receiver.gnssObs->insTime.toYMDHMS(GPST), freq.first, freq.second.value, freq.second.stdDev);
692 }
693 2245 }
694
695 ✗ void Algorithm::assignKalmanFilterResult(const KeyedVectorXd<States::StateKeyType>& state,
696 const KeyedMatrixXd<States::StateKeyType, States::StateKeyType>& variance,
697 [[maybe_unused]] const std::string& nameId)
698 {
699 ✗ _receiver.e_posMarker = state(PosKey);
700 ✗ _receiver.lla_posMarker = trafo::ecef2lla_WGS84(_receiver.e_posMarker);
701 ✗ _receiver.e_vel = state(VelKey);
702 ✗ for (const auto& s : state.rowKeys())
703 {
704 ✗ if (const auto* bias = std::get_if<Keys::RecvClkBias>(&s))
705 {
706 ✗ size_t idx = _receiver.recvClk.getIdx(bias->satSys).value();
707 ✗ _receiver.recvClk.bias.at(idx) = state(*bias) / InsConst::C;
708 ✗ if (variance(*bias, *bias) < 0)
709 {
710 ✗ _receiver.recvClk.biasStdDev.at(idx) = 1000 / InsConst::C;
711 ✗ LOG_WARN("{}: Negative variance for {}. Defauting to {:.0f} [m]", nameId, *bias, _receiver.recvClk.biasStdDev.at(idx) * InsConst::C);
712 }
713 else
714 {
715 ✗ _receiver.recvClk.biasStdDev.at(idx) = std::sqrt(variance(*bias, *bias)) / InsConst::C;
716 }
717 LOG_DATA("{}: Setting Clock Bias [{}] = {}", nameId, bias->satSys, _receiver.recvClk.bias.at(idx));
718 }
719 ✗ else if (const auto* drift = std::get_if<Keys::RecvClkDrift>(&s))
720 {
721 ✗ size_t idx = _receiver.recvClk.getIdx(drift->satSys).value();
722 ✗ _receiver.recvClk.drift.at(idx) = state(*drift) / InsConst::C;
723 ✗ if (variance(*drift, *drift) < 0)
724 {
725 ✗ _receiver.recvClk.driftStdDev.at(idx) = 1000 / InsConst::C;
726 ✗ LOG_WARN("{}: Negative variance for {}. Defauting to {:.0f} [m/s]", nameId, *drift, _receiver.recvClk.driftStdDev.at(idx) * InsConst::C);
727 }
728 else
729 {
730 ✗ _receiver.recvClk.driftStdDev.at(idx) = std::sqrt(variance(*drift, *drift)) / InsConst::C;
731 }
732 LOG_DATA("{}: Setting Clock Drift [{}] = {}", nameId, drift->satSys, _receiver.recvClk.drift.at(idx));
733 }
734 ✗ else if (const auto* bias = std::get_if<Keys::InterFreqBias>(&s))
735 {
736 ✗ auto& freqDiff = _receiver.interFrequencyBias.at(bias->freq);
737 ✗ freqDiff.value = state(*bias) / InsConst::C;
738 ✗ if (variance(*bias, *bias) < 0)
739 {
740 ✗ freqDiff.stdDev = 1000 / InsConst::C;
741 ✗ LOG_WARN("{}: Negative variance for {}. Defauting to {:.0f} [m/s]", nameId, *bias, freqDiff.stdDev * InsConst::C);
742 }
743 else
744 {
745 ✗ freqDiff.stdDev = std::sqrt(variance(*bias, *bias)) / InsConst::C;
746 }
747 LOG_DATA("{}: Setting Inter-Freq Bias [{}] = {}", nameId, bias->freq, freqDiff.value);
748 }
749 }
750
751 LOG_DATA("{}: [{}] Assigning solution to _receiver", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST));
752 LOG_DATA("{}: [{}] e_pos = {:.6f}m, {:.6f}m, {:.6f}m", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST),
753 _receiver.e_posMarker(0), _receiver.e_posMarker(1), _receiver.e_posMarker(2));
754 LOG_DATA("{}: [{}] e_vel = {}", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), _receiver.e_vel.transpose());
755 LOG_DATA("{}: [{}] lla_pos = {:.6f}°, {:.6f}°, {:.3f}m", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST),
756 rad2deg(_receiver.lla_posMarker.x()), rad2deg(_receiver.lla_posMarker.y()), _receiver.lla_posMarker.z());
757 ✗ for ([[maybe_unused]] const auto& satSys : _receiver.recvClk.satelliteSystems)
758 {
759 LOG_DATA("{}: [{}] clkBias = {} s", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), *_receiver.recvClk.biasFor(satSys));
760 LOG_DATA("{}: [{}] clkDrift = {} s/s", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), *_receiver.recvClk.driftFor(satSys));
761 }
762
763 ✗ for ([[maybe_unused]] const auto& freq : _receiver.interFrequencyBias)
764 {
765 LOG_DATA("{}: [{}] IFBBias [{:3}] = {} s", nameId, _receiver.gnssObs->insTime.toYMDHMS(GPST), freq.first, freq.second.value);
766 }
767 ✗ }
768
769 945 void Algorithm::computeDOPs(const std::shared_ptr<SppSolution>& sppSol,
770 const KeyedMatrixXd<Meas::MeasKeyTypes, States::StateKeyType>& H,
771 [[maybe_unused]] const std::string& nameId)
772 {
773
3/6
✓ Branch 5 taken 945 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 945 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 945 times.
✗ Branch 12 not taken.
 945 KeyedMatrixXd<States::StateKeyType, States::StateKeyType> N(H(all, all).transpose() * H(all, all), H.colKeys());
774
1/2
✓ Branch 2 taken 945 times.
✗ Branch 3 not taken.
 945 Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(N(all, all));
775
2/4
✓ Branch 1 taken 945 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 945 times.
✗ Branch 5 not taken.
 945 if (lu_decomp.rank() == H.cols())
776 {
777
1/2
✓ Branch 1 taken 945 times.
✗ Branch 2 not taken.
 945 auto Q = N.inverse();
778
2/4
✓ Branch 3 taken 945 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 945 times.
✗ Branch 7 not taken.
 945 Eigen::Matrix3d Qpp = Q(PosKey, PosKey);
779
4/8
✓ Branch 2 taken 945 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 945 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 945 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 945 times.
✗ Branch 13 not taken.
 945 Eigen::Matrix3d Qlocal = sppSol->n_Quat_e() * Qpp * sppSol->e_Quat_n();
780
2/4
✓ Branch 1 taken 945 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 945 times.
✗ Branch 5 not taken.
 945 sppSol->HDOP = std::sqrt(Qlocal(0, 0) + Qlocal(1, 1));
781
1/2
✓ Branch 1 taken 945 times.
✗ Branch 2 not taken.
 945 sppSol->VDOP = std::sqrt(Qlocal(2, 2));
782
1/2
✓ Branch 1 taken 945 times.
✗ Branch 2 not taken.
 945 sppSol->PDOP = std::sqrt(Qpp.trace());
783 945 }
784
785 LOG_DATA("{}: HDOP = {}", nameId, sppSol->HDOP);
786 LOG_DATA("{}: VDOP = {}", nameId, sppSol->VDOP);
787 LOG_DATA("{}: PDOP = {}", nameId, sppSol->PDOP);
788 945 }
789
790 /// @brief Converts the provided object into json
791 /// @param[out] j Json object which gets filled with the info
792 /// @param[in] obj Object to convert into json
793 ✗ void to_json(json& j, const Algorithm& obj)
794 {
795 ✗ j = json{
796 ✗ { "obsFilter", obj._obsFilter },
797 ✗ { "obsEstimator", obj._obsEstimator },
798 ✗ { "estimatorType", obj._estimatorType },
799 ✗ { "kalmanFilter", obj._kalmanFilter },
800 ✗ { "estimateInterFrequencyBiases", obj._estimateInterFreqBiases },
801 ✗ };
802 ✗ }
803 /// @brief Converts the provided json object into a node object
804 /// @param[in] j Json object with the needed values
805 /// @param[out] obj Object to fill from the json
806 8 void from_json(const json& j, Algorithm& obj)
807 {
808
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("obsFilter")) { j.at("obsFilter").get_to(obj._obsFilter); }
809
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("obsEstimator")) { j.at("obsEstimator").get_to(obj._obsEstimator); }
810
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("estimatorType")) { j.at("estimatorType").get_to(obj._estimatorType); }
811
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("kalmanFilter")) { j.at("kalmanFilter").get_to(obj._kalmanFilter); }
812
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("estimateInterFrequencyBiases")) { j.at("estimateInterFrequencyBiases").get_to(obj._estimateInterFreqBiases); }
813 8 }
814
815 } // namespace SPP
816
817 ✗ const char* to_string(SPP::Algorithm::EstimatorType estimatorType)
818 {
819 ✗ switch (estimatorType)
820 {
821 ✗ case SPP::Algorithm::EstimatorType::LeastSquares:
822 ✗ return "Least Squares";
823 ✗ case SPP::Algorithm::EstimatorType::WeightedLeastSquares:
824 ✗ return "Weighted Least Squares";
825 ✗ case SPP::Algorithm::EstimatorType::KalmanFilter:
826 ✗ return "Kalman Filter";
827 ✗ case SPP::Algorithm::EstimatorType::COUNT:
828 ✗ break;
829 }
830 ✗ return "";
831 }
832
833 ✗ const char* to_string(SPP::Algorithm::ReceiverType receiver)
834 {
835 ✗ switch (receiver)
836 {
837 ✗ case SPP::Algorithm::ReceiverType::Rover:
838 ✗ return "Rover";
839 ✗ case SPP::Algorithm::ReceiverType::ReceiverType_COUNT:
840 ✗ break;
841 }
842 ✗ return "";
843 }
844
845 } // namespace NAV
846
847 ✗ std::ostream& operator<<(std::ostream& os, const NAV::SPP::Algorithm::EstimatorType& obj)
848 {
849 ✗ return os << fmt::format("{}", obj);
850 }
851
852 ✗ std::ostream& operator<<(std::ostream& os, const NAV::SPP::Algorithm::ReceiverType& obj)
853 {
854 ✗ return os << fmt::format("{}", obj);
855 }
856