INSTINCT Code Coverage Report

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