INSTINCT Code Coverage Report

Directory: src/
File: Nodes/DataProcessor/GNSS/RealTimeKinematic.cpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 728 1382 52.7%
Functions: 40 51 78.4%
Branches: 946 3262 29.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 #include "RealTimeKinematic.hpp"
10
11 #include <array>
12 #include <algorithm>
13 #include <chrono>
14 #include <cmath>
15 #include <cstddef>
16 #include <cstdint>
17 #include <imgui.h>
18 #include <iterator>
19 #include <string>
20 #include <unordered_map>
21 #include <unordered_set>
22 #include <tuple>
23 #include <ranges>
24 #include <regex>
25 #include <utility>
26 #include <variant>
27 #include <vector>
28 #include <spdlog/common.h>
29 #include <spdlog/spdlog.h>
30
31 #include "Navigation/GNSS/Ambiguity/AmbiguityResolution.hpp"
32 #include "Navigation/GNSS/Ambiguity/CycleSlipDetector.hpp"
33 #include "Navigation/GNSS/Core/Code.hpp"
34 #include "Navigation/GNSS/Core/SatelliteIdentifier.hpp"
35 #include "Navigation/GNSS/Positioning/Observation.hpp"
36 #include "Navigation/GNSS/Positioning/RTK/Keys.hpp"
37 #include "Navigation/Math/KeyedLeastSquares.hpp"
38 #include "Navigation/Time/InsTime.hpp"
39 #include "Navigation/Time/TimeSystem.hpp"
40 #include "NodeData/GNSS/GnssObs.hpp"
41 #include "NodeData/GNSS/RtkSolution.hpp"
42 #include "internal/Node/Node.hpp"
43 #include "util/Json.hpp"
44 #include "util/Logger.hpp"
45 #include "util/Container/Vector.hpp"
46 #include "util/Container/UncertainValue.hpp"
47
48 #include "Navigation/Constants.hpp"
49
50 #include "internal/gui/NodeEditorApplication.hpp"
51 #include "internal/gui/widgets/HelpMarker.hpp"
52 #include "internal/gui/widgets/imgui_ex.hpp"
53 #include "internal/gui/widgets/InputWithUnit.hpp"
54 #include "internal/gui/widgets/EnumCombo.hpp"
55
56 #include <fmt/core.h>
57 #include <fmt/format.h>
58 #include "internal/FlowManager.hpp"
59
60 #include "NodeData/State/Pos.hpp"
61
62 #include "Navigation/GNSS/Ambiguity/internal/Search.hpp"
63 #include "Navigation/GNSS/Ambiguity/internal/Decorrelation.hpp"
64 #include "Navigation/GNSS/Core/Frequency.hpp"
65 #include "Navigation/GNSS/Functions.hpp"
66 #include "Navigation/Math/LeastSquares.hpp"
67
68 namespace States = NAV::RTK::States;
69 namespace Meas = NAV::RTK::Meas;
70
71 namespace NAV
72 {
73
74 115 RealTimeKinematic::RealTimeKinematic()
75
11/26
✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 115 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 115 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 115 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 115 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 115 times.
✗ Branch 22 not taken.
✓ Branch 25 taken 115 times.
✗ Branch 26 not taken.
✓ Branch 29 taken 115 times.
✗ Branch 30 not taken.
✓ Branch 36 taken 115 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 115 times.
✗ Branch 40 not taken.
✓ Branch 47 taken 115 times.
✗ Branch 48 not taken.
✗ Branch 57 not taken.
✗ Branch 58 not taken.
✗ Branch 59 not taken.
✗ Branch 60 not taken.
 345 : Node(typeStatic())
76 {
77 LOG_TRACE("{}: called", name);
78
79 115 _hasConfig = true;
80 115 _guiConfigDefaultWindowSize = { 633, 670 };
81
82
4/8
✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 115 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 115 times.
✓ Branch 9 taken 115 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 345 CreateInputPin("Base Position", Pin::Type::Flow, { Pos::type() }, &RealTimeKinematic::recvBasePos, nullptr, 1);
83
4/8
✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 115 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 115 times.
✓ Branch 9 taken 115 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 345 CreateInputPin("GnssObs (Base)", Pin::Type::Flow, { GnssObs::type() }, &RealTimeKinematic::recvBaseGnssObs);
84
4/8
✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 115 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 115 times.
✓ Branch 9 taken 115 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 345 CreateInputPin("GnssObs (Rover)", Pin::Type::Flow, { GnssObs::type() }, &RealTimeKinematic::recvRoverGnssObs);
85
1/2
✓ Branch 1 taken 115 times.
✗ Branch 2 not taken.
 115 _dynamicInputPins.addPin(this); // GnssNavInfo
86
87
5/10
✓ Branch 2 taken 115 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 115 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 115 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 115 times.
✓ Branch 15 taken 115 times.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 460 CreateOutputPin(RtkSolution::type().c_str(), Pin::Type::Flow, { RtkSolution::type() });
88 575 }
89
90 232 RealTimeKinematic::~RealTimeKinematic()
91 {
92 LOG_TRACE("{}: called", nameId());
93 232 }
94
95 229 std::string RealTimeKinematic::typeStatic()
96 {
97
1/2
✓ Branch 1 taken 229 times.
✗ Branch 2 not taken.
 458 return "RealTimeKinematic - RTK";
98 }
99
100 ✗ std::string RealTimeKinematic::type() const
101 {
102 ✗ return typeStatic();
103 }
104
105 114 std::string RealTimeKinematic::category()
106 {
107
1/2
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
 228 return "Data Processor";
108 }
109
110 ✗ void RealTimeKinematic::guiConfig()
111 {
112 ✗ auto nSatColumnContent = [&](size_t pinIndex) -> bool {
113 ✗ if (auto gnssNavInfo = getInputValue<GnssNavInfo>(pinIndex))
114 {
115 ✗ size_t usedSatNum = 0;
116 ✗ std::string usedSats;
117 ✗ std::string allSats;
118
119 ✗ std::string filler = ", ";
120 ✗ for (const auto& satellite : gnssNavInfo->v->satellites())
121 {
122 ✗ if (_obsFilter.isSatelliteAllowed(satellite.first))
123 {
124 ✗ usedSatNum++;
125 ✗ usedSats += (allSats.empty() ? "" : filler) + fmt::format("{}", satellite.first);
126 }
127 ✗ allSats += (allSats.empty() ? "" : filler) + fmt::format("{}", satellite.first);
128 }
129 ✗ ImGui::TextUnformatted(fmt::format("{} / {}", usedSatNum, gnssNavInfo->v->nSatellites()).c_str());
130 ✗ if (ImGui::IsItemHovered())
131 {
132 ✗ ImGui::SetTooltip("Used satellites: %s\n"
133 "Avail satellites: %s",
134 usedSats.c_str(), allSats.c_str());
135 }
136 ✗ }
137 ✗ return false;
138 ✗ };
139
140 ✗ if (_dynamicInputPins.ShowGuiWidgets(size_t(id), inputPins, this, { { .header = "# Sat", .content = nSatColumnContent } }))
141 {
142 ✗ flow::ApplyChanges();
143 }
144
145 ✗ ImGui::SameLine();
146 ✗ ImGui::BeginGroup();
147 ✗ ImGui::PushFont(Application::MonoFont());
148 ✗ auto totalSolutions = nFixSolutions + nFloatSolutions + nSingleSolutions;
149 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_Always);
150 ✗ if (ImGui::TreeNode("Solution statistics:"))
151 {
152 ✗ ImGui::BulletText("Fix: %6.2f%% (%zu)", totalSolutions > 0 ? static_cast<double>(nFixSolutions) / static_cast<double>(totalSolutions) * 100.0 : 0.0, nFixSolutions);
153 ✗ ImGui::BulletText("Float: %6.2f%% (%zu)", totalSolutions > 0 ? static_cast<double>(nFloatSolutions) / static_cast<double>(totalSolutions) * 100.0 : 0.0, nFloatSolutions);
154 ✗ ImGui::BulletText("Single: %6.2f%% (%zu)", totalSolutions > 0 ? static_cast<double>(nSingleSolutions) / static_cast<double>(totalSolutions) * 100.0 : 0.0, nSingleSolutions);
155 ✗ ImGui::TreePop();
156 }
157 ✗ if (ImGui::TreeNode(fmt::format("Cycle-slips: {}", _nCycleSlipsLLI + _nCycleSlipsSingle + _nCycleSlipsDual).c_str()))
158 {
159 ✗ ImGui::BulletText("LLI: %zu", _nCycleSlipsLLI);
160 ✗ ImGui::BulletText("Single Freq: %zu", _nCycleSlipsSingle);
161 ✗ ImGui::SameLine();
162 ✗ gui::widgets::HelpMarker("Single Frequency detection is confirmed by\nthe dual frequency detector before accepted.\nIt then only counts towards the single frequency count though.");
163 ✗ ImGui::BulletText("Dual Freq: %zu", _nCycleSlipsDual);
164 ✗ ImGui::SameLine();
165 ✗ gui::widgets::HelpMarker("Only cycle-slips which were not recognized by\nthe single frequency detector or had the LLI flag set.");
166 ✗ ImGui::TreePop();
167 }
168 ✗ ImGui::BulletText("Pivot changes: %zu", _nPivotChange);
169 ✗ if (_kalmanFilter.isNISenabled()) { ImGui::BulletText("NIS triggered: %zu", _nMeasExcludedNIS); }
170 ✗ ImGui::PopFont();
171 ✗ ImGui::EndGroup();
172
173 ✗ const float unitWidth = 120.0F * gui::NodeEditorApplication::windowFontRatio();
174 ✗ const float itemWidth = 310.0F * gui::NodeEditorApplication::windowFontRatio();
175 ✗ const float cWidth = itemWidth - ImGui::GetStyle().IndentSpacing;
176
177 ✗ if (_obsFilter.ShowGuiWidgets<ReceiverType>(nameId().c_str(), itemWidth))
178 {
179 ✗ flow::ApplyChanges();
180 }
181
182 ✗ if (_obsEstimator.ShowGuiWidgets<ReceiverType>(nameId().c_str(), itemWidth))
183 {
184 ✗ flow::ApplyChanges();
185 }
186
187 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
188 ✗ if (ImGui::TreeNode(fmt::format("Kalman Filter##{}", nameId()).c_str()))
189 {
190 ✗ if (_kalmanFilter.showKalmanFilterGUI(nameId().c_str(), itemWidth))
191 {
192 ✗ flow::ApplyChanges();
193 }
194
195 ✗ if (_kalmanFilter.isNISenabled())
196 {
197 ✗ auto tmp = static_cast<int>(_maxRemoveOutlier);
198 ✗ ImGui::SetNextItemWidth(cWidth);
199 ✗ if (ImGui::InputIntL(fmt::format("Max. remove per epoch##{}", nameId()).c_str(), &tmp, 0))
200 {
201 ✗ _maxRemoveOutlier = static_cast<size_t>(tmp);
202 ✗ flow::ApplyChanges();
203 }
204 ✗ ImGui::SameLine();
205 ✗ gui::widgets::HelpMarker("Maximum amount of observations to remove per epoch if the NIS triggers");
206
207 ✗ tmp = static_cast<int>(_outlierRemoveEpochs);
208 ✗ ImGui::SetNextItemWidth(cWidth);
209 ✗ if (ImGui::InputIntL(fmt::format("Epochs to exclude outlier##{}", nameId()).c_str(), &tmp, 1))
210 {
211 ✗ _outlierRemoveEpochs = static_cast<size_t>(tmp);
212 ✗ flow::ApplyChanges();
213 }
214 ✗ ImGui::SameLine();
215 ✗ gui::widgets::HelpMarker("Amount of epochs to exclude an outlier for after being flagged");
216
217 ✗ tmp = static_cast<int>(_outlierMinSat);
218 ✗ ImGui::SetNextItemWidth(cWidth);
219 ✗ if (ImGui::InputIntL(fmt::format("Min. satellites for check##{}", nameId()).c_str(), &tmp, 1))
220 {
221 ✗ _outlierMinSat = static_cast<size_t>(tmp);
222 ✗ flow::ApplyChanges();
223 }
224 ✗ ImGui::SameLine();
225 ✗ gui::widgets::HelpMarker("Minumum amount of satellites for doing a NIS check");
226
227 ✗ tmp = static_cast<int>(_outlierMinPsrObsKeep);
228 ✗ ImGui::SetNextItemWidth(cWidth);
229 ✗ if (ImGui::InputIntL(fmt::format("# of pseudorange obs to keep##{}", nameId()).c_str(), &tmp, 1))
230 {
231 ✗ _outlierMinPsrObsKeep = static_cast<size_t>(tmp);
232 ✗ flow::ApplyChanges();
233 }
234 ✗ ImGui::SameLine();
235 ✗ gui::widgets::HelpMarker("Minumum amount of pseudorange observables to leave when doing a NIS check");
236
237 ✗ ImGui::SetNextItemWidth(cWidth);
238 ✗ if (ImGui::InputDoubleL(fmt::format("Max Position Var for NIS in startup##{}", size_t(id)).c_str(), &_outlierMaxPosVarStartup, 0, std::numeric_limits<double>::max(), 0.0, 0.0, "%.4f m"))
239 {
240 ✗ flow::ApplyChanges();
241 }
242 ✗ ImGui::SameLine();
243 ✗ gui::widgets::HelpMarker("Maximum position variance\nin order to attempt outlier removal during the startup phase.");
244 }
245 ✗ ImGui::SetNextItemOpen(false, ImGuiCond_FirstUseEver);
246 ✗ if (ImGui::TreeNode(fmt::format("Matrices##Kalman Filter {}", nameId()).c_str()))
247 {
248 ✗ _kalmanFilter.showKalmanFilterMatrixViews(nameId().c_str());
249 ✗ ImGui::TreePop();
250 }
251 ✗ ImGui::TreePop();
252 }
253
254 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
255 ✗ if (ImGui::TreeNode(fmt::format("System/Process noise (Standard Deviations)##{}", size_t(id)).c_str()))
256 {
257 ✗ if (gui::widgets::InputDouble2WithUnit(fmt::format("Acceleration due to user motion (Hor/Ver)##{}", size_t(id)).c_str(), cWidth, unitWidth, _gui_stdevAccel.data(), _gui_stdevAccelUnits, "m/√(s^3)\0\0", "%.2e", ImGuiInputTextFlags_CharsScientific))
258 {
259 ✗ LOG_DEBUG("{}: stdevAccel changed to horizontal {} and vertical {}", nameId(), _gui_stdevAccel.at(0), _gui_stdevAccel.at(1));
260 ✗ LOG_DEBUG("{}: stdevAccelNoiseUnits changed to {}", nameId(), fmt::underlying(_gui_stdevAccelUnits));
261 ✗ flow::ApplyChanges();
262 }
263 ✗ if (gui::widgets::InputDoubleWithUnit(fmt::format("Ambiguity noise (carrier-phase bias) (float)##{}", size_t(id)).c_str(),
264 cWidth, unitWidth, &_gui_ambiguityFloatProcessNoiseStDev,
265 ✗ _gui_stdevAmbiguityFloatUnits, "cycle\0\0",
266 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
267 {
268 ✗ LOG_DEBUG("{}: ambiguityFloatProcessNoiseStDev changed to {}", nameId(), _gui_ambiguityFloatProcessNoiseStDev);
269 ✗ LOG_DEBUG("{}: stdevAmbiguityFloatUnits changed to {}", nameId(), fmt::underlying(_gui_stdevAmbiguityFloatUnits));
270
271 ✗ switch (_gui_stdevAmbiguityFloatUnits)
272 {
273 ✗ case StdevAmbiguityUnits::Cycle:
274 ✗ _ambiguityFloatProcessNoiseVariance = std::pow(_gui_ambiguityFloatProcessNoiseStDev, 2);
275 ✗ break;
276 }
277
278 ✗ flow::ApplyChanges();
279 }
280 ✗ if (gui::widgets::InputDoubleWithUnit(fmt::format("Ambiguity noise (carrier-phase bias) (fix)##{}", size_t(id)).c_str(),
281 cWidth, unitWidth, &_gui_ambiguityFixProcessNoiseStDev,
282 ✗ _gui_stdevAmbiguityFixUnits, "cycle\0\0",
283 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
284 {
285 ✗ LOG_DEBUG("{}: ambiguityFixProcessNoiseStDev changed to {}", nameId(), _gui_ambiguityFixProcessNoiseStDev);
286 ✗ LOG_DEBUG("{}: stdevAmbiguityFixUnits changed to {}", nameId(), fmt::underlying(_gui_stdevAmbiguityFixUnits));
287
288 ✗ switch (_gui_stdevAmbiguityFixUnits)
289 {
290 ✗ case StdevAmbiguityUnits::Cycle:
291 ✗ _ambiguityFixProcessNoiseVariance = std::pow(_gui_ambiguityFixProcessNoiseStDev, 2);
292 ✗ break;
293 }
294
295 ✗ flow::ApplyChanges();
296 }
297
298 ✗ ImGui::TreePop();
299 }
300
301 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
302 ✗ if (ImGui::TreeNode(fmt::format("Ambiguity resolution##{}", size_t(id)).c_str()))
303 {
304 ✗ if (!_obsFilter.isObsTypeUsed(GnssObs::Carrier)) { ImGui::BeginDisabled(); }
305
306 ✗ ImGui::SetNextItemWidth(cWidth);
307 ✗ if (gui::widgets::EnumCombo(fmt::format("Strategy##{}", size_t(id)).c_str(), _ambiguityResolutionStrategy))
308 {
309 ✗ LOG_DEBUG("{}: Ambiguity resolution strategy changed to {}", nameId(), NAV::to_string(_ambiguityResolutionStrategy));
310 ✗ flow::ApplyChanges();
311 }
312
313 ✗ if (GuiAmbiguityResolution(fmt::format("Ambiguity resolution##{}", size_t(id)).c_str(), _ambiguityResolutionParameters, itemWidth))
314 {
315 ✗ LOG_DEBUG("{}: Ambiguity resolution parameters changed to [{}][{}]", nameId(),
316 NAV::to_string(_ambiguityResolutionParameters.decorrelationAlgorithm),
317 NAV::to_string(_ambiguityResolutionParameters.searchAlgorithm));
318 ✗ flow::ApplyChanges();
319 }
320
321 ✗ ImGui::SetNextItemWidth(cWidth);
322 ✗ if (ImGui::InputDoubleL(fmt::format("Max Position Var for AR##{}", size_t(id)).c_str(), &_maxPosVar, 0, std::numeric_limits<double>::max(), 0.0, 0.0, "%.4f m"))
323 {
324 ✗ flow::ApplyChanges();
325 }
326 ✗ ImGui::SameLine();
327 ✗ gui::widgets::HelpMarker("Maximum position variance\nin order to attempt ambiguity fixing.");
328
329 ✗ auto tmp = static_cast<int>(_nMinSatForAmbFix);
330 ✗ ImGui::SetNextItemWidth(cWidth);
331 ✗ if (ImGui::InputIntL(fmt::format("Min Sat for Fix##{}", size_t(id)).c_str(), &tmp, 0))
332 {
333 ✗ _nMinSatForAmbFix = static_cast<size_t>(tmp);
334 ✗ flow::ApplyChanges();
335 }
336 ✗ ImGui::SameLine();
337 ✗ gui::widgets::HelpMarker("Minimum amount of satellites with carrier observations\nin order to attempt ambiguity fixing.");
338 ✗ if (_ambiguityResolutionStrategy == AmbiguityResolutionStrategy::FixAndHold)
339 {
340 ✗ tmp = static_cast<int>(_nMinSatForAmbHold);
341 ✗ ImGui::SetNextItemWidth(cWidth);
342 ✗ if (ImGui::InputIntL(fmt::format("Min Sat for Hold##{}", size_t(id)).c_str(), &tmp, 0))
343 {
344 ✗ _nMinSatForAmbHold = static_cast<size_t>(tmp);
345 ✗ flow::ApplyChanges();
346 }
347 ✗ ImGui::SameLine();
348 ✗ gui::widgets::HelpMarker("Minimum amount of satellites with carrier observations\nin order to attempt ambiguity holding.");
349 }
350 ✗ if (ImGui::Checkbox(fmt::format("Apply fixed ambiguities with KF update##{}", size_t(id)).c_str(), &_applyFixedAmbiguitiesWithUpdate))
351 {
352 ✗ flow::ApplyChanges();
353 }
354 ✗ ImGui::SameLine();
355 ✗ gui::widgets::HelpMarker("Make a Kalman Filter update with the fixed ambiguities when checked.\n"
356 "Otherwise apply via\n"
357 "- a = a_fix\n"
358 "- b = b_float - Q_ba * Q_aa^-1 (a_fix - a_float)");
359
360 ✗ if (_applyFixedAmbiguitiesWithUpdate
361 ✗ && gui::widgets::InputDoubleWithUnit(fmt::format("Apply fixed ambiguities measurement noise##{}", size_t(id)).c_str(),
362 cWidth, unitWidth, &_gui_ambFixUpdateStdDev,
363 ✗ _gui_ambFixUpdateStdDevUnits, "cycle\0\0",
364 0.0, 0.0, "%.2e", ImGuiInputTextFlags_CharsScientific))
365 {
366 ✗ LOG_DEBUG("{}: ambFixUpdateStdDev changed to {}", nameId(), _gui_ambFixUpdateStdDev);
367 ✗ LOG_DEBUG("{}: ambFixUpdateStdDevUnits changed to {}", nameId(), fmt::underlying(_gui_ambFixUpdateStdDevUnits));
368
369 ✗ switch (_gui_ambFixUpdateStdDevUnits)
370 {
371 ✗ case StdevAmbiguityUnits::Cycle:
372 ✗ _ambFixUpdateVariance = std::pow(_gui_ambFixUpdateStdDev, 2);
373 ✗ break;
374 }
375
376 ✗ flow::ApplyChanges();
377 }
378
379 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
380 ✗ if (ImGui::TreeNode(fmt::format("Cycle-slip detection##Tree{}", size_t(id)).c_str()))
381 {
382 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
383 ✗ if (ImGui::TreeNode(fmt::format("Base##Tree{}", size_t(id)).c_str()))
384 {
385 ✗ if (CycleSlipDetectorGui(fmt::format("Cycle-slip detector Base##{}", size_t(id)).c_str(), _cycleSlipDetector[Base], 145.0F))
386 {
387 ✗ flow::ApplyChanges();
388 }
389 ✗ ImGui::TreePop();
390 }
391 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
392 ✗ if (ImGui::TreeNode(fmt::format("Rover##Tree{}", size_t(id)).c_str()))
393 {
394 ✗ if (CycleSlipDetectorGui(fmt::format("Cycle-slip detector Rover##{}", size_t(id)).c_str(), _cycleSlipDetector[Rover], 145.0F))
395 {
396 ✗ flow::ApplyChanges();
397 }
398 ✗ ImGui::TreePop();
399 }
400
401 ✗ ImGui::TreePop();
402 }
403
404 ✗ if (!_obsFilter.isObsTypeUsed(GnssObs::Carrier)) { ImGui::EndDisabled(); }
405 ✗ ImGui::TreePop();
406 }
407 ✗ if (ImGui::TreeNode(fmt::format("Misc##{}", size_t(id)).c_str()))
408 {
409 ✗ if (ImGui::Checkbox(fmt::format("Output a SPP solution if no base observation available##{}", size_t(id)).c_str(), &_calcSPPIfNoBase))
410 {
411 ✗ flow::ApplyChanges();
412 }
413 ✗ ImGui::SetNextItemWidth(cWidth);
414 ✗ if (ImGui::InputDoubleL(fmt::format("Max Time between rover & base obs##{}", size_t(id)).c_str(), &_maxTimeBetweenBaseRoverForRTK, 1e-3))
415 {
416 ✗ flow::ApplyChanges();
417 }
418
419 ✗ ImGui::TreePop();
420 }
421
422 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
423 ✗ if (ImGui::TreeNode(fmt::format("Events##{}", size_t(id)).c_str()))
424 {
425 ✗ if (ImGui::Checkbox(fmt::format("Output State Change Events##{}", size_t(id)).c_str(), &_outputStateEvents))
426 {
427 ✗ flow::ApplyChanges();
428 }
429 ✗ ImGui::SetNextItemWidth(cWidth);
430 ✗ if (ImGui::InputText(fmt::format("Events Filter Regex##{}", size_t(id)).c_str(), &_eventFilterRegex))
431 {
432 ✗ flow::ApplyChanges();
433 }
434 ✗ if (!_events.empty())
435 {
436 ✗ std::optional<std::regex> filter;
437 try
438 {
439 ✗ filter = std::regex(_eventFilterRegex, std::regex_constants::ECMAScript | std::regex_constants::icase);
440 }
441 ✗ catch (...) // NOLINT(bugprone-empty-catch)
442 ✗ {}
443
444 ✗ if (ImGui::BeginChild(fmt::format("Events list {}", size_t(id)).c_str(), ImVec2(0.0F, 600.0F), true))
445 {
446 ✗ ImGui::PushFont(Application::MonoFont());
447 ✗ for (size_t i = 0; i < _events.size(); i++)
448 {
449 ✗ if (!std::any_of(_events.at(i).second.begin(), _events.at(i).second.end(), [&](const std::string& text) {
450 ✗ return _eventFilterRegex.empty() || (filter && std::regex_search(text, *filter));
451 }))
452 {
453 ✗ continue;
454 }
455
456 ✗ if (i != 0) { ImGui::Separator(); }
457 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_Always);
458 ✗ if (ImGui::TreeNode(fmt::format("{} GPST", _events.at(i).first.toYMDHMS(GPST)).c_str()))
459 {
460 ✗ for (const auto& text : _events.at(i).second)
461 {
462 ✗ if (_eventFilterRegex.empty() || (filter && std::regex_search(text, *filter)))
463 {
464 ✗ ImGui::BulletText("%s", text.c_str());
465 }
466 }
467 ✗ ImGui::TreePop();
468 }
469 }
470 ✗ ImGui::PopFont();
471 }
472 ✗ ImGui::EndChild();
473 ✗ }
474 ✗ ImGui::TreePop();
475 }
476 ✗ }
477
478 ✗ [[nodiscard]] json RealTimeKinematic::save() const
479 {
480 LOG_TRACE("{}: called", nameId());
481
482 ✗ json j;
483
484 ✗ j["dynamicInputPins"] = _dynamicInputPins;
485
486 ✗ j["obsFilter"] = _obsFilter;
487 ✗ j["obsEstimator"] = _obsEstimator;
488 ✗ j["kalmanFilter"] = _kalmanFilter;
489 ✗ j["maxRemoveOutlier"] = _maxRemoveOutlier;
490 ✗ j["outlierRemoveEpochs"] = _outlierRemoveEpochs;
491 ✗ j["outlierMinSat"] = _outlierMinSat;
492 ✗ j["outlierMinPsrObsKeep"] = _outlierMinPsrObsKeep;
493 ✗ j["outlierMaxPosVarStartup"] = _outlierMaxPosVarStartup;
494 ✗ j["calcSPPIfNoBase"] = _calcSPPIfNoBase;
495 ✗ j["maxTimeBetweenBaseRoverForRTK"] = _maxTimeBetweenBaseRoverForRTK;
496
497 ✗ j["ambiguityResolutionParameters"] = _ambiguityResolutionParameters;
498 ✗ j["ambiguityResolutionStrategy"] = _ambiguityResolutionStrategy;
499 ✗ j["nMinSatForAmbFix"] = _nMinSatForAmbFix;
500 ✗ j["nMinSatForAmbHold"] = _nMinSatForAmbHold;
501 ✗ j["maxPosVar"] = _maxPosVar;
502 ✗ j["applyFixedAmbiguitiesWithUpdate"] = _applyFixedAmbiguitiesWithUpdate;
503 ✗ j["ambFixUpdateStdDev"] = _gui_ambFixUpdateStdDev;
504 ✗ j["ambFixUpdateStdDevUnits"] = _gui_ambFixUpdateStdDevUnits;
505
506 ✗ j["cycleSlipDetector"] = _cycleSlipDetector;
507 ✗ j["eventFilterRegex"] = _eventFilterRegex;
508 ✗ j["outputStateEvents"] = _outputStateEvents;
509
510 ✗ j["stdevAccelUnits"] = _gui_stdevAccelUnits;
511 ✗ j["stdevAccel"] = _gui_stdevAccel;
512 ✗ j["stdevAmbiguityFloatUnits"] = _gui_stdevAmbiguityFloatUnits;
513 ✗ j["ambiguityFloatProcessNoiseStDev"] = _gui_ambiguityFloatProcessNoiseStDev;
514 ✗ j["stdevAmbiguityFixUnits"] = _gui_stdevAmbiguityFixUnits;
515 ✗ j["ambiguityFixProcessNoiseStDev"] = _gui_ambiguityFixProcessNoiseStDev;
516
517 ✗ return j;
518 ✗ }
519
520 1 void RealTimeKinematic::restore(json const& j)
521 {
522 LOG_TRACE("{}: called", nameId());
523
524
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("dynamicInputPins")) { NAV::gui::widgets::from_json(j.at("dynamicInputPins"), _dynamicInputPins, this); }
525
526
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("obsFilter")) { j.at("obsFilter").get_to(_obsFilter); }
527
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("obsEstimator")) { j.at("obsEstimator").get_to(_obsEstimator); }
528
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("kalmanFilter")) { j.at("kalmanFilter").get_to(_kalmanFilter); }
529
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("maxRemoveOutlier")) { j.at("maxRemoveOutlier").get_to(_maxRemoveOutlier); }
530
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("outlierRemoveEpochs")) { j.at("outlierRemoveEpochs").get_to(_outlierRemoveEpochs); }
531
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("outlierMinSat")) { j.at("outlierMinSat").get_to(_outlierMinSat); }
532
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("outlierMinPsrObsKeep")) { j.at("outlierMinPsrObsKeep").get_to(_outlierMinPsrObsKeep); }
533
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("outlierMaxPosVarStartup")) { j.at("outlierMaxPosVarStartup").get_to(_outlierMaxPosVarStartup); }
534
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("calcSPPIfNoBase")) { j.at("calcSPPIfNoBase").get_to(_calcSPPIfNoBase); }
535
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("maxTimeBetweenBaseRoverForRTK")) { j.at("maxTimeBetweenBaseRoverForRTK").get_to(_maxTimeBetweenBaseRoverForRTK); }
536
537
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("ambiguityResolutionParameters")) { j.at("ambiguityResolutionParameters").get_to(_ambiguityResolutionParameters); }
538
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("ambiguityResolutionStrategy")) { j.at("ambiguityResolutionStrategy").get_to(_ambiguityResolutionStrategy); }
539
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("nMinSatForAmbFix")) { j.at("nMinSatForAmbFix").get_to(_nMinSatForAmbFix); }
540
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("nMinSatForAmbHold")) { j.at("nMinSatForAmbHold").get_to(_nMinSatForAmbHold); }
541
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("maxPosVar")) { j.at("maxPosVar").get_to(_maxPosVar); }
542
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("applyFixedAmbiguitiesWithUpdate")) { j.at("applyFixedAmbiguitiesWithUpdate").get_to(_applyFixedAmbiguitiesWithUpdate); }
543
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("ambFixUpdateStdDevUnits")) { j.at("ambFixUpdateStdDevUnits").get_to(_gui_ambFixUpdateStdDevUnits); }
544
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("ambFixUpdateStdDev"))
545 {
546 1 j.at("ambFixUpdateStdDev").get_to(_gui_ambFixUpdateStdDev);
547
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 switch (_gui_ambFixUpdateStdDevUnits)
548 {
549 1 case StdevAmbiguityUnits::Cycle:
550 1 _ambFixUpdateVariance = std::pow(_gui_ambFixUpdateStdDev, 2);
551 1 break;
552 }
553 }
554
555
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("cycleSlipDetector")) { j.at("cycleSlipDetector").get_to(_cycleSlipDetector); }
556
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("eventFilterRegex")) { j.at("eventFilterRegex").get_to(_eventFilterRegex); }
557
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("outputStateEvents")) { j.at("outputStateEvents").get_to(_outputStateEvents); }
558
559
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("stdevAccelUnits")) { _gui_stdevAccelUnits = j.at("stdevAccelUnits"); }
560
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("stdevAccel")) { _gui_stdevAccel = j.at("stdevAccel"); }
561
562
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("stdevAmbiguityFloatUnits")) { j.at("stdevAmbiguityFloatUnits").get_to(_gui_stdevAmbiguityFloatUnits); }
563
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("ambiguityFloatProcessNoiseStDev"))
564 {
565 1 j.at("ambiguityFloatProcessNoiseStDev").get_to(_gui_ambiguityFloatProcessNoiseStDev);
566
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 switch (_gui_stdevAmbiguityFloatUnits)
567 {
568 1 case StdevAmbiguityUnits::Cycle:
569 1 _ambiguityFloatProcessNoiseVariance = std::pow(_gui_ambiguityFloatProcessNoiseStDev, 2);
570 1 break;
571 }
572 }
573
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("stdevAmbiguityFixUnits")) { j.at("stdevAmbiguityFixUnits").get_to(_gui_stdevAmbiguityFixUnits); }
574
575
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (j.contains("ambiguityFixProcessNoiseStDev"))
576 {
577 1 j.at("ambiguityFixProcessNoiseStDev").get_to(_gui_ambiguityFixProcessNoiseStDev);
578
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 switch (_gui_stdevAmbiguityFixUnits)
579 {
580 1 case StdevAmbiguityUnits::Cycle:
581 1 _ambiguityFixProcessNoiseVariance = std::pow(_gui_ambiguityFixProcessNoiseStDev, 2);
582 1 break;
583 }
584 }
585 1 }
586
587 3 bool RealTimeKinematic::initialize()
588 {
589 LOG_TRACE("{}: called", nameId());
590
591
2/4
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
 6 if (std::all_of(inputPins.begin() + INPUT_PORT_INDEX_GNSS_NAV_INFO, inputPins.end(), [](const InputPin& inputPin) { return !inputPin.isPinLinked(); }))
592 {
593 ✗ LOG_ERROR("{}: You need to connect a GNSS NavigationInfo provider", nameId());
594 ✗ return false;
595 }
596
597 3 _obsFilter.reset();
598 3 _events.clear();
599 3 _pivotSatellites.clear();
600 3 _lastUpdate.reset();
601 3 _dataInterval = 1;
602
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 _receiver = { { Receiver(static_cast<ReceiverType>(0), _obsFilter.getSystemFilter().toVector()),
603
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 Receiver(static_cast<ReceiverType>(1), _obsFilter.getSystemFilter().toVector()) } };
604 3 _lastSolutionStatus = RtkSolution::SolutionType::RTK_Float;
605 3 _ambiguitiesHold.clear();
606 3 nFixSolutions = 0;
607 3 nFloatSolutions = 0;
608 3 nSingleSolutions = 0;
609 3 _nPivotChange = 0;
610 3 _nCycleSlipsLLI = 0;
611 3 _nCycleSlipsSingle = 0;
612 3 _nCycleSlipsDual = 0;
613 3 _nMeasExcludedNIS = 0;
614
615 3 _outlierMaxPosVarStartupTriggered = false;
616 3 _maxPosVarTriggered = false;
617 3 _nMinSatForAmbFixTriggered = false;
618 3 _nMinSatForAmbHoldTriggered = false;
619
620
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 _sppAlgorithm.reset();
621
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 _sppAlgorithm._obsFilter = _obsFilter;
622
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 _sppAlgorithm._obsFilter.useObsType(GnssObs::Pseudorange);
623
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 _sppAlgorithm._obsFilter.useObsType(GnssObs::Doppler);
624
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 _sppAlgorithm._obsEstimator = _obsEstimator;
625 3 _sppAlgorithm._estimateInterFreqBiases = false;
626
627 3 json j;
628
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 to_json(j, _kalmanFilter);
629
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 _kalmanFilter = KeyedKalmanFilterD<States::StateKeyType,
630 3 Meas::MeasKeyTypes>{ States::PosVel, {} };
631
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 from_json(j, _kalmanFilter);
632
633
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
 3 _kalmanFilter.F.block<3>(States::Pos, States::Vel) = Eigen::Matrix3d::Identity();
634
635
3/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
✓ Branch 4 taken 3 times.
 9 for (auto& detector : _cycleSlipDetector) { detector.reset(); }
636
637 // 𝜎²_a Variance of the acceleration due to user motion in horizontal and vertical component in [m^2 / s^3]
638 3 std::array<double, 2> varAccel{};
639
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 3 switch (_gui_stdevAccelUnits)
640 {
641 3 case StdevAccelUnits::m_sqrts3: // [m / √(s^3)]
642
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 varAccel = { std::pow(_gui_stdevAccel.at(0), 2), std::pow(_gui_stdevAccel.at(1), 2) };
643 3 break;
644 }
645 LOG_DATA(" sigma2_accel = h: {}, v: {} [m^2 / s^3]", nameId(), varAccel.at(0), varAccel.at(1));
646
8/16
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
 3 _kalmanFilter.W.block<3>(States::Vel, States::Vel).diagonal() << varAccel.at(0), varAccel.at(0), varAccel.at(1);
647
648
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 6 LOG_DEBUG("RealTimeKinematic initialized");
649
650 3 return true;
651 12 }
652
653 1 void RealTimeKinematic::deinitialize()
654 {
655 LOG_TRACE("{}: called", nameId());
656 1 }
657
658 116 void RealTimeKinematic::pinAddCallback(Node* node)
659 {
660
5/10
✓ Branch 2 taken 116 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 116 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 116 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 116 times.
✓ Branch 15 taken 116 times.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 464 node->CreateInputPin(NAV::GnssNavInfo::type().c_str(), Pin::Type::Object, { NAV::GnssNavInfo::type() });
661 232 }
662
663 ✗ void RealTimeKinematic::pinDeleteCallback(Node* node, size_t pinIdx)
664 {
665 ✗ node->DeleteInputPin(pinIdx);
666 ✗ }
667
668 69 void RealTimeKinematic::addEventToGui(const std::shared_ptr<RtkSolution>& rtkSol, const std::string& text)
669 {
670
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 152 auto iter = std::ranges::find_if(_events, [&](const auto& event) { return event.first == rtkSol->insTime; });
671
2/2
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 65 times.
 69 if (iter == _events.end())
672 {
673
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✓ Branch 9 taken 4 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 12 _events.emplace_back(rtkSol->insTime, std::vector{ text });
674 }
675 else
676 {
677
1/2
✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
 65 iter->second.emplace_back(text);
678 }
679
1/2
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
 69 rtkSol->addEvent(text);
680 73 }
681
682 601 void RealTimeKinematic::printObservations([[maybe_unused]] const Observations& observations)
683 {
684 #if LOG_LEVEL <= LOG_LEVEL_DATA
685 unordered_map<SatId, std::pair<Frequency, Code>> satData;
686 unordered_map<SatSigId, std::set<GnssObs::ObservationType>> sigData;
687 for (const auto& obs : observations.signals)
688 {
689 satData[obs.first.toSatId()].first |= obs.first.freq();
690 satData[obs.first.toSatId()].second |= obs.first.code;
691 for (size_t obsType = 0; obsType < GnssObs::ObservationType_COUNT; obsType++)
692 {
693 if (std::ranges::all_of(obs.second.recvObs, [&obsType](const auto& recvObs) {
694 return recvObs.second->obs.contains(static_cast<GnssObs::ObservationType>(obsType));
695 }))
696 {
697 sigData[obs.first].insert(static_cast<GnssObs::ObservationType>(obsType));
698 }
699 }
700 }
701 for ([[maybe_unused]] const auto& [satId, freqCode] : satData)
702 {
703 LOG_DATA("{}: [{}] on frequencies [{}] with codes [{}]", nameId(), satId, freqCode.first, freqCode.second);
704 for (const auto& [satSigId, obs] : sigData)
705 {
706 if (satSigId.toSatId() != satId) { continue; }
707 std::string str;
708 for (const auto& o : obs)
709 {
710 if (!str.empty()) { str += ", "; }
711 str += fmt::format("{}", o);
712 }
713 LOG_DATA("{}: [{}] has obs: {}", nameId(), satSigId.code, str);
714 }
715 }
716 #endif
717 601 }
718
719 1 void RealTimeKinematic::recvBasePos(InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
720 {
721
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 auto gnssObs = std::static_pointer_cast<const Pos>(queue.extract_front());
722 LOG_DATA("{}: Received Base Position for [{}]", nameId(), gnssObs->insTime.toYMDHMS(GPST));
723
724
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 if (_receiver[Base].e_posMarker.isZero())
725 {
726
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 _receiver[Base].e_posMarker = gnssObs->e_position();
727
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 _receiver[Base].lla_posMarker = gnssObs->lla_position();
728 }
729 else
730 {
731 ✗ _receiver[Base].e_posMarker = (_receiver[Base].e_posMarker + gnssObs->e_position()) / 2;
732 ✗ _receiver[Base].lla_posMarker = trafo::ecef2lla_WGS84(_receiver[Base].e_posMarker);
733 }
734
735
2/14
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
 1 if (_receiver[Base].gnssObs && _receiver[Rover].gnssObs && std::chrono::abs(_receiver[Base].gnssObs->insTime - _receiver[Rover].gnssObs->insTime) < std::chrono::milliseconds(10))
736 {
737 ✗ calcRealTimeKinematicSolution();
738 }
739 1 }
740
741 601 void RealTimeKinematic::recvBaseGnssObs(InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
742 {
743
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 _receiver[Base].gnssObs = std::static_pointer_cast<const GnssObs>(queue.extract_front());
744 601 _baseObsReceivedThisEpoch = true;
745 LOG_DATA("{}: Received Base GNSS Obs for [{}]", nameId(), _receiver[Base].gnssObs->insTime.toYMDHMS(GPST));
746
747
2/10
✗ Branch 2 not taken.
✓ Branch 3 taken 601 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✓ Branch 19 taken 601 times.
 601 if (_receiver[Rover].gnssObs && (_receiver[Base].gnssObs->insTime - _receiver[Rover].gnssObs->insTime) > std::chrono::milliseconds(500))
748 {
749 LOG_TRACE("{}: Data gap between Base [{}] and Rover [{}] is {:.1f}s. Falling back to SPP for gaps larger than 0.5s", nameId(),
750 _receiver[Base].gnssObs->insTime.toYMDHMS(GPST), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST),
751 static_cast<double>((_receiver[Base].gnssObs->insTime - _receiver[Rover].gnssObs->insTime).count()));
752 ✗ if (auto rtkSol = calcFallbackSppSolution())
753 {
754 ✗ nSingleSolutions++;
755 ✗ invokeCallbacks(OUTPUT_PORT_INDEX_RTKSOL, rtkSol);
756 ✗ }
757 ✗ _receiver[Rover].gnssObs = nullptr;
758 }
759
3/6
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 601 times.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 601 times.
 1202 else if (!_receiver[Base].e_posMarker.isZero() && _receiver[Base].gnssObs && _receiver[Rover].gnssObs
760
2/12
✓ Branch 0 taken 601 times.
✗ Branch 1 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 601 times.
 1202 && std::chrono::abs(_receiver[Base].gnssObs->insTime - _receiver[Rover].gnssObs->insTime) < std::chrono::milliseconds(100))
761 {
762 ✗ calcRealTimeKinematicSolution();
763 }
764 601 }
765
766 601 void RealTimeKinematic::recvRoverGnssObs(InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
767 {
768
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 _receiver[Rover].gnssObs = std::static_pointer_cast<const GnssObs>(queue.extract_front());
769 LOG_DATA("{}: Received Rover GNSS Obs for [{}]", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST));
770
771
5/10
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
✓ Branch 10 taken 601 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 601 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 601 times.
✗ Branch 18 not taken.
✓ Branch 19 taken 601 times.
 601 if (_receiver[Base].gnssObs && (_receiver[Rover].gnssObs->insTime - _receiver[Base].gnssObs->insTime) > std::chrono::milliseconds(static_cast<int>(_maxTimeBetweenBaseRoverForRTK * 1e3)))
772 {
773 ✗ if (!_calcSPPIfNoBase) { return; }
774 LOG_TRACE("{}: Calculating SPP Fallback, as Rover obs [{}] is {:.3f} seconds away from last base obs [{}]. Maximum time to consider the base observation is {} seconds.",
775 nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST),
776 static_cast<double>((_receiver[Rover].gnssObs->insTime - _receiver[Base].gnssObs->insTime).count()),
777 _receiver[Base].gnssObs->insTime.toYMDHMS(GPST),
778 _maxTimeBetweenBaseRoverForRTK);
779 ✗ if (auto rtkSol = calcFallbackSppSolution())
780 {
781 ✗ nSingleSolutions++;
782 ✗ invokeCallbacks(OUTPUT_PORT_INDEX_RTKSOL, rtkSol);
783 ✗ }
784 ✗ _receiver[Rover].gnssObs = nullptr;
785 }
786
3/6
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 601 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 601 times.
✗ Branch 12 not taken.
 1202 else if (!_receiver[Base].e_posMarker.isZero() && _receiver[Base].gnssObs && _receiver[Rover].gnssObs
787
6/12
✓ Branch 0 taken 601 times.
✗ Branch 1 not taken.
✓ Branch 8 taken 601 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 601 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 601 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 601 times.
✗ Branch 18 not taken.
✓ Branch 19 taken 601 times.
✗ Branch 20 not taken.
 1202 && std::chrono::abs(_receiver[Base].gnssObs->insTime - _receiver[Rover].gnssObs->insTime) < std::chrono::milliseconds(100))
788 {
789 601 calcRealTimeKinematicSolution();
790 }
791 }
792
793 1 void RealTimeKinematic::assignSolutionToFilter(const std::shared_ptr<NAV::SppSolution>& sppSol)
794 {
795 1 _lastUpdate = sppSol->insTime;
796 1 _receiver[Rover].e_posMarker = sppSol->e_position();
797
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 _receiver[Rover].lla_posMarker = trafo::ecef2lla_WGS84(_receiver[Rover].e_posMarker);
798
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 if (!sppSol->e_velocity().hasNaN())
799 {
800 1 _receiver[Rover].e_vel = sppSol->e_velocity();
801 }
802
803
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 _kalmanFilter.x.segment<3>(States::Pos) = _receiver[Rover].e_posMarker;
804
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 _kalmanFilter.x.segment<3>(States::Vel) = _receiver[Rover].e_vel;
805
806
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 if (sppSol->e_CovarianceMatrix().has_value())
807 {
808
2/4
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 if (sppSol->e_CovarianceMatrix()->get().hasRows(Keys::PosVel<Keys::MotionModelKey>))
809 {
810
3/6
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 _kalmanFilter.P(States::PosVel, States::PosVel) = (*sppSol->e_CovarianceMatrix())(Keys::PosVel<Keys::MotionModelKey>, Keys::PosVel<Keys::MotionModelKey>);
811 }
812 else
813 {
814 ✗ _kalmanFilter.P(States::Pos, States::Pos) = (*sppSol->e_CovarianceMatrix())(Keys::Pos<Keys::MotionModelKey>, Keys::Pos<Keys::MotionModelKey>);
815 ✗ _kalmanFilter.P(States::Vel, States::Vel) = Eigen::DiagonalMatrix<double, 3>(1, 1, 1);
816 }
817 }
818 1 }
819
820 601 void RealTimeKinematic::calcRealTimeKinematicSolution()
821 {
822
3/4
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 600 times.
✓ Branch 6 taken 600 times.
✗ Branch 7 not taken.
 601 auto dt = _lastUpdate.empty() ? 0.0 : static_cast<double>((_receiver[Rover].gnssObs->insTime - _lastUpdate).count());
823 LOG_DATA("{}: Calculate RTK Solution for rover [{}], base [{}] (dt = {:.2f})", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), _receiver[Base].gnssObs->insTime.toYMDHMS(GPST), dt);
824
825 // Collection of all connected navigation data providers
826 601 std::vector<InputPin::IncomingLink::ValueWrapper<GnssNavInfo>> gnssNavInfoWrappers;
827 601 std::vector<const GnssNavInfo*> gnssNavInfos;
828
3/4
✓ Branch 1 taken 1803 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1202 times.
✓ Branch 4 taken 601 times.
 1803 for (size_t i = 0; i < _dynamicInputPins.getNumberOfDynamicPins(); i++)
829 {
830
2/4
✓ Branch 1 taken 1202 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1202 times.
✗ Branch 5 not taken.
 1202 if (auto gnssNavInfo = getInputValue<GnssNavInfo>(INPUT_PORT_INDEX_GNSS_NAV_INFO + i))
831 {
832
1/2
✓ Branch 2 taken 1202 times.
✗ Branch 3 not taken.
 1202 gnssNavInfoWrappers.push_back(*gnssNavInfo);
833
1/2
✓ Branch 2 taken 1202 times.
✗ Branch 3 not taken.
 1202 gnssNavInfos.push_back(gnssNavInfo->v);
834 1202 }
835 }
836
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 if (!gnssNavInfos.empty())
837 {
838
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 auto rtkSol = std::make_shared<RtkSolution>();
839 601 rtkSol->insTime = _receiver[Rover].gnssObs->insTime;
840 601 rtkSol->baseTime = _receiver[Base].gnssObs->insTime;
841
842
4/4
✓ Branch 0 taken 600 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 60 times.
✓ Branch 3 taken 540 times.
 601 bool startupPhase = dt == 0.0 ? true : static_cast<double>(nFixSolutions + nFloatSolutions + nSingleSolutions) * dt < 60.0;
843
844
6/8
✓ Branch 0 taken 540 times.
✓ Branch 1 taken 61 times.
✓ Branch 5 taken 540 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 540 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 540 times.
✓ Branch 10 taken 61 times.
 601 if (!startupPhase && !_receiver[Rover].e_posMarker.isZero())
845 {
846
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 540 times.
 540 if (dt > 10.0)
847 {
848 ✗ _ambiguitiesHold.clear();
849 ✗ _pivotSatellites.clear();
850 ✗ std::vector<States::StateKeyType> ambKeys;
851 ✗ for (size_t i = States::KFStates_COUNT; i < _kalmanFilter.x.rowKeys().size(); i++) // 0-2 Pos, 3-5 Vel
852 {
853 ✗ if (const auto* ambDD = std::get_if<States::AmbiguityDD>(&_kalmanFilter.x.rowKeys().at(i)))
854 {
855 ✗ ambKeys.emplace_back(*ambDD);
856 }
857 }
858 ✗ _kalmanFilter.removeStates(ambKeys);
859 ✗ _receiver[Rover].e_posMarker.setZero();
860 ✗ _receiver[Rover].e_vel.setZero();
861
862 ✗ std::string text = fmt::format("Outage of {:.1f} [s] (interval {:.1f} [s]). Reinitializing with SPP solution.", dt, _dataInterval);
863 ✗ LOG_WARN("{}: [{}] {}", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), text);
864 ✗ addEventToGui(rtkSol, text);
865 ✗ }
866
3/6
✓ Branch 1 taken 540 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 540 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 540 times.
 540 else if (_kalmanFilter.x.rows() > States::KFStates_COUNT && dt > 3.0 * _dataInterval)
867 {
868 ✗ _ambiguitiesHold.clear();
869 ✗ std::vector<States::StateKeyType> ambKeys;
870 ✗ for (size_t i = States::KFStates_COUNT; i < _kalmanFilter.x.rowKeys().size(); i++) // 0-2 Pos, 3-5 Vel
871 {
872 ✗ if (const auto* ambDD = std::get_if<States::AmbiguityDD>(&_kalmanFilter.x.rowKeys().at(i)))
873 {
874 ✗ ambKeys.emplace_back(*ambDD);
875 }
876 }
877 ✗ _kalmanFilter.removeStates(ambKeys);
878
879 ✗ std::string text = fmt::format("Outage of {:.1f} [s] (interval {:.1f} [s]). Resetting ambiguities.", dt, _dataInterval);
880 ✗ LOG_WARN("{}: [{}] {}", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), text);
881 ✗ addEventToGui(rtkSol, text);
882 ✗ }
883 }
884
885 // Calculate a Single point solution if the Rover Position is not known yet
886
3/4
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 600 times.
 601 if (_receiver[Rover].e_posMarker.isZero())
887 {
888
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 1 if (auto sol = _sppAlgorithm.calcSppSolution(_receiver[Rover].gnssObs, gnssNavInfos, nameId()))
889 {
890
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assignSolutionToFilter(sol);
891
892
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
 1 LOG_DEBUG("{}: Initial base position: {}°, {}°, {}m (ECEF {} {} {} [m])", nameId(),
893 rad2deg(_receiver[Base].lla_posMarker(0)), rad2deg(_receiver[Base].lla_posMarker(1)), _receiver[Base].lla_posMarker(2),
894 _receiver[Base].e_posMarker.x(), _receiver[Base].e_posMarker.y(), _receiver[Base].e_posMarker.z());
895
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
 1 LOG_DEBUG("{}: Initial rover position: {}°, {}°, {}m (ECEF {} {} {} [m])", nameId(),
896 rad2deg(_receiver[Rover].lla_posMarker(0)), rad2deg(_receiver[Rover].lla_posMarker(1)), _receiver[Rover].lla_posMarker(2),
897 _receiver[Rover].e_posMarker.x(), _receiver[Rover].e_posMarker.y(), _receiver[Rover].e_posMarker.z());
898
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 LOG_DEBUG("{}: Initial rover velocity: {} [m/s] (ECEF)", nameId(), _receiver[Rover].e_vel.transpose());
899 }
900 else
901 {
902 LOG_DATA("{}: Could not calculate initial position solution.", nameId());
903 ✗ return;
904 1 }
905 }
906
907
2/2
✓ Branch 0 taken 600 times.
✓ Branch 1 taken 1 times.
 601 if (_outlierMaxPosVarStartupTriggered) { startupPhase = false; }
908
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 else if (startupPhase)
909 {
910
3/6
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 1 if (double posVar = _kalmanFilter.P(States::Pos, States::Pos).diagonal().sum() / 3.0;
911
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 posVar < _outlierMaxPosVarStartup)
912 {
913 1 _outlierMaxPosVarStartupTriggered = true;
914
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 addEventToGui(rtkSol, fmt::format("Doing NIS check from now on because\nposition variance is below {:.4f}m^2", _outlierMaxPosVarStartup));
915 }
916 }
917
918 // Collection of all connected Ionospheric Corrections
919
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 auto ionosphericCorrections = std::make_shared<const IonosphericCorrections>(gnssNavInfos);
920
921
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 kalmanFilterPrediction();
922
923 {
924
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 auto logLevel = spdlog::get_level();
925
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 spdlog::set_level(spdlog::level::debug);
926 // Calculate all possible observations here, to put into solution for analysis
927
2/4
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 601 times.
✗ Branch 6 not taken.
 1202 ObservationFilter obsFilter{ ReceiverType::ReceiverType_COUNT };
928
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 obsFilter.disableFilter();
929
930 601 Observations observations;
931 601 obsFilter.selectObservationsForCalculation(Rover,
932
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 _receiver[Rover].e_posMarker,
933 601 _receiver[Rover].lla_posMarker,
934 601 _receiver[Rover].gnssObs,
935 gnssNavInfos,
936
2/4
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 601 times.
✗ Branch 5 not taken.
 1202 observations, nullptr, nameId() + " no filter");
937 601 obsFilter.selectObservationsForCalculation(Base,
938
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 _receiver[Base].e_posMarker,
939 601 _receiver[Base].lla_posMarker,
940 601 _receiver[Base].gnssObs,
941 gnssNavInfos,
942
2/4
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 601 times.
✗ Branch 5 not taken.
 1202 observations, nullptr, nameId() + " no filter");
943
1/2
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
 601 removeSingleObservations(observations, nullptr, nullptr);
944
945
2/2
✓ Branch 7 taken 40868 times.
✓ Branch 8 taken 601 times.
 41469 for (const auto& [satSigId, sigObs] : observations.signals)
946 {
947
1/2
✓ Branch 1 taken 40868 times.
✗ Branch 2 not taken.
 40868 auto satId = satSigId.toSatId();
948
1/2
✓ Branch 2 taken 40868 times.
✗ Branch 3 not taken.
 40868 if (std::ranges::find_if(rtkSol->satData, [&satId](const auto& sat) {
949 388246 return satId == sat.first;
950 })
951
2/2
✓ Branch 3 taken 28848 times.
✓ Branch 4 taken 12020 times.
 81736 != rtkSol->satData.end())
952 {
953 28848 continue;
954 }
955
1/2
✓ Branch 2 taken 12020 times.
✗ Branch 3 not taken.
 24040 rtkSol->satData.emplace_back(satSigId.toSatId(),
956
2/4
✓ Branch 1 taken 12020 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 12020 times.
✗ Branch 8 not taken.
 12020 RtkSolution::SatData{ .satElevation = sigObs.recvObs.at(Rover)->satElevation(_receiver[Rover].e_posMarker, _receiver[Rover].lla_posMarker),
957
3/6
✓ Branch 1 taken 12020 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 12020 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12020 times.
✗ Branch 11 not taken.
 24040 .satAzimuth = sigObs.recvObs.at(Rover)->satElevation(_receiver[Rover].e_posMarker, _receiver[Rover].lla_posMarker) });
958 }
959
2/2
✓ Branch 7 taken 40868 times.
✓ Branch 8 taken 601 times.
 41469 for (const auto& [satSigId, sigObs] : observations.signals)
960 {
961
3/4
✓ Branch 1 taken 40868 times.
✗ Branch 2 not taken.
✓ Branch 9 taken 122604 times.
✓ Branch 10 taken 40868 times.
 163472 for (const auto& obs : sigObs.recvObs.at(Rover)->obs)
962 {
963
1/2
✓ Branch 2 taken 122604 times.
✗ Branch 3 not taken.
 122604 rtkSol->observableReceived.emplace(satSigId, obs.first);
964 }
965 }
966
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 spdlog::set_level(logLevel);
967 601 }
968
969 601 Observations observations;
970 601 ObservationFilter::Filtered filtered;
971 1202 _obsFilter.selectObservationsForCalculation(Rover,
972
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 _receiver[Rover].e_posMarker,
973 601 _receiver[Rover].lla_posMarker,
974 601 _receiver[Rover].gnssObs,
975 gnssNavInfos,
976
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 1202 observations, &filtered, nameId());
977 1202 _obsFilter.selectObservationsForCalculation(Base,
978
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 _receiver[Base].e_posMarker,
979 601 _receiver[Base].lla_posMarker,
980 601 _receiver[Base].gnssObs,
981 gnssNavInfos,
982
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 1202 observations, &filtered, nameId());
983
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 removeSingleObservations(observations, &filtered, rtkSol);
984
1/2
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
 601 rtkSol->filtered = filtered;
985
986
2/2
✓ Branch 7 taken 27786 times.
✓ Branch 8 taken 601 times.
 28387 for (const auto& [satSigId, sigObs] : observations.signals)
987 {
988
3/4
✓ Branch 1 taken 27786 times.
✗ Branch 2 not taken.
✓ Branch 9 taken 83358 times.
✓ Branch 10 taken 27786 times.
 111144 for (const auto& obs : sigObs.recvObs.at(Rover)->obs)
989 {
990
1/2
✓ Branch 2 taken 83358 times.
✗ Branch 3 not taken.
 83358 rtkSol->observableFiltered.emplace(satSigId, obs.first);
991 }
992 }
993 601 rtkSol->solType = RtkSolution::SolutionType::Predicted;
994
995
3/6
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 601 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 601 times.
✗ Branch 7 not taken.
 601 if (!observations.signals.empty() && observations.satellites.size() > 1)
996 {
997
2/4
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 601 times.
✗ Branch 6 not taken.
 601 _obsEstimator.calcObservationEstimates(observations, _receiver[Rover], ionosphericCorrections, nameId(), ObservationEstimator::DoubleDifference);
998
2/4
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 601 times.
✗ Branch 6 not taken.
 601 _obsEstimator.calcObservationEstimates(observations, _receiver[Base], ionosphericCorrections, nameId(), ObservationEstimator::DoubleDifference);
999
1000
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 checkForCycleSlip(observations, rtkSol);
1001
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 updatePivotSatellites(observations, rtkSol);
1002
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 addOrRemoveKalmanFilterAmbiguities(observations, rtkSol);
1003
1004 601 printObservations(observations);
1005
1006
3/4
✓ Branch 0 taken 600 times.
✓ Branch 1 taken 1 times.
✓ Branch 3 taken 600 times.
✗ Branch 4 not taken.
 601 bool nisEnabled = !startupPhase && _kalmanFilter.isNISenabled();
1007
3/4
✓ Branch 0 taken 600 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 600 times.
✗ Branch 3 not taken.
 601 size_t maxIter = nisEnabled ? (_maxRemoveOutlier == 0 ? 1000 : _maxRemoveOutlier) : 1;
1008
1009 601 bool pivotChanged = true;
1010 601 Differences singleDifferences;
1011 601 Differences doubleDifferences;
1012 601 unordered_map<GnssObs::ObservationType, KeyedMatrixXd<RTK::Meas::SingleObs<RealTimeKinematic::ReceiverType>>> Rtilde;
1013
1014
2/2
✓ Branch 0 taken 601 times.
✓ Branch 1 taken 1 times.
 602 for (size_t i = 0; i < maxIter; i++)
1015 {
1016 LOG_DATA("{}: Iteration: {}/{}", nameId(), i, maxIter);
1017
1018
1/2
✓ Branch 0 taken 601 times.
✗ Branch 1 not taken.
 601 if (pivotChanged)
1019 {
1020
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 singleDifferences = calcSingleDifferences(observations);
1021
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 doubleDifferences = calcDoubleDifferences(observations, singleDifferences);
1022
1023
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 Rtilde = calcSingleObsMeasurementNoiseMatrices(observations);
1024
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 calcKalmanUpdateMatrices(observations, doubleDifferences, Rtilde);
1025 601 pivotChanged = false;
1026 }
1027
1028
3/4
✓ Branch 0 taken 600 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 600 times.
✗ Branch 3 not taken.
 601 if (nisEnabled && !startupPhase)
1029 {
1030 LOG_DATA("{}: Doing NIS check", nameId());
1031
2/4
✓ Branch 1 taken 600 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 600 times.
✗ Branch 5 not taken.
 600 auto nisResult = _kalmanFilter.checkForOutliersNIS(nameId());
1032
1/2
✓ Branch 2 taken 600 times.
✗ Branch 3 not taken.
 600 if (!rtkSol->nisResultInitial) { rtkSol->nisResultInitial = nisResult; }
1033 600 rtkSol->nisResultFinal = nisResult;
1034
2/6
✗ Branch 0 not taken.
✓ Branch 1 taken 600 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 600 times.
 600 if (nisResult.triggered && observations.satellites.size() <= _outlierMinSat)
1035 {
1036 ✗ nisResult.triggered = false;
1037 ✗ addEventToGui(rtkSol, "Stopped doing NIS check, as minimum amount of satellites reached.");
1038 }
1039
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 600 times.
 600 if (nisResult.triggered)
1040 {
1041 LOG_DATA("{}: NIS triggered", nameId());
1042
1043 ✗ if (auto outliers = removeOutlier(observations, rtkSol);
1044 ✗ !outliers.empty())
1045 {
1046 ✗ removeSingleObservations(observations, &rtkSol->filtered, rtkSol);
1047 ✗ for (const auto& outlier : outliers)
1048 {
1049 ✗ pivotChanged |= outlier.pivot.has_value();
1050 }
1051 }
1052 else
1053 {
1054 LOG_DATA("{}: NIS check finished as nothing removed", nameId());
1055 ✗ break;
1056 ✗ }
1057 }
1058 else
1059 {
1060 LOG_DATA("{}: NIS check successful", nameId());
1061 600 break;
1062 }
1063 }
1064 }
1065
2/2
✓ Branch 0 taken 600 times.
✓ Branch 1 taken 1 times.
 601 if (nisEnabled)
1066 {
1067 LOG_DATA("{}: Final NIS = {} < {} = r2. {} removed", nameId(), rtkSol->nisResultFinal->NIS, rtkSol->nisResultFinal->r2, rtkSol->nisRemovedCnt);
1068
2/4
✓ Branch 1 taken 600 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 600 times.
✗ Branch 5 not taken.
 600 observations.recalcObservableCounts(nameId());
1069 }
1070
1071
2/4
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
 601 if (kalmanFilterUpdate(rtkSol).valid)
1072 {
1073
3/6
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 601 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 601 times.
✗ Branch 7 not taken.
 601 if (resolveAmbiguities(observations.nObservablesUniqueSatellite.at(GnssObs::Carrier), rtkSol))
1074 {
1075 601 rtkSol->solType = RtkSolution::SolutionType::RTK_Fixed;
1076 601 nFixSolutions++;
1077 }
1078 else
1079 {
1080 ✗ rtkSol->solType = RtkSolution::SolutionType::RTK_Float;
1081 ✗ nFloatSolutions++;
1082 }
1083 }
1084
1085
1/2
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
 601 rtkSol->measInnovation = _kalmanFilter.z;
1086 601 }
1087 else
1088 {
1089 LOG_TRACE("{}: No observations found. Check that your base and rover file match and reconfigure the filter.", nameId());
1090 }
1091
1092 601 _lastSolutionStatus = rtkSol->solType;
1093
1094 // Write out results
1095
2/2
✓ Branch 0 taken 1803 times.
✓ Branch 1 taken 601 times.
 2404 for (size_t obsType = 0; obsType < GnssObs::ObservationType_COUNT; obsType++)
1096 {
1097
2/4
✓ Branch 2 taken 1803 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1803 times.
✗ Branch 6 not taken.
 1803 rtkSol->nObservations.emplace(static_cast<GnssObs::ObservationType>(obsType), observations.nObservables.at(obsType));
1098
2/4
✓ Branch 2 taken 1803 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1803 times.
✗ Branch 6 not taken.
 1803 rtkSol->nObservationsUniqueSatellite.emplace(static_cast<GnssObs::ObservationType>(obsType), observations.nObservablesUniqueSatellite.at(obsType));
1099 }
1100 601 rtkSol->nSatellites = observations.satellites.size();
1101
2/4
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 601 times.
✗ Branch 7 not taken.
 601 rtkSol->distanceBaseRover = (_receiver[Base].e_posMarker - _receiver[Rover].e_posMarker).norm();
1102
1103
1/2
✓ Branch 4 taken 601 times.
✗ Branch 5 not taken.
 1202 rtkSol->setPosVelAndCov_e(_receiver[Rover].e_posMarker, _receiver[Rover].e_vel,
1104
1/2
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
 601 _kalmanFilter.P.block<6>(States::PosVel, States::PosVel));
1105
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 601 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 601 times.
✗ Branch 7 not taken.
 601 if (rtkSol->solType == RtkSolution::SolutionType::RTK_Fixed || rtkSol->solType == RtkSolution::SolutionType::RTK_Float)
1106 {
1107
1/2
✓ Branch 4 taken 601 times.
✗ Branch 5 not taken.
 601 rtkSol->ambiguityDD_br.reserve(_kalmanFilter.x.rowKeys().size() - States::KFStates_COUNT);
1108
2/2
✓ Branch 2 taken 23579 times.
✓ Branch 3 taken 601 times.
 24180 for (size_t i = States::KFStates_COUNT; i < _kalmanFilter.x.rowKeys().size(); i++) // 0-2 Pos, 3-5 Vel
1109 {
1110
2/4
✓ Branch 2 taken 23579 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23579 times.
✗ Branch 6 not taken.
 23579 if (const auto* ambDD = std::get_if<States::AmbiguityDD>(&_kalmanFilter.x.rowKeys().at(i)))
1111 {
1112 23579 const auto& key = States::AmbiguityDD(ambDD->satSigId);
1113 LOG_DATA("{}: Key: {}", nameId(), key);
1114 auto ambDDSol = RtkSolution::AmbiguityDD{
1115
1/2
✓ Branch 2 taken 23579 times.
✗ Branch 3 not taken.
 23579 .pivotSatSigId = _pivotSatellites.at({ ambDD->satSigId.code, GnssObs::Carrier }).satSigId,
1116 .satSigId = ambDD->satSigId,
1117
1/2
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
 23579 .value = UncertainValue<double>{ .value = _kalmanFilter.x(key),
1118
1/2
✓ Branch 2 taken 23579 times.
✗ Branch 3 not taken.
 23579 .stdDev = std::sqrt(_kalmanFilter.P(key, key)) }
1119 47158 };
1120
1/2
✓ Branch 2 taken 23579 times.
✗ Branch 3 not taken.
 23579 rtkSol->ambiguityDD_br.push_back(ambDDSol);
1121 }
1122 }
1123
2/2
✓ Branch 5 taken 12621 times.
✓ Branch 6 taken 601 times.
 13222 for (const auto& pivot : _pivotSatellites)
1124 {
1125
1/2
✓ Branch 2 taken 12621 times.
✗ Branch 3 not taken.
 12621 rtkSol->pivots.emplace(pivot.second.satSigId, pivot.first.second);
1126 }
1127
2/2
✓ Branch 7 taken 27786 times.
✓ Branch 8 taken 601 times.
 28387 for (const auto& [satSigId, sigObs] : observations.signals)
1128 {
1129
3/4
✓ Branch 1 taken 27786 times.
✗ Branch 2 not taken.
✓ Branch 9 taken 83358 times.
✓ Branch 10 taken 27786 times.
 111144 for (const auto& obs : sigObs.recvObs.at(Rover)->obs)
1130 {
1131
1/2
✓ Branch 2 taken 83358 times.
✗ Branch 3 not taken.
 83358 rtkSol->observableUsed.emplace(satSigId, obs.first);
1132 }
1133 }
1134 }
1135
1136
2/2
✓ Branch 0 taken 600 times.
✓ Branch 1 taken 1 times.
 601 if (dt > 1e-3) { _dataInterval = dt; }
1137 601 _lastUpdate = rtkSol->insTime;
1138
1/2
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
 601 invokeCallbacks(OUTPUT_PORT_INDEX_RTKSOL, rtkSol);
1139 601 }
1140
1141 601 _receiver[Rover].gnssObs = nullptr;
1142 601 _baseObsReceivedThisEpoch = false;
1143 601 }
1144
1145 ✗ std::shared_ptr<RtkSolution> RealTimeKinematic::calcFallbackSppSolution()
1146 {
1147 LOG_DATA("{}: Calculate SPP Solution for [{}] (fallback)", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST));
1148
1149 // Collection of all connected navigation data providers
1150 ✗ std::vector<InputPin::IncomingLink::ValueWrapper<GnssNavInfo>> gnssNavInfoWrappers;
1151 ✗ std::vector<const GnssNavInfo*> gnssNavInfos;
1152 ✗ for (size_t i = 0; i < _dynamicInputPins.getNumberOfDynamicPins(); i++)
1153 {
1154 ✗ if (auto gnssNavInfo = getInputValue<GnssNavInfo>(INPUT_PORT_INDEX_GNSS_NAV_INFO + i))
1155 {
1156 ✗ gnssNavInfoWrappers.push_back(*gnssNavInfo);
1157 ✗ gnssNavInfos.push_back(gnssNavInfo->v);
1158 ✗ }
1159 }
1160
1161 ✗ if (auto sppSol = _sppAlgorithm.calcSppSolution(_receiver[Rover].gnssObs, gnssNavInfos, nameId()))
1162 {
1163 ✗ auto rtkSol = std::make_shared<RtkSolution>();
1164 ✗ rtkSol->insTime = sppSol->insTime;
1165 ✗ rtkSol->solType = RtkSolution::SolutionType::SPP;
1166 ✗ rtkSol->nSatellites = sppSol->nSatellites;
1167 ✗ if (!_receiver[Base].e_posMarker.isZero())
1168 {
1169 ✗ rtkSol->distanceBaseRover = (_receiver[Base].e_posMarker - _receiver[Rover].e_posMarker).norm();
1170 }
1171 ✗ rtkSol->nObservations[GnssObs::Pseudorange] = sppSol->nMeasPsr;
1172 ✗ rtkSol->nObservations[GnssObs::Doppler] = sppSol->nMeasDopp;
1173
1174 ✗ if (sppSol->e_CovarianceMatrix().has_value())
1175 {
1176 ✗ if (sppSol->e_CovarianceMatrix()->get().hasAnyRows(Keys::Vel<Keys::MotionModelKey>))
1177 {
1178 ✗ rtkSol->setPosVelAndCov_e(sppSol->e_position(), sppSol->e_velocity(),
1179 ✗ (*sppSol->e_CovarianceMatrix())(Keys::PosVel<Keys::MotionModelKey>, Keys::PosVel<Keys::MotionModelKey>));
1180 }
1181 else
1182 {
1183 ✗ rtkSol->setPositionAndCov_e(sppSol->e_position(),
1184 ✗ (*sppSol->e_CovarianceMatrix())(Keys::Pos<Keys::MotionModelKey>, Keys::Pos<Keys::MotionModelKey>));
1185 }
1186 }
1187 else
1188 {
1189 ✗ rtkSol->setPosition_e(sppSol->e_position());
1190 ✗ rtkSol->setVelocity_e(sppSol->e_velocity());
1191 }
1192
1193 ✗ assignSolutionToFilter(sppSol);
1194
1195 ✗ return rtkSol;
1196 ✗ }
1197
1198 ✗ return nullptr;
1199 ✗ }
1200
1201 601 void RealTimeKinematic::kalmanFilterPrediction()
1202 {
1203 // Update the State transition matrix (𝚽) and the Process noise covariance matrix (𝐐)
1204
1205
1/2
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
 601 auto dt = static_cast<double>((_receiver[Rover].gnssObs->insTime - _lastUpdate).count());
1206 LOG_DATA("{}: dt = {}s", nameId(), dt);
1207
1208
6/12
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 601 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 601 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 601 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 601 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 601 times.
✗ Branch 21 not taken.
 601 _kalmanFilter.G.block<3>(States::Vel, States::Vel) = trafo::e_Quat_n(_receiver[Rover].lla_posMarker(0), _receiver[Rover].lla_posMarker(1)).toRotationMatrix();
1209
1210 LOG_DATA("{}: F =\n{}", nameId(), _kalmanFilter.F);
1211 LOG_DATA("{}: G =\n{}", nameId(), _kalmanFilter.G);
1212 LOG_DATA("{}: W =\n{}", nameId(), _kalmanFilter.W);
1213 LOG_DATA("{}: GWG^T =\n{}", nameId(),
1214 KeyedMatrixXd<States::StateKeyType>(_kalmanFilter.G(all, all)
1215 * _kalmanFilter.W(all, all)
1216 * _kalmanFilter.G(all, all).transpose(),
1217 _kalmanFilter.G.rowKeys()));
1218
1219
1/2
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
 1202 auto [Phi, Q] = calcPhiAndQWithVanLoanMethod(_kalmanFilter.F.block<6>(States::PosVel, States::PosVel),
1220
1/2
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
 601 _kalmanFilter.G.block<6>(States::PosVel, States::PosVel),
1221
1/2
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
 1202 _kalmanFilter.W.block<6>(States::PosVel, States::PosVel),
1222
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 dt);
1223
2/4
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 601 times.
✗ Branch 7 not taken.
 601 _kalmanFilter.Phi.block<6>(States::PosVel, States::PosVel) = Phi;
1224
2/4
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 601 times.
✗ Branch 7 not taken.
 601 _kalmanFilter.Q.block<6>(States::PosVel, States::PosVel) = Q;
1225
1226
2/2
✓ Branch 0 taken 600 times.
✓ Branch 1 taken 1 times.
 601 auto ambNoiseVar = _lastSolutionStatus == RtkSolution::SolutionType::RTK_Fixed ? _ambiguityFixProcessNoiseVariance : _ambiguityFloatProcessNoiseVariance;
1227
2/2
✓ Branch 2 taken 23538 times.
✓ Branch 3 taken 601 times.
 24139 for (size_t i = States::KFStates_COUNT; i < _kalmanFilter.x.rowKeys().size(); i++) // 0-2 Pos, 3-5 Vel
1228 {
1229
2/4
✓ Branch 2 taken 23538 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23538 times.
✗ Branch 6 not taken.
 23538 if (const auto* ambDD = std::get_if<States::AmbiguityDD>(&_kalmanFilter.x.rowKeys().at(i)))
1230 {
1231 23538 const auto& key = States::AmbiguityDD(ambDD->satSigId);
1232
1233
1/2
✓ Branch 3 taken 23538 times.
✗ Branch 4 not taken.
 23538 _kalmanFilter.W(key, key) = ambNoiseVar;
1234
1/2
✓ Branch 3 taken 23538 times.
✗ Branch 4 not taken.
 23538 _kalmanFilter.Q(key, key) = ambNoiseVar * dt;
1235 }
1236 }
1237
1238 LOG_DATA("{}: Phi =\n{}", nameId(), _kalmanFilter.Phi);
1239 LOG_DATA("{}: Q =\n{}", nameId(), _kalmanFilter.Q);
1240
1241 LOG_DATA("{}: P (a posteriori, t-1 = {}) =\n{}", nameId(), _lastUpdate, _kalmanFilter.P);
1242 LOG_DATA("{}: x (a posteriori, t-1 = {}) =\n{}", nameId(), _lastUpdate, _kalmanFilter.x.transposed());
1243
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 _kalmanFilter.predict();
1244 LOG_DATA("{}: Doing KF prediction", nameId());
1245 LOG_DATA("{}: x (a priori , t = {}) =\n{}", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), _kalmanFilter.x.transposed());
1246 LOG_DATA("{}: P (a priori , t = {}) =\n{}", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), _kalmanFilter.P);
1247
2/4
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 601 times.
✗ Branch 7 not taken.
 601 _receiver[Rover].e_posMarker = _kalmanFilter.x.segment<3>(States::Pos);
1248
2/4
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 601 times.
✗ Branch 7 not taken.
 601 _receiver[Rover].e_vel = _kalmanFilter.x.segment<3>(States::Vel);
1249
1/2
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
 601 _receiver[Rover].lla_posMarker = trafo::ecef2lla_WGS84(_receiver[Rover].e_posMarker);
1250 601 }
1251
1252 601 void RealTimeKinematic::checkForCycleSlip(Observations& observations, const std::shared_ptr<RtkSolution>& rtkSol)
1253 {
1254
2/4
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 601 times.
 601 if (std::ranges::none_of(_cycleSlipDetector, [](const CycleSlipDetector& detector) {
1255
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 return detector.isEnabled(CycleSlipDetector::Detector::LLI)
1256 ✗ || detector.isEnabled(CycleSlipDetector::Detector::SingleFrequency)
1257
1/6
✗ Branch 0 not taken.
✓ Branch 1 taken 601 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 601 || detector.isEnabled(CycleSlipDetector::Detector::DualFrequency);
1258 }))
1259 {
1260 ✗ return;
1261 }
1262 LOG_DATA("{}: Checking for cycle-slips", nameId());
1263
1264 601 std::array<std::vector<CycleSlipDetector::SatelliteObservation>, ReceiverType_COUNT> cycleSlipObservations;
1265
1266
2/2
✓ Branch 5 taken 8060 times.
✓ Branch 6 taken 601 times.
 8661 for (const auto& satId : observations.satellites)
1267 {
1268 8060 bool added = false;
1269
1/2
✓ Branch 7 taken 151712 times.
✗ Branch 8 not taken.
 151712 for (const auto& [satSigId, signalObs] : observations.signals) // Signals
1270 {
1271
3/4
✓ Branch 1 taken 151712 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 143652 times.
✓ Branch 5 taken 8060 times.
 151712 if (satSigId.toSatId() != satId) { continue; }
1272
1273
2/2
✓ Branch 0 taken 16120 times.
✓ Branch 1 taken 8060 times.
 24180 for (size_t r = 0; r < ReceiverType_COUNT; r++) // ReceiverType
1274 {
1275 16120 auto recvType = static_cast<ReceiverType>(r);
1276
1277
3/4
✓ Branch 0 taken 8060 times.
✓ Branch 1 taken 8060 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8060 times.
 16120 if (recvType == Base && !_baseObsReceivedThisEpoch) { continue; } // Process base observation only in epoch where received
1278
1279
2/4
✓ Branch 1 taken 16120 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16120 times.
✗ Branch 5 not taken.
 16120 if (auto detector = _cycleSlipDetector.at(recvType);
1280
1/2
✓ Branch 1 taken 16120 times.
✗ Branch 2 not taken.
 16120 !detector.isEnabled(CycleSlipDetector::Detector::LLI)
1281 ✗ && !detector.isEnabled(CycleSlipDetector::Detector::SingleFrequency)
1282
2/8
✗ Branch 0 not taken.
✓ Branch 1 taken 16120 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 16120 times.
 16120 && !detector.isEnabled(CycleSlipDetector::Detector::DualFrequency))
1283 {
1284 ✗ continue;
1285 16120 }
1286
1287
3/4
✓ Branch 1 taken 16120 times.
✗ Branch 2 not taken.
✓ Branch 9 taken 1096160 times.
✓ Branch 10 taken 16120 times.
 1112280 for (const auto& receiverObs : _receiver.at(recvType).gnssObs->data) // ObservationType
1288 {
1289
6/8
✓ Branch 1 taken 1096160 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 55572 times.
✓ Branch 5 taken 1040588 times.
✓ Branch 7 taken 55572 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 55572 times.
✓ Branch 10 taken 1040588 times.
 1096160 if (receiverObs.satSigId.toSatId() == satId && receiverObs.carrierPhase)
1290 {
1291
2/4
✓ Branch 1 taken 55572 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 55572 times.
✗ Branch 6 not taken.
 55572 [[maybe_unused]] auto lambda = InsConst::C / receiverObs.satSigId.freq().getFrequency(signalObs.freqNum());
1292 LOG_DATA("[{}] [{}] Added to cycle-slip detection: {:.1f} [m]", receiverObs.satSigId, recvType, receiverObs.carrierPhase->value * lambda);
1293
1294 55572 GnssObs::ObservationData::CarrierPhase carrier{ .value = receiverObs.carrierPhase->value, .LLI = receiverObs.carrierPhase->LLI };
1295 55572 auto signal = CycleSlipDetector::SatelliteObservation::Signal{ .code = receiverObs.satSigId.code, .measurement = carrier };
1296
1297
2/4
✓ Branch 1 taken 55572 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 55572 times.
✗ Branch 5 not taken.
 55572 auto sat = std::ranges::find_if(cycleSlipObservations.at(recvType),
1298 407950 [&satId](const auto& satObs) { return satObs.satId == satId; });
1299
1300
3/4
✓ Branch 1 taken 55572 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 16120 times.
✓ Branch 6 taken 39452 times.
 55572 if (sat == cycleSlipObservations.at(recvType).end())
1301 {
1302 CycleSlipDetector::SatelliteObservation satObs = { .satId = satId,
1303 .signals = { signal },
1304
1/2
✓ Branch 1 taken 16120 times.
✗ Branch 2 not taken.
 32240 .freqNum = signalObs.freqNum() };
1305
2/4
✓ Branch 1 taken 16120 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16120 times.
✗ Branch 5 not taken.
 16120 cycleSlipObservations.at(recvType).push_back(satObs);
1306 16120 }
1307 else
1308 {
1309
1/2
✓ Branch 2 taken 39452 times.
✗ Branch 3 not taken.
 39452 sat->signals.push_back(signal);
1310 }
1311 55572 added = true;
1312 }
1313 }
1314 }
1315
1/2
✓ Branch 0 taken 8060 times.
✗ Branch 1 not taken.
 8060 if (added) { break; }
1316 }
1317 }
1318
1319 601 std::vector<NAV::Code> pivotsToChange;
1320 601 std::vector<States::AmbiguityDD> removedStates;
1321 ✗ auto removeSatSig = [&](const SatSigId& satSigId, [[maybe_unused]] const std::string& reason) -> bool {
1322 ✗ auto key = States::AmbiguityDD(satSigId);
1323 ✗ if (_kalmanFilter.x.hasRow(key))
1324 {
1325 LOG_TRACE("{}: [{}] Cycle-slip detected in [{}], due to {}. Removing state: {}", nameId(),
1326 _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), satSigId, reason, key);
1327 ✗ _ambiguitiesHold.erase(satSigId);
1328 ✗ _kalmanFilter.removeState(key);
1329 ✗ if (_outputStateEvents) { removedStates.push_back(key); }
1330 ✗ for (auto& detector : _cycleSlipDetector) { detector.resetSignal(key.satSigId); }
1331 }
1332 ✗ else if (_pivotSatellites.contains({ satSigId.code, GnssObs::Carrier }) && _pivotSatellites.at({ satSigId.code, GnssObs::Carrier }).satSigId == satSigId)
1333 {
1334 LOG_TRACE("{}: [{}] Cycle-slip detected in pivot satellite [{}], due to {}", nameId(),
1335 _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), satSigId, reason);
1336 ✗ pivotsToChange.push_back(satSigId.code);
1337 }
1338 else
1339 {
1340 LOG_DATA("{}: [{}] Cycle-slip detected in [{}], due to {}. Not yet estimated (no action taken).", nameId(),
1341 _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), satSigId, reason);
1342 ✗ return false;
1343 }
1344 ✗ return true;
1345 601 };
1346
1347
2/2
✓ Branch 0 taken 1202 times.
✓ Branch 1 taken 601 times.
 3606 for (size_t i = 0; i < cycleSlipObservations.size(); ++i)
1348 {
1349 1202 std::string receiverName = fmt::format("{}", static_cast<ReceiverType>(i));
1350 LOG_DATA("{}: [{}][{}] Checking for cycle-slips", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), receiverName);
1351
1352
5/10
✓ Branch 1 taken 1202 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1202 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1202 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1202 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1202 times.
✗ Branch 15 not taken.
 1202 auto cycleSlips = _cycleSlipDetector.at(i).checkForCycleSlip(_receiver.at(i).gnssObs->insTime, cycleSlipObservations.at(i), nameId());
1353
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 1202 times.
 1202 for (const auto& cycleSlip : cycleSlips)
1354 {
1355 ✗ if (const auto* s = std::get_if<CycleSlipDetector::CycleSlipLossOfLockIndicator>(&cycleSlip))
1356 {
1357 ✗ if (s->signal.code & _obsFilter.getCodeFilter()
1358 ✗ && removeSatSig(s->signal, "LLI set in " + receiverName))
1359 {
1360 ✗ rtkSol->cycleSlipDetectorResult.emplace_back(cycleSlip, receiverName);
1361 }
1362 }
1363 ✗ else if (const auto* s = std::get_if<CycleSlipDetector::CycleSlipSingleFrequency>(&cycleSlip))
1364 {
1365 ✗ if (s->signal.code & _obsFilter.getCodeFilter()
1366 ✗ && removeSatSig(s->signal, "single frequency check in " + receiverName))
1367 {
1368 ✗ rtkSol->cycleSlipDetectorResult.emplace_back(cycleSlip, receiverName);
1369 }
1370 }
1371 ✗ else if (const auto* s = std::get_if<CycleSlipDetector::CycleSlipDualFrequency>(&cycleSlip))
1372 {
1373 ✗ bool first = true;
1374 ✗ for (const auto& signal : s->signals)
1375 {
1376 ✗ if (signal.code & _obsFilter.getCodeFilter()
1377 ✗ && removeSatSig(signal, "dual frequency check in " + receiverName)
1378 ✗ && first)
1379 {
1380 ✗ rtkSol->cycleSlipDetectorResult.emplace_back(cycleSlip, receiverName);
1381 ✗ first = false;
1382 }
1383 }
1384 }
1385 }
1386 1202 }
1387
1388
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 601 times.
 601 for (const auto& code : pivotsToChange)
1389 {
1390 LOG_TRACE("{}: [{}] Changing pivot because of cycle-slip", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), code);
1391 ✗ updatePivotSatellite(code, GnssObs::Carrier, observations, rtkSol, RtkSolution::PivotChange::Reason::PivotCycleSlip);
1392 }
1393
1394
1/2
✗ Branch 6 not taken.
✓ Branch 7 taken 601 times.
 601 for (const auto& cycleSlip : rtkSol->cycleSlipDetectorResult)
1395 {
1396 ✗ switch (cycleSlip.first.index())
1397 {
1398 ✗ case 0: // CycleSlipLossOfLockIndicator
1399 ✗ _nCycleSlipsLLI++;
1400 ✗ break;
1401 ✗ case 1: // CycleSlipDualFrequency
1402 ✗ _nCycleSlipsDual++;
1403 ✗ break;
1404 ✗ case 2: // CycleSlipSingleFrequency
1405 ✗ _nCycleSlipsSingle++;
1406 ✗ break;
1407 ✗ default:
1408 ✗ break;
1409 }
1410 ✗ auto text = fmt::format("{}: {}", cycleSlip.second, cycleSlip.first);
1411 LOG_DATA("{}: {}", nameId(), text);
1412 ✗ addEventToGui(rtkSol, text);
1413 ✗ }
1414
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 601 times.
 601 for (const auto& state : removedStates)
1415 {
1416 ✗ auto text = fmt::format("State [{}] removed, because cycle-slip detected.", state);
1417 LOG_DATA("{}: {}", nameId(), text);
1418 ✗ addEventToGui(rtkSol, text);
1419 ✗ }
1420 601 }
1421
1422 1203 void RealTimeKinematic::removeSingleObservations(Observations& observations, ObservationFilter::Filtered* filtered, const std::shared_ptr<RtkSolution>& rtkSol)
1423 {
1424 LOG_DATA("{}: Checking for observations to remove when only one observable per code for double difference", nameId());
1425
1426 1203 std::unordered_map<std::pair<Code, GnssObs::ObservationType>, uint16_t> signalCount;
1427
2/2
✓ Branch 7 taken 68699 times.
✓ Branch 8 taken 1203 times.
 69902 for (const auto& [satSigId, sigObs] : observations.signals)
1428 {
1429
1/2
✓ Branch 1 taken 68699 times.
✗ Branch 2 not taken.
 68699 const auto& recvObs = sigObs.recvObs.at(Base);
1430
2/2
✓ Branch 5 taken 206097 times.
✓ Branch 6 taken 68699 times.
 274796 for (const auto& obs : recvObs->obs)
1431 {
1432
1/2
✓ Branch 2 taken 206097 times.
✗ Branch 3 not taken.
 206097 signalCount[std::make_pair(satSigId.code, obs.first)]++;
1433 }
1434 }
1435
1436 1203 std::vector<SatSigId> sigToRemove;
1437
2/2
✓ Branch 7 taken 68699 times.
✓ Branch 8 taken 1203 times.
 69902 for (auto& [satSigId, sigObs] : observations.signals)
1438 {
1439
2/2
✓ Branch 5 taken 137398 times.
✓ Branch 6 taken 68699 times.
 206097 for (auto& recvObs : sigObs.recvObs)
1440 {
1441 137398 std::vector<GnssObs::ObservationType> obsToRemove;
1442
2/2
✓ Branch 6 taken 412194 times.
✓ Branch 7 taken 137398 times.
 549592 for (const auto& obs : recvObs.second->obs)
1443 {
1444
2/4
✓ Branch 2 taken 412194 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 412194 times.
 412194 if (signalCount[std::make_pair(satSigId.code, obs.first)] < 2)
1445 {
1446 ✗ obsToRemove.push_back(obs.first);
1447 }
1448 }
1449
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 137398 times.
 137398 for (const GnssObs::ObservationType& obs : obsToRemove)
1450 {
1451 LOG_DATA("{}: Removing [{}] {}", nameId(), satSigId, obs);
1452 ✗ recvObs.second->obs.erase(obs);
1453 ✗ if (filtered) { filtered->singleObservation.push_back(satSigId); }
1454 }
1455
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 137398 times.
 137398 if (recvObs.second->obs.empty())
1456 {
1457 ✗ sigToRemove.push_back(satSigId);
1458 ✗ break;
1459 }
1460 137398 }
1461 }
1462
1463
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 1203 times.
 1203 for (const auto& satSigId : sigToRemove)
1464 {
1465 LOG_DATA("{}: Removing [{}], because no more observations left for this signal", nameId(), satSigId);
1466
1467 ✗ if (filtered) // Only update pivot if called with filtered (means we modifying the actual observation data)
1468 {
1469 ✗ for (const auto& obsType : _obsFilter.getUsedObservationTypes())
1470 {
1471 ✗ auto key = std::make_pair(satSigId.code, obsType);
1472 ✗ if (_pivotSatellites.contains(key) && signalCount[key] < 2)
1473 {
1474 LOG_DATA("{}: Pivot [{}][{}] does not have enough signals anymore", nameId(), satSigId.code, obsType);
1475 ✗ updatePivotSatellite(satSigId.code, obsType, observations, rtkSol, RtkSolution::PivotChange::Reason::PivotNotObservedInEpoch);
1476 }
1477 }
1478 }
1479
1480 ✗ observations.removeSignal(satSigId, nameId());
1481 }
1482 1203 }
1483
1484 12621 std::optional<RtkSolution::PivotChange> RealTimeKinematic::updatePivotSatellite(Code code, GnssObs::ObservationType obsType,
1485 Observations& observations, const std::shared_ptr<RtkSolution>& rtkSol,
1486 const RtkSolution::PivotChange::Reason& reason)
1487 {
1488 using Reason = RtkSolution::PivotChange::Reason;
1489 12621 std::optional<RtkSolution::PivotChange> pivotChange;
1490
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 12621 times.
 12621 if (reason != Reason::None)
1491 {
1492 ✗ pivotChange = RtkSolution::PivotChange{};
1493 ✗ pivotChange->reason = reason;
1494 ✗ pivotChange->obsType = obsType;
1495
1496 ✗ if (_pivotSatellites.contains({ code, obsType }))
1497 {
1498 ✗ const auto& pivot = _pivotSatellites.at({ code, obsType });
1499 ✗ if (observations.signals.contains(pivot.satSigId))
1500 {
1501 ✗ auto oldPivotObs = observations.signals.at(pivot.satSigId).recvObs.begin();
1502 ✗ pivotChange->oldPivotSat = pivot.satSigId;
1503 ✗ pivotChange->oldPivotElevation = oldPivotObs->second->satElevation(_receiver[Rover].e_posMarker, _receiver[Rover].lla_posMarker);
1504 }
1505 ✗ _pivotSatellites.erase({ code, obsType });
1506 }
1507 }
1508
1509 316566 auto sumRecvElevation = [&](double el, const auto& recvData) {
1510 316566 return el + recvData.second->satElevation(_receiver[Rover].e_posMarker, _receiver[Rover].lla_posMarker);
1511 12621 };
1512
1513
2/2
✓ Branch 7 taken 583506 times.
✓ Branch 8 taken 12621 times.
 596127 for (const auto& [satSigId, observation] : observations.signals)
1514 {
1515
3/4
✓ Branch 1 taken 583506 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 500148 times.
✓ Branch 4 taken 83358 times.
 583506 if (satSigId.code != code) { continue; }
1516
1517
1/2
✓ Branch 1 taken 83358 times.
✗ Branch 2 not taken.
 83358 bool anySignalOnThisCodeWasEstimatedYet = std::ranges::any_of(observations.signals,
1518
1/2
✓ Branch 1 taken 903048 times.
✗ Branch 2 not taken.
 903048 [&](const auto& obs) { return obs.first.code == code
1519
5/6
✓ Branch 0 taken 84096 times.
✓ Branch 1 taken 818952 times.
✓ Branch 5 taken 84096 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 83223 times.
✓ Branch 8 taken 873 times.
 903048 && _kalmanFilter.x.hasRow(States::AmbiguityDD(obs.first)); });
1520
1/2
✓ Branch 1 taken 83358 times.
✗ Branch 2 not taken.
 83358 bool anySignalOnThisCodeWithFix = std::ranges::any_of(_ambiguitiesHold,
1521 537807 [&code](const auto& amb) { return amb.first.code == code; });
1522
1523 LOG_DATA("{}: [{}] Checking [{}] for new pivot and obsType [{}]",
1524 nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), satSigId, obsType);
1525
1526 83358 size_t nFound = 0;
1527
2/2
✓ Branch 5 taken 166716 times.
✓ Branch 6 taken 83358 times.
 250074 for (const auto& recvObs : observation.recvObs)
1528 {
1529
2/4
✓ Branch 2 taken 166716 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 166716 times.
✗ Branch 5 not taken.
 166716 if (recvObs.second->obs.contains(obsType)) { nFound++; }
1530 }
1531
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 83358 times.
 83358 if (nFound != observation.recvObs.size()) { continue; }
1532
1533 #if LOG_LEVEL <= LOG_LEVEL_DATA
1534 if (obsType == GnssObs::Carrier)
1535 {
1536 std::string ambHold;
1537 ambHold.reserve(_ambiguitiesHold.size() * 17);
1538 for (const auto& amb : _ambiguitiesHold)
1539 {
1540 ambHold += fmt::format("[{} = {}]", amb.first, amb.second);
1541 }
1542 LOG_DATA("{}: [{}] anySignalOnThisCodeWasEstimatedYet = {}, anySignalOnThisCodeWithFix = {}, ambHold = {}",
1543 nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), anySignalOnThisCodeWasEstimatedYet, anySignalOnThisCodeWithFix, ambHold);
1544 }
1545 #endif
1546
1547 // Don't allow same satellite to be pivot again
1548
2/4
✓ Branch 2 taken 83358 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 83358 times.
 83358 if (pivotChange->oldPivotSat == satSigId) { continue; }
1549
1550 83358 if (_kalmanFilter.x.rows() != States::KFStates_COUNT // Ignore first epoch, as we need to select a pivot
1551
2/2
✓ Branch 0 taken 27741 times.
✓ Branch 1 taken 55482 times.
 83223 && obsType == GnssObs::Carrier // Ambiguity do not matter for pseudorange or doppler
1552
1/6
✗ Branch 0 not taken.
✓ Branch 1 taken 27741 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 27741 && (anySignalOnThisCodeWasEstimatedYet || _pivotSatellites.contains({ code, GnssObs::Carrier })) // We have at least one satellite with an estimated ambiguity on this code
1553
3/4
✓ Branch 3 taken 27741 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 4206 times.
✓ Branch 6 taken 23535 times.
 27741 && !_kalmanFilter.x.hasRow(States::AmbiguityDD(satSigId)) // Only consider signals which were estimated once (float solution)
1554
1/2
✓ Branch 0 taken 4206 times.
✗ Branch 1 not taken.
 4206 && _ambiguityResolutionParameters.searchAlgorithm != AmbiguityResolutionParameters::SearchAlgorithm::None // If we estimating ambiguities
1555
1/6
✗ Branch 0 not taken.
✓ Branch 1 taken 4206 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 4206 && (anySignalOnThisCodeWithFix || _pivotSatellites.contains({ code, GnssObs::Carrier })) // We have at least one satellite with a fix on this code
1556
6/8
✓ Branch 0 taken 83223 times.
✓ Branch 1 taken 135 times.
✓ Branch 3 taken 4206 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 4206 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 4206 times.
✓ Branch 8 taken 79152 times.
 166581 && !_ambiguitiesHold.contains(satSigId)) // Only consider signals, which have a fix
1557 {
1558 4206 continue;
1559 }
1560
1561
1/2
✓ Branch 3 taken 79152 times.
✗ Branch 4 not taken.
 79152 double satElevation = std::accumulate(observation.recvObs.begin(), observation.recvObs.end(), 0.0, sumRecvElevation) // NOLINT(boost-use-ranges,modernize-use-ranges) // There is no ranges::accumulate
1562 79152 / static_cast<double>(observation.recvObs.size());
1563
1564
3/4
✓ Branch 2 taken 79152 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 21 times.
✓ Branch 5 taken 79131 times.
 79152 if (!_pivotSatellites.contains({ code, obsType })) // No pivot for this code yet
1565 {
1566
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 21 _pivotSatellites.insert(std::make_pair(std::make_pair(code, obsType), satSigId));
1567
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 if (!pivotChange)
1568 {
1569 LOG_DATA("{}: [{}] Setting [{}] as new pivot for obsType [{}] (elevation {:.4}°, no previous pivot on this code)", nameId(),
1570 _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), satSigId, obsType, rad2deg(satElevation));
1571
3/4
✓ Branch 0 taken 21 times.
✓ Branch 1 taken 21 times.
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 63 pivotChange = RtkSolution::PivotChange{ .reason = Reason::NewCode,
1572 .obsType = obsType,
1573 .oldPivotSat = {},
1574 .oldPivotElevation = 0.0,
1575 .newPivotSat = satSigId,
1576 21 .newPivotElevation = satElevation };
1577 }
1578 else
1579 {
1580 LOG_DATA("{}: [{}] Setting [{}] as new pivot for obsType [{}] (elevation {:.4}°)", nameId(),
1581 _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), satSigId, obsType, rad2deg(satElevation));
1582 ✗ pivotChange->newPivotSat = satSigId;
1583 ✗ pivotChange->newPivotElevation = satElevation;
1584 }
1585 }
1586 else // Check if better pivot available
1587 {
1588
1/2
✓ Branch 2 taken 79131 times.
✗ Branch 3 not taken.
 79131 const auto& pivotSat = _pivotSatellites.at({ code, obsType });
1589
1590
2/4
✓ Branch 1 taken 79131 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 79131 times.
 79131 if (!observations.signals.contains(pivotSat.satSigId)) // Old pivot satellite was not observed this epoch, so we have to choose another one
1591 {
1592 LOG_DATA("{}: [{}] Setting [{}] as new pivot for obsType [{}] (elevation {:.4}°, old pivot [{}] not observed this epoch)", nameId(),
1593 _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), satSigId, obsType, rad2deg(satElevation), _pivotSatellites.at({ code, obsType }).satSigId);
1594 ✗ pivotChange = RtkSolution::PivotChange{ .reason = Reason::PivotNotObservedInEpoch,
1595 .obsType = obsType,
1596 ✗ .oldPivotSat = _pivotSatellites.at({ code, obsType }).satSigId,
1597 .oldPivotElevation = 0.0,
1598 .newPivotSat = satSigId,
1599 ✗ .newPivotElevation = satElevation };
1600 ✗ _pivotSatellites.at({ code, obsType }) = PivotSatellite(satSigId);
1601 ✗ continue;
1602 }
1603
1604
1/2
✓ Branch 1 taken 79131 times.
✗ Branch 2 not taken.
 79131 const auto& pivotObs = observations.signals.at(pivotSat.satSigId);
1605
1/2
✓ Branch 3 taken 79131 times.
✗ Branch 4 not taken.
 79131 double pivElevation = std::accumulate(pivotObs.recvObs.begin(), pivotObs.recvObs.end(), 0.0, sumRecvElevation) // NOLINT(boost-use-ranges,modernize-use-ranges) // There is no ranges::accumulate
1606 79131 / static_cast<double>(pivotObs.recvObs.size());
1607
1608
2/2
✓ Branch 0 taken 36 times.
✓ Branch 1 taken 79095 times.
 79131 if (satElevation > pivElevation)
1609 {
1610 LOG_DATA("{}: [{}] Setting [{}] as new pivot for obsType [{}] (elevation {:.4}°, larger than previous pivot [{}] elevation {:.4}°)", nameId(),
1611 _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), satSigId, obsType, rad2deg(satElevation), _pivotSatellites.at({ code, obsType }).satSigId, rad2deg(pivElevation));
1612
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 36 times.
 36 if (!pivotChange)
1613 {
1614 ✗ pivotChange = RtkSolution::PivotChange{ .reason = Reason::HigherElevationFound,
1615 .obsType = obsType,
1616 ✗ .oldPivotSat = _pivotSatellites.at({ code, obsType }).satSigId,
1617 .oldPivotElevation = pivElevation,
1618 .newPivotSat = satSigId,
1619 ✗ .newPivotElevation = satElevation };
1620 }
1621 else
1622 {
1623 36 pivotChange->newPivotSat = satSigId;
1624 36 pivotChange->newPivotElevation = satElevation;
1625 }
1626
1/2
✓ Branch 3 taken 36 times.
✗ Branch 4 not taken.
 36 _pivotSatellites.at({ code, obsType }) = PivotSatellite(satSigId);
1627 }
1628 }
1629 }
1630
1631 // React on changed pivot satellite
1632
2/2
✓ Branch 1 taken 21 times.
✓ Branch 2 taken 12600 times.
 12621 if (pivotChange)
1633 {
1634
5/10
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 21 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 21 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 21 times.
✗ Branch 15 not taken.
 21 INS_ASSERT_USER_ERROR(!_pivotSatellites.contains({ code, obsType })
1635 || _pivotSatellites.at({ code, obsType }).satSigId == pivotChange->newPivotSat,
1636 "The new pivot satellite and current one must match.");
1637
2/12
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 21 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 21 INS_ASSERT_USER_ERROR(pivotChange->oldPivotSat != pivotChange->newPivotSat
1638 || pivotChange->newPivotSat == SatSigId{},
1639 "Old and new pivot satellite have to be different.");
1640
1641 LOG_TRACE("{}: [{}] {}", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), *pivotChange, obsType);
1642
1643
1/2
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
 21 rtkSol->changedPivotSatellites.emplace(std::make_pair(code, obsType), *pivotChange);
1644 21 _nPivotChange++;
1645
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 42 addEventToGui(rtkSol, fmt::format("{}", *pivotChange));
1646
1647
5/8
✓ Branch 0 taken 21 times.
✓ Branch 1 taken 21 times.
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 21 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 21 times.
 42 if (pivotChange->newPivotSat == SatSigId{}) // No new pivot selected
1648 {
1649 ✗ observations.removeMeasurementsFor(pivotChange->oldPivotSat.code, obsType, nameId());
1650 ✗ if (obsType == GnssObs::Carrier)
1651 {
1652 ✗ for (size_t i = States::KFStates_COUNT; i < _kalmanFilter.x.rowKeys().size(); i++) // 0-2 Pos, 3-5 Vel
1653 {
1654 ✗ const auto& key = _kalmanFilter.x.rowKeys().at(i);
1655 ✗ if (const auto* ambDD = std::get_if<States::AmbiguityDD>(&key))
1656 {
1657 ✗ if (ambDD->satSigId.code != pivotChange->oldPivotSat.code) { continue; }
1658
1659 LOG_DATA("{}: [{}] Removing Amibguity as no pivot anymore {}", nameId(), ambDD->satSigId, key);
1660 ✗ if (_outputStateEvents) { addEventToGui(rtkSol, fmt::format("State [{}] removed (not observed anymore)", key)); }
1661 ✗ _ambiguitiesHold.erase(ambDD->satSigId);
1662 ✗ _kalmanFilter.removeState(key); // After this 'key' is invalidated
1663 ✗ i--;
1664 }
1665 }
1666 }
1667 }
1668
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 21 times.
 21 else if (pivotChange->reason != Reason::NewCode && obsType == GnssObs::Carrier)
1669 {
1670 ✗ updateKalmanFilterAmbiguitiesForPivotChange(pivotChange->newPivotSat, pivotChange->oldPivotSat,
1671 ✗ pivotChange->reason != Reason::PivotNotObservedInEpoch
1672 ✗ && pivotChange->reason != Reason::PivotCycleSlip
1673 ✗ && pivotChange->reason != Reason::PivotOutlier,
1674 rtkSol);
1675 }
1676 }
1677 25242 return pivotChange;
1678 }
1679
1680 602 void RealTimeKinematic::printPivotSatellites()
1681 {
1682 LOG_DATA("{}: Current pivot satellites:", nameId());
1683
2/2
✓ Branch 7 taken 12621 times.
✓ Branch 8 taken 602 times.
 13223 for ([[maybe_unused]] const auto& [key, pivot] : _pivotSatellites)
1684 {
1685 LOG_DATA("{}: [{} - {}]: {}", nameId(), key.first, key.second, pivot.satSigId);
1686
2/2
✓ Branch 0 taken 8414 times.
✓ Branch 1 taken 4207 times.
 12621 if (key.second != GnssObs::Carrier) { continue; }
1687 4207 if (auto state = States::AmbiguityDD{ pivot.satSigId };
1688
2/4
✓ Branch 2 taken 4207 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 4207 times.
 4207 _kalmanFilter.x.hasRow(state))
1689 {
1690 ✗ LOG_ERROR("{}: [{}] The Kalman Filter has the state [{}] but this is a pivot. Pivots should not be in the state.", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), state);
1691 }
1692 }
1693 602 }
1694
1695 601 void RealTimeKinematic::updatePivotSatellites(Observations& observations, const std::shared_ptr<RtkSolution>& rtkSol)
1696 {
1697
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 printPivotSatellites();
1698
1699 601 std::set<Code> codes;
1700
2/2
✓ Branch 7 taken 27786 times.
✓ Branch 8 taken 601 times.
 28387 for (const auto& [satSigId, observation] : observations.signals)
1701 {
1702
1/2
✓ Branch 1 taken 27786 times.
✗ Branch 2 not taken.
 27786 codes.insert(satSigId.code);
1703 }
1704 601 bool anyPivotChanged = false;
1705
2/2
✓ Branch 5 taken 4207 times.
✓ Branch 6 taken 601 times.
 4808 for (const auto& code : codes)
1706 {
1707
2/2
✓ Branch 5 taken 12621 times.
✓ Branch 6 taken 4207 times.
 16828 for (const auto& obsType : _obsFilter.getUsedObservationTypes())
1708 {
1709 12621 auto reason = RtkSolution::PivotChange::Reason::None;
1710
1711
3/4
✓ Branch 2 taken 12621 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 12600 times.
✓ Branch 5 taken 21 times.
 12621 if (_pivotSatellites.contains({ code, obsType }))
1712 {
1713
1/2
✓ Branch 2 taken 12600 times.
✗ Branch 3 not taken.
 12600 const auto& pivotSat = _pivotSatellites.at({ code, obsType });
1714 12600 const auto& pivotSatSigId = pivotSat.satSigId;
1715
1/2
✓ Branch 1 taken 12600 times.
✗ Branch 2 not taken.
 12600 auto obs = std::ranges::find_if(observations.signals, [&](const auto& obs) {
1716 382533 size_t nFound = 0;
1717
2/2
✓ Branch 5 taken 765066 times.
✓ Branch 6 taken 382533 times.
 1147599 for (const auto& recvObs : obs.second.recvObs)
1718 {
1719
2/4
✓ Branch 2 taken 765066 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 765066 times.
✗ Branch 5 not taken.
 765066 if (recvObs.second->obs.contains(obsType)) { nFound++; }
1720 }
1721
3/4
✓ Branch 1 taken 12600 times.
✓ Branch 2 taken 369933 times.
✓ Branch 4 taken 12600 times.
✗ Branch 5 not taken.
 382533 return obs.first == pivotSatSigId && nFound == obs.second.recvObs.size();
1722 });
1723
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 12600 times.
 12600 if (obs == observations.signals.end())
1724 {
1725 LOG_DATA("{}: [{}] Removing pivot satellite [{}] on observable [{}] because not observed this epoch.", nameId(),
1726 _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), pivotSat.satSigId, obsType);
1727 ✗ reason = RtkSolution::PivotChange::Reason::PivotNotObservedInEpoch;
1728 }
1729 }
1730
1731
1/2
✓ Branch 1 taken 12621 times.
✗ Branch 2 not taken.
 12621 anyPivotChanged |= updatePivotSatellite(code, obsType, observations, rtkSol, reason).has_value();
1732 }
1733 }
1734
1735 LOG_DATA("{}: {}", nameId(), anyPivotChanged ? "Pivot satellite changed" : "No pivot satellite change");
1736
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 600 times.
 601 if (anyPivotChanged)
1737 {
1738
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 removeSingleObservations(observations, &rtkSol->filtered, rtkSol);
1739
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 printPivotSatellites();
1740 }
1741 601 }
1742
1743 ✗ void RealTimeKinematic::updateKalmanFilterAmbiguitiesForPivotChange(const SatSigId& newPivotSatSigId, const SatSigId& oldPivotSatSigId,
1744 bool oldPivotObservedInEpoch, const std::shared_ptr<RtkSolution>& rtkSol)
1745 {
1746 ✗ auto newPivotKey = States::AmbiguityDD{ newPivotSatSigId };
1747 ✗ if (_kalmanFilter.x.hasRow(newPivotKey))
1748 {
1749 ✗ std::vector<States::StateKeyType> ambiguitiesToChange;
1750 ✗ for (size_t i = States::KFStates_COUNT; i < _kalmanFilter.x.rowKeys().size(); i++)
1751 {
1752 ✗ const auto* ambDD = std::get_if<States::AmbiguityDD>(&_kalmanFilter.x.rowKeys().at(i));
1753 ✗ if (ambDD && newPivotSatSigId.code == ambDD->satSigId.code
1754 ✗ && (oldPivotObservedInEpoch || newPivotSatSigId != ambDD->satSigId))
1755 {
1756 ✗ ambiguitiesToChange.emplace_back(*ambDD);
1757 }
1758 }
1759 LOG_TRACE("{}: [{}] New pivot [{}] adapts ambiguities [{}]", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST),
1760 newPivotSatSigId, fmt::join(ambiguitiesToChange, ", "));
1761
1762 ✗ auto nStates = static_cast<int>(_kalmanFilter.x.rowKeys().size());
1763
1764 ✗ KeyedMatrixXd<States::StateKeyType> D(Eigen::MatrixXd::Identity(nStates, nStates),
1765 ✗ _kalmanFilter.x.rowKeys(), _kalmanFilter.x.rowKeys());
1766 ✗ D(ambiguitiesToChange, newPivotKey).setConstant(-1.0);
1767 ✗ if (!oldPivotObservedInEpoch) { D(newPivotKey, newPivotKey) = 0.0; }
1768 LOG_DATA("{}: D = \n{}", nameId(), D);
1769
1770 LOG_DATA("{}: x_old = \n{}", nameId(), _kalmanFilter.x.transposed());
1771 ✗ _kalmanFilter.x(all) = D(all, all) * _kalmanFilter.x(all);
1772 LOG_DATA("{}: D * x_old = \n{}", nameId(), _kalmanFilter.x.transposed());
1773
1774 LOG_DATA("{}: P_old = \n{}", nameId(), _kalmanFilter.P);
1775 // Error propagation
1776 ✗ _kalmanFilter.P(all, all) = D(all, all) * _kalmanFilter.P(all, all) * D(all, all).transpose();
1777
1778 #if LOG_LEVEL <= LOG_LEVEL_DATA
1779 {
1780 std::string ambHold;
1781 ambHold.reserve(_ambiguitiesHold.size() * 17);
1782 for (const auto& amb : ambiguitiesToChange)
1783 {
1784 if (const auto* ambDD = std::get_if<States::AmbiguityDD>(&amb))
1785 {
1786 if (_ambiguitiesHold.contains(ambDD->satSigId))
1787 {
1788 ambHold += fmt::format("[{} = {}]", ambDD->satSigId, _ambiguitiesHold.at(ambDD->satSigId));
1789 }
1790 }
1791 }
1792 LOG_DATA("{}: ambHold (pre ) = {}", nameId(), ambHold);
1793 }
1794 #endif
1795
1796 ✗ for (const auto& amb : ambiguitiesToChange)
1797 {
1798 ✗ if (const auto* ambDD = std::get_if<States::AmbiguityDD>(&amb))
1799 {
1800 ✗ if (_ambiguitiesHold.contains(ambDD->satSigId))
1801 {
1802 ✗ _ambiguitiesHold.at(ambDD->satSigId) = _kalmanFilter.x(amb);
1803 }
1804 }
1805 }
1806
1807 ✗ if (oldPivotObservedInEpoch)
1808 {
1809 ✗ auto oldPivotKey = States::AmbiguityDD{ oldPivotSatSigId };
1810 ✗ if (_ambiguityResolutionParameters.searchAlgorithm != AmbiguityResolutionParameters::SearchAlgorithm::None
1811 ✗ && _ambiguitiesHold.contains(newPivotSatSigId))
1812 {
1813 ✗ _ambiguitiesHold[oldPivotSatSigId] = _ambiguitiesHold.at(newPivotSatSigId);
1814 ✗ _ambiguitiesHold.erase(newPivotSatSigId);
1815 }
1816 ✗ _kalmanFilter.replaceState(newPivotKey, oldPivotKey);
1817 ✗ if (_outputStateEvents)
1818 {
1819 ✗ addEventToGui(rtkSol, fmt::format("State [{}] replaced with [{}] (because [{}] is new pivot satellite)", newPivotKey, oldPivotKey, newPivotSatSigId));
1820 }
1821 LOG_TRACE("{}: [{}] State [{}] replaced with [{}] (because [{}] is new pivot satellite)", nameId(),
1822 _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), newPivotKey, oldPivotKey, newPivotSatSigId);
1823 }
1824 else
1825 {
1826 ✗ _ambiguitiesHold.erase(newPivotSatSigId);
1827 ✗ _kalmanFilter.removeState(newPivotKey);
1828 ✗ for (auto& detector : _cycleSlipDetector)
1829 {
1830 ✗ detector.resetSignal(newPivotSatSigId);
1831 ✗ detector.resetSignal(oldPivotSatSigId);
1832 }
1833 ✗ if (_outputStateEvents)
1834 {
1835 ✗ addEventToGui(rtkSol, fmt::format("State [{}] removed (because new pivot satellite)", newPivotKey));
1836 }
1837 LOG_TRACE("{}: [{}] State [{}] removed (because new pivot satellite)", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), newPivotKey);
1838 }
1839 LOG_DATA("{}: x_new = \n{}", nameId(), _kalmanFilter.x.transposed());
1840 LOG_DATA("{}: P_new = \n{}", nameId(), _kalmanFilter.P);
1841
1842 #if LOG_LEVEL <= LOG_LEVEL_DATA
1843 {
1844 std::erase_if(ambiguitiesToChange, [&newPivotSatSigId](const auto& amb) {
1845 if (const auto* ambDD = std::get_if<States::AmbiguityDD>(&amb))
1846 {
1847 return ambDD->satSigId == newPivotSatSigId;
1848 }
1849 return false;
1850 });
1851 if (oldPivotObservedInEpoch)
1852 {
1853 ambiguitiesToChange.emplace_back(States::AmbiguityDD{ oldPivotSatSigId });
1854 }
1855
1856 std::string ambHold;
1857 ambHold.reserve(_ambiguitiesHold.size() * 17);
1858 for (const auto& amb : ambiguitiesToChange)
1859 {
1860 if (const auto* ambDD = std::get_if<States::AmbiguityDD>(&amb))
1861 {
1862 if (_ambiguitiesHold.contains(ambDD->satSigId))
1863 {
1864 ambHold += fmt::format("[{} = {}]", ambDD->satSigId, _ambiguitiesHold.at(ambDD->satSigId));
1865 }
1866 }
1867 }
1868 LOG_DATA("{}: ambHold (post) = {}", nameId(), ambHold);
1869 }
1870 #endif
1871 ✗ }
1872 ✗ }
1873
1874 601 void RealTimeKinematic::addOrRemoveKalmanFilterAmbiguities(const Observations& observations, const std::shared_ptr<RtkSolution>& rtkSol)
1875 {
1876 601 bool anyStateChanged = false;
1877
1878 47 auto addEvent = [&](const std::string& text) {
1879
1/2
✓ Branch 0 taken 47 times.
✗ Branch 1 not taken.
 47 if (_outputStateEvents) { addEventToGui(rtkSol, text); }
1880 648 };
1881
1882 601 std::vector<States::StateKeyType> newAddedAmbiguities;
1883
1884
2/2
✓ Branch 7 taken 27786 times.
✓ Branch 8 taken 601 times.
 28387 for (const auto& [satSigId, observation] : observations.signals)
1885 {
1886
2/4
✓ Branch 1 taken 27786 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27786 times.
✗ Branch 6 not taken.
 27786 if (!observation.recvObs.at(Rover)->obs.contains(GnssObs::Carrier)
1887
5/10
✓ Branch 0 taken 27786 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 27786 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 27786 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 27786 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 27786 times.
 27786 || !observation.recvObs.at(Base)->obs.contains(GnssObs::Carrier)) { continue; }
1888
1889 27786 auto key = States::AmbiguityDD{ satSigId };
1890
1/2
✓ Branch 2 taken 27786 times.
✗ Branch 3 not taken.
 27786 if (!_kalmanFilter.x.hasRow(key) // Ambiguity not in state yet
1891
2/4
✓ Branch 2 taken 4251 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 4251 times.
✗ Branch 5 not taken.
 4251 && _pivotSatellites.contains({ satSigId.code, GnssObs::Carrier }) // There should be a pivot satellite for this observation
1892
8/10
✓ Branch 0 taken 4251 times.
✓ Branch 1 taken 23535 times.
✓ Branch 4 taken 4251 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4251 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 44 times.
✓ Branch 10 taken 4207 times.
✓ Branch 11 taken 44 times.
✓ Branch 12 taken 27742 times.
 32037 && _pivotSatellites.at({ satSigId.code, GnssObs::Carrier }).satSigId != satSigId) // Don't add a ambiguity for pivot satellite
1893 {
1894 44 anyStateChanged = true;
1895 LOG_TRACE("{}: [{}] Adding state: {}", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), key);
1896
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
 44 _kalmanFilter.addState(key);
1897
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 newAddedAmbiguities.emplace_back(key);
1898
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
 44 if (std::ranges::find(rtkSol->newEstimatedAmbiguity, satSigId)
1899
1/2
✓ Branch 3 taken 44 times.
✗ Branch 4 not taken.
 88 == rtkSol->newEstimatedAmbiguity.end())
1900 {
1901
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
 44 rtkSol->newEstimatedAmbiguity.push_back(satSigId);
1902 }
1903
1904 // F: Entries are all 0
1905 // Ambiguities are modeled as RW with very small noise to keep numerical stability
1906
1/2
✓ Branch 3 taken 44 times.
✗ Branch 4 not taken.
 44 _kalmanFilter.G(key, key) = 1;
1907
1/2
✓ Branch 3 taken 44 times.
✗ Branch 4 not taken.
 44 _kalmanFilter.Phi(key, key) = 1; // This is set, because we only calculate the matrix for PosVel
1908
1909
3/4
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 38 times.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
 44 _kalmanFilter.W(key, key) = _lastSolutionStatus == RtkSolution::SolutionType::RTK_Fixed ? _ambiguityFixProcessNoiseVariance : _ambiguityFloatProcessNoiseVariance;
1910
1911
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
 44 const auto& pivotSat = _pivotSatellites.at({ satSigId.code, GnssObs::Carrier });
1912 LOG_DATA("{}: {} - pivot {}", nameId(), satSigId, pivotSat.satSigId);
1913
2/4
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 44 times.
 44 if (!observations.signals.contains(pivotSat.satSigId))
1914 {
1915 ✗ LOG_CRITICAL("{}: [{}] The pivot satellite [{}] does not have a carrier measurement. It should have been switched before.",
1916 nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), pivotSat.satSigId);
1917 }
1918
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 const auto& pivotObs = observations.signals.at(pivotSat.satSigId);
1919
1920 // Initialize with difference of 1/lambda (carrier-phase_DD - pseudorange_DD) measurement.
1921
2/4
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
 44 double lambda_j = InsConst::C / satSigId.freq().getFrequency(observation.freqNum());
1922
1923 // Carrier is always there, otherwise we would not initialize the ambiguity
1924
2/4
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
 44 const auto& phi_r_1 = pivotObs.recvObs.at(Rover)->obs.at(GnssObs::Carrier);
1925
2/4
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
 44 const auto& phi_b_1 = pivotObs.recvObs.at(Base)->obs.at(GnssObs::Carrier);
1926
2/4
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
 44 const auto& phi_r_s = observation.recvObs.at(Rover)->obs.at(GnssObs::Carrier);
1927
2/4
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
 44 const auto& phi_b_s = observation.recvObs.at(Base)->obs.at(GnssObs::Carrier);
1928
1929 44 double phi_br_s_meas = phi_r_s.measurement - phi_b_s.measurement;
1930 44 double phi_br_1_meas = phi_r_1.measurement - phi_b_1.measurement;
1931 44 double phi_br_1s_meas = phi_br_s_meas - phi_br_1_meas;
1932
1933 44 double varCarrierPseudorange = std::numeric_limits<double>::infinity();
1934 44 bool forceCarrierPseudorange = false;
1935
1936 // Pseudorange could be missing
1937
2/4
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
 44 if (observation.recvObs.at(Rover)->obs.contains(GnssObs::Pseudorange)
1938
3/6
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 44 times.
✗ Branch 8 not taken.
 44 && observation.recvObs.at(Base)->obs.contains(GnssObs::Pseudorange)
1939
4/8
✓ Branch 0 taken 44 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 44 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 44 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 44 times.
✗ Branch 9 not taken.
 88 && _pivotSatellites.contains({ satSigId.code, GnssObs::Pseudorange }))
1940 {
1941
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
 44 const auto& pivotSatPsr = _pivotSatellites.at({ satSigId.code, GnssObs::Pseudorange });
1942
2/4
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 44 times.
 44 if (pivotSatPsr.satSigId == satSigId) // We cannot initialize the ambiguity with different pivot satellites
1943 {
1944 ✗ const auto& pivotObsPsr = observations.signals.at(pivotSatPsr.satSigId);
1945 ✗ const auto& p_r_1 = pivotObsPsr.recvObs.at(Rover)->obs.at(GnssObs::Pseudorange);
1946 ✗ const auto& p_b_1 = pivotObsPsr.recvObs.at(Base)->obs.at(GnssObs::Pseudorange);
1947 ✗ const auto& p_r_s = observation.recvObs.at(Rover)->obs.at(GnssObs::Pseudorange);
1948 ✗ const auto& p_b_s = observation.recvObs.at(Base)->obs.at(GnssObs::Pseudorange);
1949
1950 ✗ varCarrierPseudorange = phi_r_s.measVar + p_r_s.measVar + phi_b_s.measVar + p_b_s.measVar
1951 ✗ + phi_r_1.measVar + p_r_1.measVar + phi_b_1.measVar + p_b_1.measVar;
1952
1953 ✗ double posUncertanty = _kalmanFilter.P.block<3>(States::Pos, States::Pos).diagonal().cwiseSqrt().norm();
1954 ✗ forceCarrierPseudorange = posUncertanty > 0.06; // Always use the Carrier-Pseudorange difference when position is not good (stdDev 5cm)
1955 }
1956 }
1957
1958
2/4
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 44 times.
✗ Branch 7 not taken.
 44 Eigen::Vector3d e_pLOS_1s = pivotObs.recvObs.at(Rover)->e_pLOS(_receiver[Rover].e_posMarker)
1959
4/8
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 44 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 44 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 44 times.
✗ Branch 13 not taken.
 88 - observation.recvObs.at(Rover)->e_pLOS(_receiver[Rover].e_posMarker);
1960 44 const double varCarrierGeometry = phi_r_s.measVar + phi_b_s.measVar + phi_r_1.measVar + phi_b_1.measVar
1961
5/10
✓ Branch 3 taken 44 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 44 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 44 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 44 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 44 times.
✗ Branch 16 not taken.
 44 + e_pLOS_1s.transpose() * _kalmanFilter.P.block<3>(States::Pos, States::Pos) * e_pLOS_1s;
1962 LOG_DATA("{}: varCarrierGeometry = {:.2g} [m], varCarrierPseudorange = {:.2g} [m]", nameId(), varCarrierGeometry, varCarrierPseudorange);
1963
1964
2/4
✓ Branch 0 taken 44 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 44 times.
 44 if (varCarrierPseudorange < varCarrierGeometry || forceCarrierPseudorange)
1965 {
1966 ✗ const auto& p_r_1 = pivotObs.recvObs.at(Rover)->obs.at(GnssObs::Pseudorange);
1967 ✗ const auto& p_b_1 = pivotObs.recvObs.at(Base)->obs.at(GnssObs::Pseudorange);
1968 ✗ const auto& p_r_s = observation.recvObs.at(Rover)->obs.at(GnssObs::Pseudorange);
1969 ✗ const auto& p_b_s = observation.recvObs.at(Base)->obs.at(GnssObs::Pseudorange);
1970
1971 ✗ double p_br_s_meas = p_r_s.measurement - p_b_s.measurement;
1972 ✗ double p_br_1_meas = p_r_1.measurement - p_b_1.measurement;
1973 ✗ double p_br_1s_meas = p_br_s_meas - p_br_1_meas;
1974
1975 ✗ _kalmanFilter.x(key) = (phi_br_1s_meas - p_br_1s_meas) / lambda_j;
1976
1977 ✗ _kalmanFilter.P(key, key) = varCarrierPseudorange * 3.0 / std::pow(lambda_j, 2); // Safety factor of 3 and division by lambda^2, because state in [cycles]
1978 auto msg = fmt::format("ddCP({:.1f}m) - ddPR({:.1f}m) = {:.1f} [cycles], std = {:.2f} [cycles]",
1979 ✗ phi_br_1s_meas, p_br_1s_meas, _kalmanFilter.x(key), std::sqrt(_kalmanFilter.P(key, key)));
1980 LOG_DATA("{}: [{}] Init Amibguity: {}", nameId(), satSigId, msg);
1981
1982 ✗ addEvent(fmt::format("State [{}] added ({})", key, msg));
1983 ✗ }
1984 else // Carrier-Geometry Difference
1985 {
1986 44 double phi_br_s_est = phi_r_s.estimate - phi_b_s.estimate;
1987 44 double phi_br_1_est = phi_r_1.estimate - phi_b_1.estimate;
1988 44 double phi_br_1s_est = phi_br_s_est - phi_br_1_est;
1989
1990
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
 44 _kalmanFilter.x(key) = (phi_br_1s_meas - phi_br_1s_est) / lambda_j;
1991
1992
1/2
✓ Branch 4 taken 44 times.
✗ Branch 5 not taken.
 44 _kalmanFilter.P(key, key) = varCarrierGeometry * 3.0 / std::pow(lambda_j, 2); // Safety factor of 3 and division by lambda^2, because state in [cycles]
1993 auto msg = fmt::format("ddCP({:.1f}m) - ddEst({:.1f}m) = {:.1f} [cycles], std = {:.2f} [cycles]",
1994
2/4
✓ Branch 3 taken 44 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 44 times.
✗ Branch 8 not taken.
 44 phi_br_1s_meas, phi_br_1s_est, _kalmanFilter.x(key), std::sqrt(_kalmanFilter.P(key, key)));
1995 LOG_DATA("{}: [{}] Init Amibguity: {}", nameId(), satSigId, msg);
1996
1997
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
 88 addEvent(fmt::format("State [{}] added ({})", key, msg));
1998 44 }
1999 }
2000 }
2001
2002
2/2
✓ Branch 2 taken 23582 times.
✓ Branch 3 taken 601 times.
 24183 for (size_t i = States::KFStates_COUNT; i < _kalmanFilter.x.rowKeys().size(); i++) // 0-2 Pos, 3-5 Vel
2003 {
2004
1/2
✓ Branch 2 taken 23582 times.
✗ Branch 3 not taken.
 23582 const auto& key = _kalmanFilter.x.rowKeys().at(i);
2005
1/2
✓ Branch 1 taken 23582 times.
✗ Branch 2 not taken.
 23582 if (const auto* ambDD = std::get_if<States::AmbiguityDD>(&key))
2006 {
2007
3/4
✓ Branch 1 taken 23582 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 23579 times.
✓ Branch 4 taken 3 times.
 23582 if (observations.countObservations(ambDD->satSigId, GnssObs::Carrier) == ReceiverType_COUNT) { continue; }
2008
2009 LOG_DATA("{}: [{}] Removing Ambiguity as not observed anymore {}", nameId(), ambDD->satSigId, key);
2010
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 _ambiguitiesHold.erase(ambDD->satSigId);
2011
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 addEvent(fmt::format("State [{}] removed (not observed anymore)", key));
2012
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 _kalmanFilter.removeState(key); // After this 'key' is invalidated
2013 3 i--;
2014 }
2015 }
2016
2017 if (anyStateChanged)
2018 {
2019 LOG_DATA("{}: x =\n{}", nameId(), _kalmanFilter.x.transposed());
2020 LOG_DATA("{}: P =\n{}", nameId(), _kalmanFilter.P);
2021 LOG_DATA("{}: G =\n{}", nameId(), _kalmanFilter.G);
2022 LOG_DATA("{}: W =\n{}", nameId(), _kalmanFilter.W);
2023 }
2024 601 }
2025
2026 601 RealTimeKinematic::Differences RealTimeKinematic::calcSingleDifferences(const Observations& observations) const // NOLINT(readability-convert-member-functions-to-static)
2027 {
2028 601 Differences singleDifferences;
2029
1/2
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
 601 singleDifferences.reserve(observations.signals.size());
2030
2031 LOG_DATA("{}: Calculating single differences (rover - base):", nameId());
2032
2/2
✓ Branch 7 taken 27786 times.
✓ Branch 8 taken 601 times.
 28387 for (const auto& [satSigId, observation] : observations.signals)
2033 {
2034
1/2
✓ Branch 1 taken 27786 times.
✗ Branch 2 not taken.
 27786 const auto& baseObservations = observation.recvObs.at(Base)->obs;
2035
1/2
✓ Branch 1 taken 27786 times.
✗ Branch 2 not taken.
 27786 const auto& roverObservations = observation.recvObs.at(Rover)->obs;
2036
2037
2/2
✓ Branch 6 taken 83358 times.
✓ Branch 7 taken 27786 times.
 111144 for (const auto& [obsType, baseObs] : baseObservations)
2038 {
2039
1/2
✓ Branch 1 taken 83358 times.
✗ Branch 2 not taken.
 83358 const auto& roverObs = roverObservations.at(obsType);
2040
2041
2/4
✓ Branch 1 taken 83358 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 83358 times.
✗ Branch 5 not taken.
 83358 singleDifferences[satSigId][obsType].estimate = roverObs.estimate - baseObs.estimate;
2042
2/4
✓ Branch 1 taken 83358 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 83358 times.
✗ Branch 5 not taken.
 83358 singleDifferences[satSigId].at(obsType).measurement = roverObs.measurement - baseObs.measurement;
2043
2044 LOG_DATA("{}: [{}][{:11}][Meas] r {:.3f} - b {:.3f} = {:.3f}", nameId(), satSigId, obsType,
2045 roverObs.measurement, baseObs.measurement, singleDifferences[satSigId][obsType].measurement);
2046 LOG_DATA("{}: [{}][{:11}][Est ] r {:.3f} - b {:.3f} = {:.3f} (diff to meas {:.3e})", nameId(), satSigId, obsType,
2047 roverObs.estimate, baseObs.estimate, singleDifferences[satSigId][obsType].estimate,
2048 singleDifferences[satSigId][obsType].measurement - singleDifferences[satSigId][obsType].estimate);
2049 }
2050 }
2051
2052 601 return singleDifferences;
2053 ✗ }
2054
2055 601 RealTimeKinematic::Differences RealTimeKinematic::calcDoubleDifferences(const Observations& observations, const Differences& singleDifferences) const
2056 {
2057 601 Differences doubleDifferences;
2058
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 601 times.
 601 if (singleDifferences.empty()) { return doubleDifferences; }
2059
1/2
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
 601 doubleDifferences.reserve(singleDifferences.size() - 1);
2060
2061 LOG_DATA("{}: [{}] Calculating double differences (sat - pivot):", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST));
2062
2/2
✓ Branch 7 taken 27786 times.
✓ Branch 8 taken 601 times.
 28387 for (const auto& [satSigId_s, singleDiff_s] : singleDifferences)
2063 {
2064
2/2
✓ Branch 7 taken 83358 times.
✓ Branch 8 taken 27786 times.
 111144 for (const auto& [obsType, sDiff_s] : singleDiff_s)
2065 {
2066
2/4
✓ Branch 2 taken 83358 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 83358 times.
 83358 if (!_pivotSatellites.contains({ satSigId_s.code, obsType }))
2067 {
2068 LOG_DATA("{}: [{}] Not calculating double difference for [{} {}] because no pivot satellite.", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), satSigId_s.code, obsType);
2069 ✗ continue;
2070 }
2071
1/2
✓ Branch 2 taken 83358 times.
✗ Branch 3 not taken.
 83358 const auto& satSigId_1 = _pivotSatellites.at({ satSigId_s.code, obsType }).satSigId;
2072
3/4
✓ Branch 1 taken 83358 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12621 times.
✓ Branch 4 taken 70737 times.
 83358 if (satSigId_s == satSigId_1) { continue; } // No double difference with itself
2073
2074
2/4
✓ Branch 1 taken 70737 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 70737 times.
 70737 if (!singleDifferences.contains(satSigId_1))
2075 {
2076 ✗ LOG_WARN("{}: [{}] Pivot [{}] has no single difference. This is a bug.", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), satSigId_1);
2077 ✗ continue;
2078 }
2079
3/6
✓ Branch 1 taken 70737 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70737 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 70737 times.
 70737 if (!singleDifferences.at(satSigId_1).contains(obsType))
2080 {
2081 ✗ LOG_WARN("{}: [{}] Pivot [{}] has no observation type [{}]. This is a bug.", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), satSigId_1, obsType);
2082 ✗ continue;
2083 }
2084
2085
1/2
✓ Branch 1 taken 70737 times.
✗ Branch 2 not taken.
 70737 const auto& singleDiff_1 = singleDifferences.at(satSigId_1);
2086
2087
1/2
✓ Branch 1 taken 70737 times.
✗ Branch 2 not taken.
 70737 const auto& sDiff_1 = singleDiff_1.at(obsType);
2088
2089
2/4
✓ Branch 1 taken 70737 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70737 times.
✗ Branch 5 not taken.
 70737 doubleDifferences[satSigId_s][obsType].estimate = sDiff_s.estimate - sDiff_1.estimate;
2090
2/4
✓ Branch 1 taken 70737 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70737 times.
✗ Branch 5 not taken.
 70737 doubleDifferences[satSigId_s].at(obsType).measurement = sDiff_s.measurement - sDiff_1.measurement;
2091 LOG_DATA("{}: [{} - {}][{:11}][Meas] sat {:.3f} - piv {:.3f} = {:.3f}", nameId(), satSigId_s, satSigId_1, obsType,
2092 sDiff_s.measurement, sDiff_1.measurement, doubleDifferences[satSigId_s][obsType].measurement);
2093
2094
2/2
✓ Branch 0 taken 23579 times.
✓ Branch 1 taken 47158 times.
 70737 if (obsType == GnssObs::Carrier) // Pivot satellite ambiguity is 0 and not in state
2095 {
2096
3/6
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23579 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 23579 times.
✗ Branch 9 not taken.
 23579 double lambda_j_1 = InsConst::C / satSigId_1.freq().getFrequency(observations.signals.at(satSigId_1).freqNum());
2097
1/2
✓ Branch 3 taken 23579 times.
✗ Branch 4 not taken.
 23579 double N_br_1s = _kalmanFilter.x(States::AmbiguityDD{ satSigId_s });
2098
2/4
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23579 times.
✗ Branch 5 not taken.
 23579 doubleDifferences[satSigId_s].at(obsType).estimate += lambda_j_1 * N_br_1s;
2099 }
2100 LOG_DATA("{}: [{} - {}][{:11}][Est ] sat {:.3f} - piv {:.3f} = {:.3f} (diff to meas {:.3e})", nameId(), satSigId_s, satSigId_1, obsType,
2101 sDiff_s.estimate, sDiff_1.estimate, doubleDifferences[satSigId_s][obsType].estimate,
2102 doubleDifferences[satSigId_s][obsType].measurement - doubleDifferences[satSigId_s][obsType].estimate);
2103 }
2104 }
2105
2106 601 return doubleDifferences;
2107 ✗ }
2108
2109 unordered_map<GnssObs::ObservationType, KeyedMatrixXd<RTK::Meas::SingleObs<RealTimeKinematic::ReceiverType>>>
2110 601 RealTimeKinematic::calcSingleObsMeasurementNoiseMatrices(const Observations& observations) const // NOLINT(readability-convert-member-functions-to-static)
2111 {
2112 601 unordered_map<GnssObs::ObservationType, std::vector<RTK::Meas::SingleObs<ReceiverType>>> measKeys;
2113
2/2
✓ Branch 7 taken 27786 times.
✓ Branch 8 taken 601 times.
 28387 for (const auto& [satSigId, observation] : observations.signals)
2114 {
2115
3/4
✓ Branch 1 taken 27786 times.
✗ Branch 2 not taken.
✓ Branch 10 taken 83358 times.
✓ Branch 11 taken 27786 times.
 111144 for (const auto& [obsType, baseObs] : observation.recvObs.at(Base)->obs)
2116 {
2117
2/4
✓ Branch 1 taken 83358 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 83358 times.
✗ Branch 5 not taken.
 83358 measKeys[obsType].emplace_back(satSigId, Rover, obsType);
2118
2/4
✓ Branch 1 taken 83358 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 83358 times.
✗ Branch 5 not taken.
 83358 measKeys[obsType].emplace_back(satSigId, Base, obsType);
2119 }
2120 }
2121
2122 601 unordered_map<GnssObs::ObservationType, KeyedMatrixXd<RTK::Meas::SingleObs<ReceiverType>>> Rtilde;
2123
2/2
✓ Branch 6 taken 1803 times.
✓ Branch 7 taken 601 times.
 2404 for (const auto& [obsType, keys] : measKeys)
2124 {
2125
3/6
✓ Branch 4 taken 1803 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1803 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1803 times.
✗ Branch 11 not taken.
 1803 Rtilde[obsType] = KeyedMatrixXd<RTK::Meas::SingleObs<ReceiverType>>(Eigen::MatrixXd::Zero(static_cast<int>(keys.size()), static_cast<int>(keys.size())), keys);
2126 }
2127
2128
2/2
✓ Branch 7 taken 27786 times.
✓ Branch 8 taken 601 times.
 28387 for (const auto& [satSigId, observation] : observations.signals)
2129 {
2130
1/2
✓ Branch 1 taken 27786 times.
✗ Branch 2 not taken.
 27786 const auto& baseObservations = observation.recvObs.at(Base)->obs;
2131
1/2
✓ Branch 1 taken 27786 times.
✗ Branch 2 not taken.
 27786 const auto& roverObservations = observation.recvObs.at(Rover)->obs;
2132
2133
2/2
✓ Branch 6 taken 83358 times.
✓ Branch 7 taken 27786 times.
 111144 for (const auto& [obsType, baseObs] : baseObservations)
2134 {
2135
1/2
✓ Branch 1 taken 83358 times.
✗ Branch 2 not taken.
 83358 const auto& roverObs = roverObservations.at(obsType);
2136
2137 83358 RTK::Meas::SingleObs<ReceiverType> roverKey(satSigId, Rover, obsType);
2138 83358 RTK::Meas::SingleObs<ReceiverType> baseKey(satSigId, Base, obsType);
2139
2140
2/4
✓ Branch 1 taken 83358 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 83358 times.
✗ Branch 5 not taken.
 83358 Rtilde.at(obsType)(roverKey, roverKey) = roverObs.measVar;
2141
2/4
✓ Branch 1 taken 83358 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 83358 times.
✗ Branch 5 not taken.
 83358 Rtilde.at(obsType)(baseKey, baseKey) = baseObs.measVar;
2142 }
2143 }
2144
2145 // for ([[maybe_unused]] const auto& [obsType, R] : Rtilde)
2146 // {
2147 // LOG_DATA("{}: R_tilde({}) =\n{}", nameId(), obsType, R);
2148 // }
2149
2150 1202 return Rtilde;
2151 601 }
2152
2153 601 void RealTimeKinematic::calcKalmanUpdateMatrices(const Observations& observations, const Differences& doubleDifferences,
2154 const unordered_map<GnssObs::ObservationType, KeyedMatrixXd<RTK::Meas::SingleObs<RealTimeKinematic::ReceiverType>>>& Rtilde)
2155 {
2156 // Update the Measurement sensitivity Matrix (𝐇), the Measurement noise covariance matrix (𝐑) and the Measurement vector (𝐳)
2157
2158 601 std::vector<Meas::MeasKeyTypes> measKeys;
2159
1/2
✓ Branch 4 taken 601 times.
✗ Branch 5 not taken.
 601 measKeys.reserve(doubleDifferences.size() * _obsFilter.getUsedObservationTypes().size());
2160
2/2
✓ Branch 0 taken 1803 times.
✓ Branch 1 taken 601 times.
 2404 for (size_t i = 0; i < GnssObs::ObservationType_COUNT; i++)
2161 {
2162
4/4
✓ Branch 0 taken 1202 times.
✓ Branch 1 taken 601 times.
✓ Branch 2 taken 601 times.
✓ Branch 3 taken 601 times.
 1803 GnssObs::ObservationType oType = i == 0 ? GnssObs::Carrier : (i == 1 ? GnssObs::Pseudorange : GnssObs::Doppler);
2163
2164
2/2
✓ Branch 7 taken 70737 times.
✓ Branch 8 taken 1803 times.
 72540 for (const auto& [satSigId, doubleDiff] : doubleDifferences)
2165 {
2166
2/2
✓ Branch 7 taken 212211 times.
✓ Branch 8 taken 70737 times.
 282948 for (const auto& [obsType, diff] : doubleDiff)
2167 {
2168
2/2
✓ Branch 0 taken 70737 times.
✓ Branch 1 taken 141474 times.
 212211 if (obsType == oType)
2169 {
2170
3/5
✓ Branch 0 taken 23579 times.
✓ Branch 1 taken 23579 times.
✓ Branch 2 taken 23579 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 70737 switch (obsType)
2171 {
2172 23579 case GnssObs::Pseudorange:
2173
1/2
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
 23579 measKeys.emplace_back(Meas::PsrDD{ satSigId });
2174 23579 break;
2175 23579 case GnssObs::Carrier:
2176
1/2
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
 23579 measKeys.emplace_back(Meas::CarrierDD{ satSigId });
2177 23579 break;
2178 23579 case GnssObs::Doppler:
2179
1/2
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
 23579 measKeys.emplace_back(Meas::DopplerDD{ satSigId });
2180 23579 break;
2181 ✗ case GnssObs::ObservationType_COUNT:
2182 ✗ LOG_CRITICAL("{}: ObservationType_COUNT is not a valid value", nameId());
2183 break;
2184 }
2185 }
2186 }
2187 }
2188 }
2189
2190 LOG_DATA("{}: Setting measurement keys: {}", nameId(), joinToString(measKeys));
2191
1/2
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
 601 _kalmanFilter.setMeasurements(measKeys);
2192
2193 // R matrix
2194
2/2
✓ Branch 7 taken 1803 times.
✓ Branch 8 taken 601 times.
 2404 for (const auto& [obsType, R] : Rtilde)
2195 {
2196 1803 std::vector<Meas::MeasKeyTypes> obsTypeMeasKeys;
2197
2/2
✓ Branch 5 taken 212211 times.
✓ Branch 6 taken 1803 times.
 214014 for (const auto& key : measKeys)
2198 {
2199
7/8
✓ Branch 1 taken 70737 times.
✓ Branch 2 taken 141474 times.
✓ Branch 3 taken 23579 times.
✓ Branch 4 taken 47158 times.
✓ Branch 5 taken 23579 times.
✓ Branch 6 taken 188632 times.
✓ Branch 8 taken 23579 times.
✗ Branch 9 not taken.
 212211 if (std::holds_alternative<Meas::PsrDD>(key) && obsType == GnssObs::Pseudorange) { obsTypeMeasKeys.push_back(key); }
2200
7/8
✓ Branch 1 taken 70737 times.
✓ Branch 2 taken 117895 times.
✓ Branch 3 taken 23579 times.
✓ Branch 4 taken 47158 times.
✓ Branch 5 taken 23579 times.
✓ Branch 6 taken 165053 times.
✓ Branch 8 taken 23579 times.
✗ Branch 9 not taken.
 188632 else if (std::holds_alternative<Meas::CarrierDD>(key) && obsType == GnssObs::Carrier) { obsTypeMeasKeys.push_back(key); }
2201
7/8
✓ Branch 1 taken 70737 times.
✓ Branch 2 taken 94316 times.
✓ Branch 3 taken 23579 times.
✓ Branch 4 taken 47158 times.
✓ Branch 5 taken 23579 times.
✓ Branch 6 taken 141474 times.
✓ Branch 8 taken 23579 times.
✗ Branch 9 not taken.
 165053 else if (std::holds_alternative<Meas::DopplerDD>(key) && obsType == GnssObs::Doppler) { obsTypeMeasKeys.push_back(key); }
2202 }
2203
1/2
✓ Branch 3 taken 1803 times.
✗ Branch 4 not taken.
 1803 KeyedMatrixXd<RTK::Meas::MeasKeyTypes, RTK::Meas::SingleObs<ReceiverType>> J(Eigen::MatrixXd::Zero(static_cast<int>(obsTypeMeasKeys.size()), R.rows()),
2204
1/2
✓ Branch 4 taken 1803 times.
✗ Branch 5 not taken.
 3606 obsTypeMeasKeys, R.rowKeys());
2205
2206
2/2
✓ Branch 4 taken 70737 times.
✓ Branch 5 taken 1803 times.
 72540 for (const auto& rowKey : obsTypeMeasKeys)
2207 {
2208
1/2
✓ Branch 1 taken 70737 times.
✗ Branch 2 not taken.
 70737 SatSigId satSigId_s;
2209
3/5
✓ Branch 0 taken 23579 times.
✓ Branch 1 taken 23579 times.
✓ Branch 2 taken 23579 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 70737 switch (obsType)
2210 {
2211 23579 case GnssObs::Pseudorange:
2212
1/2
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
 23579 satSigId_s = std::get<Meas::PsrDD>(rowKey).satSigId;
2213 23579 break;
2214 23579 case GnssObs::Carrier:
2215
1/2
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
 23579 satSigId_s = std::get<Meas::CarrierDD>(rowKey).satSigId;
2216 23579 break;
2217 23579 case GnssObs::Doppler:
2218
1/2
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
 23579 satSigId_s = std::get<Meas::DopplerDD>(rowKey).satSigId;
2219 23579 break;
2220 ✗ case GnssObs::ObservationType_COUNT:
2221 ✗ LOG_CRITICAL("{}: ObservationType_COUNT is not a valid value", nameId());
2222 break;
2223 }
2224
2225
1/2
✓ Branch 2 taken 70737 times.
✗ Branch 3 not taken.
 70737 const auto& satSigId_1 = _pivotSatellites.at({ satSigId_s.code, obsType }).satSigId;
2226
2227
1/2
✓ Branch 2 taken 70737 times.
✗ Branch 3 not taken.
 70737 J(rowKey, RTK::Meas::SingleObs<ReceiverType>(satSigId_s, Rover, obsType)) = 1;
2228
1/2
✓ Branch 2 taken 70737 times.
✗ Branch 3 not taken.
 70737 J(rowKey, RTK::Meas::SingleObs<ReceiverType>(satSigId_s, Base, obsType)) = -1;
2229
1/2
✓ Branch 2 taken 70737 times.
✗ Branch 3 not taken.
 70737 J(rowKey, RTK::Meas::SingleObs<ReceiverType>(satSigId_1, Rover, obsType)) = -1;
2230
1/2
✓ Branch 2 taken 70737 times.
✗ Branch 3 not taken.
 70737 J(rowKey, RTK::Meas::SingleObs<ReceiverType>(satSigId_1, Base, obsType)) = 1;
2231 }
2232 // LOG_DATA("{}: J({}) =\n{}", nameId(), obsType, J);
2233
2234
5/10
✓ Branch 2 taken 1803 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1803 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1803 times.
✗ Branch 11 not taken.
✓ Branch 15 taken 1803 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1803 times.
✗ Branch 19 not taken.
 1803 _kalmanFilter.R(obsTypeMeasKeys, obsTypeMeasKeys) = J(all, all) * R(all, all) * J(all, all).transpose();
2235 1803 }
2236 LOG_DATA("{}: R =\n{}", nameId(), _kalmanFilter.R);
2237
2238
2/2
✓ Branch 7 taken 23579 times.
✓ Branch 8 taken 601 times.
 24180 for (const auto& [satSigId_s, doubleDiff] : doubleDifferences)
2239 {
2240
1/2
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
 23579 const auto& obs_s = observations.signals.at(satSigId_s);
2241
2242
2/4
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23579 times.
✗ Branch 6 not taken.
 23579 double lambda_j = InsConst::C / satSigId_s.freq().getFrequency(obs_s.freqNum());
2243
2244
2/4
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 23579 times.
✗ Branch 7 not taken.
 23579 const auto& e_pLOS_s = obs_s.recvObs.at(Rover)->e_pLOS(_receiver[Rover].e_posMarker);
2245
2246
2/2
✓ Branch 7 taken 70737 times.
✓ Branch 8 taken 23579 times.
 94316 for (const auto& [obsType, obs] : doubleDiff)
2247 {
2248
1/2
✓ Branch 2 taken 70737 times.
✗ Branch 3 not taken.
 70737 const auto& satSigId_1 = _pivotSatellites.at({ satSigId_s.code, obsType }).satSigId;
2249
1/2
✓ Branch 1 taken 70737 times.
✗ Branch 2 not taken.
 70737 const auto& obs_1 = observations.signals.at(satSigId_1);
2250
2/4
✓ Branch 1 taken 70737 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 70737 times.
✗ Branch 7 not taken.
 70737 const auto& e_pLOS_1 = obs_1.recvObs.at(Rover)->e_pLOS(_receiver[Rover].e_posMarker);
2251
2252
3/5
✓ Branch 0 taken 23579 times.
✓ Branch 1 taken 23579 times.
✓ Branch 2 taken 23579 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 70737 switch (obsType)
2253 {
2254 23579 case GnssObs::Pseudorange:
2255
1/2
✓ Branch 2 taken 23579 times.
✗ Branch 3 not taken.
 23579 _kalmanFilter.z(Meas::PsrDD{ satSigId_s }) = obs.measurement - obs.estimate;
2256
4/8
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23579 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 23579 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 23579 times.
✗ Branch 13 not taken.
 23579 _kalmanFilter.H.block<3>(Meas::PsrDD{ satSigId_s }, States::Pos) = (e_pLOS_1 - e_pLOS_s).transpose();
2257 23579 break;
2258 23579 case GnssObs::Carrier:
2259
1/2
✓ Branch 2 taken 23579 times.
✗ Branch 3 not taken.
 23579 _kalmanFilter.z(Meas::CarrierDD{ satSigId_s }) = obs.measurement - obs.estimate;
2260
4/8
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23579 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 23579 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 23579 times.
✗ Branch 13 not taken.
 23579 _kalmanFilter.H.block<3>(Meas::CarrierDD{ satSigId_s }, States::Pos) = (e_pLOS_1 - e_pLOS_s).transpose();
2261
1/2
✓ Branch 4 taken 23579 times.
✗ Branch 5 not taken.
 23579 _kalmanFilter.H(Meas::CarrierDD{ satSigId_s }, States::AmbiguityDD{ satSigId_s }) = lambda_j;
2262 23579 break;
2263 23579 case GnssObs::Doppler:
2264 {
2265
1/2
✓ Branch 2 taken 23579 times.
✗ Branch 3 not taken.
 23579 _kalmanFilter.z(Meas::DopplerDD{ satSigId_s }) = obs.measurement - obs.estimate;
2266
2/4
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 23579 times.
✗ Branch 8 not taken.
 23579 const auto& e_vLOS_1 = obs_1.recvObs.at(Rover)->e_vLOS(_receiver[Rover].e_posMarker, _receiver[Rover].e_vel);
2267
2/4
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 23579 times.
✗ Branch 8 not taken.
 23579 const auto& e_vLOS_s = obs_s.recvObs.at(Rover)->e_vLOS(_receiver[Rover].e_posMarker, _receiver[Rover].e_vel);
2268
2269
1/2
✓ Branch 3 taken 23579 times.
✗ Branch 4 not taken.
 23579 _kalmanFilter.H.block<3>(Meas::DopplerDD{ satSigId_s }, States::Pos) = Eigen::RowVector3d(
2270
20/40
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23579 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23579 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23579 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 23579 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 23579 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23579 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23579 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23579 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 23579 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 23579 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 23579 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 23579 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 23579 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 23579 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 23579 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 23579 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 23579 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 23579 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 23579 times.
✗ Branch 59 not taken.
 23579 -e_vLOS_1.x() * e_pLOS_1.x() * e_pLOS_1.x() + e_vLOS_1.x() + e_vLOS_s.x() * e_pLOS_s.x() * e_pLOS_s.x() - e_vLOS_s.x() - e_vLOS_1.y() * e_pLOS_1.x() * e_pLOS_1.y() + e_vLOS_s.y() * e_pLOS_s.x() * e_pLOS_s.y() - e_vLOS_1.z() * e_pLOS_1.x() * e_pLOS_1.z() + e_vLOS_s.z() * e_pLOS_s.x() * e_pLOS_s.z(),
2271
20/40
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23579 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23579 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23579 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 23579 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 23579 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23579 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23579 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23579 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 23579 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 23579 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 23579 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 23579 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 23579 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 23579 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 23579 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 23579 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 23579 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 23579 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 23579 times.
✗ Branch 59 not taken.
 23579 -e_vLOS_1.x() * e_pLOS_1.x() * e_pLOS_1.y() + e_vLOS_s.x() * e_pLOS_s.x() * e_pLOS_s.y() - e_vLOS_1.y() * e_pLOS_1.y() * e_pLOS_1.y() + e_vLOS_1.y() + e_vLOS_s.y() * e_pLOS_s.y() * e_pLOS_s.y() - e_vLOS_s.y() - e_vLOS_1.z() * e_pLOS_1.y() * e_pLOS_1.z() + e_vLOS_s.z() * e_pLOS_s.y() * e_pLOS_s.z(),
2272
22/44
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23579 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23579 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23579 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 23579 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 23579 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23579 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23579 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23579 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 23579 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 23579 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 23579 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 23579 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 23579 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 23579 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 23579 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 23579 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 23579 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 23579 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 23579 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 23579 times.
✗ Branch 62 not taken.
✓ Branch 64 taken 23579 times.
✗ Branch 65 not taken.
 47158 -e_vLOS_1.x() * e_pLOS_1.x() * e_pLOS_1.z() + e_vLOS_s.x() * e_pLOS_s.x() * e_pLOS_s.z() - e_vLOS_1.y() * e_pLOS_1.y() * e_pLOS_1.z() + e_vLOS_s.y() * e_pLOS_s.y() * e_pLOS_s.z() - e_vLOS_1.z() * e_pLOS_1.z() * e_pLOS_1.z() + e_vLOS_1.z() + e_vLOS_s.z() * e_pLOS_s.z() * e_pLOS_s.z() - e_vLOS_s.z());
2273
4/8
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23579 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 23579 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 23579 times.
✗ Branch 13 not taken.
 23579 _kalmanFilter.H.block<3>(Meas::DopplerDD{ satSigId_s }, States::Vel) = (e_pLOS_1 - e_pLOS_s).transpose();
2274 23579 break;
2275 }
2276 ✗ case GnssObs::ObservationType_COUNT:
2277 ✗ LOG_CRITICAL("{}: ObservationType_COUNT is not a valid value", nameId());
2278 break;
2279 }
2280 }
2281 }
2282
2283 LOG_DATA("{}: z =\n{}", nameId(), _kalmanFilter.z.transposed());
2284 LOG_DATA("{}: H =\n{}", nameId(), _kalmanFilter.H);
2285 601 }
2286
2287 ✗ std::vector<RealTimeKinematic::OutlierInfo> RealTimeKinematic::removeOutlier(Observations& observations, const std::shared_ptr<RtkSolution>& rtkSol)
2288 {
2289 ✗ KeyedVectorXd<RTK::Meas::MeasKeyTypes> normInno((_kalmanFilter.z(all).array().abs()
2290 ✗ / _kalmanFilter.S(all, all).diagonal().array().abs().sqrt())
2291 ✗ .matrix(),
2292 ✗ _kalmanFilter.z.rowKeys());
2293 ✗ if (normInno(all).hasNaN())
2294 {
2295 ✗ LOG_ERROR("{}: NIS check has NaN values. Skipping check this epoch.", nameId());
2296 LOG_DATA("{}: normInno has NaN", nameId());
2297 LOG_DATA("{}: S =\n{}", nameId(), _kalmanFilter.S);
2298 ✗ return {};
2299 }
2300
2301 LOG_DATA("{}: normInno = \n{}", nameId(), normInno.transposed());
2302 LOG_DATA("{}: dz (post-fit) = \n{}", nameId(), _kalmanFilter.z.transposed());
2303 LOG_DATA("{}: dz (innovation) = \n{}", nameId(), _kalmanFilter.savedPreUpdate().z.transposed());
2304
2305 // Check pivot satellites for outlier by calculating mean / stdev
2306 ✗ OutlierInfo faultyMeas;
2307
2308 ✗ for (auto& [pivotCodeObsType, pivot] : _pivotSatellites)
2309 {
2310 ✗ auto code = pivotCodeObsType.first;
2311 ✗ auto obsType = pivotCodeObsType.second;
2312 ✗ auto filterMeasurements = [&](const Meas::MeasKeyTypes& key) -> bool {
2313 ✗ SatSigId satSigId;
2314 ✗ GnssObs::ObservationType obsType2 = GnssObs::ObservationType_COUNT;
2315 ✗ if (const auto* meas = std::get_if<Meas::PsrDD>(&key))
2316 {
2317 ✗ obsType2 = GnssObs::Pseudorange;
2318 ✗ satSigId = meas->satSigId;
2319 }
2320 ✗ else if (const auto* meas = std::get_if<Meas::CarrierDD>(&key))
2321 {
2322 ✗ obsType2 = GnssObs::Carrier;
2323 ✗ satSigId = meas->satSigId;
2324 }
2325 ✗ else if (const auto* meas = std::get_if<Meas::DopplerDD>(&key))
2326 {
2327 ✗ obsType2 = GnssObs::Doppler;
2328 ✗ satSigId = meas->satSigId;
2329 }
2330 ✗ return satSigId.code != code || obsType2 != obsType;
2331 ✗ };
2332 ✗ double mean = 0.0;
2333 ✗ size_t amount = 0;
2334 ✗ for (const auto& key : _kalmanFilter.savedPreUpdate().z.rowKeys())
2335 {
2336 ✗ if (filterMeasurements(key)) { continue; }
2337 ✗ mean += _kalmanFilter.savedPreUpdate().z(key);
2338 ✗ amount++;
2339 }
2340 ✗ if (amount <= 1) { continue; }
2341 ✗ mean /= static_cast<double>(amount);
2342
2343 ✗ double variance = 0.0;
2344 ✗ for (const auto& key : _kalmanFilter.savedPreUpdate().z.rowKeys())
2345 {
2346 ✗ if (filterMeasurements(key)) { continue; }
2347 ✗ variance += std::pow(std::abs(_kalmanFilter.savedPreUpdate().z(key) - mean), 2);
2348 }
2349 ✗ variance *= 1.0 / (static_cast<double>(amount) - 1.0);
2350
2351 LOG_DATA("{}: pivot [{}][{}]: {:.3f} / {:.3f} = {:.3f}", nameId(), code, obsType, mean, std::sqrt(variance), mean / std::sqrt(variance));
2352
2353 ✗ if (std::abs(mean / std::sqrt(variance)) > 5.0) // 3 or 5 sigma (but t-distributed https://en.wikipedia.org/wiki/Student%27s_t-distribution)
2354 {
2355 ✗ switch (obsType)
2356 {
2357 ✗ case GnssObs::Pseudorange:
2358 ✗ faultyMeas.key = Meas::PsrDD{ pivot.satSigId };
2359 ✗ break;
2360 ✗ case GnssObs::Carrier:
2361 ✗ faultyMeas.key = Meas::CarrierDD{ pivot.satSigId };
2362 ✗ break;
2363 ✗ case GnssObs::Doppler:
2364 ✗ faultyMeas.key = Meas::DopplerDD{ pivot.satSigId };
2365 ✗ break;
2366 ✗ case GnssObs::ObservationType_COUNT:
2367 ✗ break;
2368 }
2369 ✗ faultyMeas.obsType = obsType;
2370 ✗ faultyMeas.satSigId = pivot.satSigId;
2371 ✗ faultyMeas.pivot = RtkSolution::PivotChange::Reason::PivotOutlier;
2372
2373 ✗ break; // Pivot is problematic throw out and then break. Only one pivot change at a time
2374 }
2375 }
2376 ✗ if (faultyMeas.obsType == GnssObs::ObservationType_COUNT)
2377 {
2378 ✗ int maxIdx = 0;
2379 ✗ normInno(all).maxCoeff(&maxIdx);
2380 ✗ faultyMeas.key = normInno.rowKeys().at(static_cast<size_t>(maxIdx));
2381 LOG_DATA("{}: Largest post-fit innovation: {} ({:.1f})", nameId(), faultyMeas.key, normInno(faultyMeas.key));
2382
2383 ✗ if (const auto* meas = std::get_if<Meas::PsrDD>(&faultyMeas.key))
2384 {
2385 ✗ faultyMeas.obsType = GnssObs::Pseudorange;
2386 ✗ faultyMeas.satSigId = meas->satSigId;
2387 }
2388 ✗ else if (const auto* meas = std::get_if<Meas::CarrierDD>(&faultyMeas.key))
2389 {
2390 ✗ faultyMeas.obsType = GnssObs::Carrier;
2391 ✗ faultyMeas.satSigId = meas->satSigId;
2392 }
2393 ✗ else if (const auto* meas = std::get_if<Meas::DopplerDD>(&faultyMeas.key))
2394 {
2395 ✗ faultyMeas.obsType = GnssObs::Doppler;
2396 ✗ faultyMeas.satSigId = meas->satSigId;
2397 }
2398
2399 ✗ auto& pivot = _pivotSatellites.at({ faultyMeas.satSigId.code, faultyMeas.obsType });
2400 LOG_DATA("{}: Pivot for [{}] is [{}]", nameId(), faultyMeas.satSigId, pivot.satSigId);
2401 ✗ if (pivot.satSigId == faultyMeas.satSigId)
2402 {
2403 ✗ faultyMeas.pivot = RtkSolution::PivotChange::Reason::PivotOutlier;
2404 LOG_DATA("{}: Pivot [{}][{}] with outlier.", nameId(), pivot.satSigId, faultyMeas.obsType);
2405 }
2406 }
2407
2408 LOG_DATA("{}: faultyMeas = {}", nameId(), faultyMeas.key);
2409
2410 ✗ if (faultyMeas.obsType == GnssObs::Pseudorange && _outlierMinPsrObsKeep != 0 && observations.nObservables[GnssObs::Pseudorange] <= _outlierMinPsrObsKeep)
2411 {
2412 ✗ addEventToGui(rtkSol, "Stopped doing NIS check, as minimum amount of pseudorange observables reached.");
2413 ✗ return {};
2414 }
2415
2416 ✗ std::vector<RealTimeKinematic::OutlierInfo> faulty{ faultyMeas };
2417 // Exclude Pseudorange and Carrier toghether
2418 ✗ if (faultyMeas.obsType == GnssObs::Pseudorange && _kalmanFilter.z.hasRow(Meas::CarrierDD{ faultyMeas.satSigId }))
2419 {
2420 LOG_DATA("{}: Also flagging [{}][{}] as outlier.", nameId(), faultyMeas.satSigId, GnssObs::Carrier);
2421 ✗ faulty.push_back(faultyMeas);
2422 ✗ faulty.back().obsType = GnssObs::Carrier;
2423 ✗ faulty.back().key = Meas::CarrierDD{ faulty.back().satSigId };
2424 }
2425 ✗ else if (faultyMeas.obsType == GnssObs::Carrier && _kalmanFilter.z.hasRow(Meas::PsrDD{ faultyMeas.satSigId }))
2426 {
2427 LOG_DATA("{}: Also flagging [{}][{}] as outlier.", nameId(), faultyMeas.satSigId, GnssObs::Pseudorange);
2428 ✗ faulty.push_back(faultyMeas);
2429 ✗ faulty.back().obsType = GnssObs::Pseudorange;
2430 ✗ faulty.back().key = Meas::PsrDD{ faulty.back().satSigId };
2431 }
2432 ✗ if (faulty.size() == 2)
2433 {
2434 ✗ if (auto pivotKey = std::make_pair(faulty.back().satSigId.code, faulty.back().obsType);
2435 ✗ _pivotSatellites.contains(pivotKey) && _pivotSatellites.at(pivotKey).satSigId == faulty.back().satSigId)
2436 {
2437 ✗ faulty.back().pivot = RtkSolution::PivotChange::Reason::PivotOutlier;
2438 }
2439 else
2440 {
2441 ✗ faulty.back().pivot.reset();
2442 }
2443 }
2444
2445 ✗ printPivotSatellites();
2446 ✗ for (const auto& faultyMeas : faulty)
2447 {
2448 LOG_DATA("{}: Erasing receiver observation [{}][{}], because outlier (NIS check).", nameId(), faultyMeas.satSigId, faultyMeas.obsType);
2449
2450 ✗ if (faultyMeas.pivot)
2451 {
2452 LOG_DATA("{}: Removing pivot satellite [{}][{}] because outlier detected.", nameId(), faultyMeas.satSigId, faultyMeas.obsType);
2453 ✗ updatePivotSatellite(faultyMeas.satSigId.code, faultyMeas.obsType, observations, rtkSol, faultyMeas.pivot.value());
2454 }
2455 else
2456 {
2457 ✗ if (States::AmbiguityDD ambStateKey{ faultyMeas.satSigId };
2458 ✗ faultyMeas.obsType == GnssObs::Carrier && _kalmanFilter.hasState(ambStateKey))
2459 {
2460 LOG_TRACE("{}: Removing state [{}]", nameId(), ambStateKey);
2461 ✗ _kalmanFilter.removeState(ambStateKey);
2462 ✗ _ambiguitiesHold.erase(faultyMeas.satSigId);
2463 ✗ if (_outputStateEvents)
2464 {
2465 ✗ addEventToGui(rtkSol, fmt::format("State [{}] removed (because outlier detected in carrier measurement [{}])", ambStateKey, faultyMeas.key));
2466 }
2467 }
2468 ✗ _kalmanFilter.removeMeasurement(faultyMeas.key);
2469
2470 LOG_DATA("{}: x = \n{}", nameId(), _kalmanFilter.x.transposed());
2471 LOG_DATA("{}: z = \n{}", nameId(), _kalmanFilter.z.transposed());
2472 }
2473
2474 ✗ observations.removeSignal(faultyMeas.satSigId, faultyMeas.obsType, nameId());
2475 ✗ _obsFilter.excludeSignalTemporarily(faultyMeas.satSigId, _outlierRemoveEpochs - 1);
2476 ✗ addEventToGui(rtkSol, fmt::format("Erasing receiver observation [{}][{}], because outlier (NIS check)", faultyMeas.satSigId, faultyMeas.obsType));
2477
2478 ✗ _nMeasExcludedNIS++;
2479 ✗ rtkSol->nisRemovedCnt++;
2480 ✗ rtkSol->outliers.emplace_back(RtkSolution::Outlier::Type::NIS, faultyMeas.satSigId, faultyMeas.obsType);
2481 }
2482 ✗ return faulty;
2483 ✗ }
2484
2485 601 RealTimeKinematic::UpdateStatus RealTimeKinematic::kalmanFilterUpdate(const std::shared_ptr<RtkSolution>& rtkSol)
2486 {
2487 601 UpdateStatus solutionValid;
2488
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 601 times.
 601 if (_kalmanFilter.z.rows() == 0)
2489 {
2490 ✗ solutionValid.valid = false;
2491 }
2492 else
2493 {
2494
1/2
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
 601 auto dt = static_cast<double>((_receiver[Rover].gnssObs->insTime - _lastUpdate).count());
2495 ✗ solutionValid.threshold = std::max({ 50.0,
2496
1/2
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
 601 10.0 * _receiver[Rover].e_vel.norm() * dt,
2497
5/10
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 601 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 601 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 601 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 601 times.
✗ Branch 16 not taken.
 601 _kalmanFilter.P(States::Pos, States::Pos).diagonal().cwiseSqrt().maxCoeff() });
2498
2499 601 _kalmanFilter.savePreUpdate();
2500 601 _kalmanFilter.correctWithMeasurementInnovation();
2501
2502 LOG_DATA("{}: x (a posteriori, t = {}) =\n{}", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), _kalmanFilter.x.transposed());
2503 LOG_DATA("{}: P (a posteriori, t = {}) =\n{}", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), _kalmanFilter.P);
2504
2505 LOG_DATA("{}: dx_ecef (a posteriori - a priori ) = {}", nameId(), (_kalmanFilter.x.segment<3>(States::Pos) - _receiver[Rover].e_posMarker).transpose());
2506 LOG_DATA("{}: dv_ecef (a posteriori - a priori ) = {}", nameId(), (_kalmanFilter.x.segment<3>(States::Vel) - _receiver[Rover].e_vel).transpose());
2507
2508
3/6
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 601 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 601 times.
✗ Branch 10 not taken.
 601 solutionValid.dx = (_kalmanFilter.x.segment<3>(States::Pos) - _receiver[Rover].e_posMarker).norm();
2509 601 solutionValid.valid = solutionValid.dx < solutionValid.threshold;
2510 }
2511
2512
1/2
✓ Branch 0 taken 601 times.
✗ Branch 1 not taken.
 601 if (solutionValid.valid)
2513 {
2514
2/4
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 601 times.
✗ Branch 7 not taken.
 601 _receiver[Rover].e_posMarker = _kalmanFilter.x.segment<3>(States::Pos);
2515
2/4
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 601 times.
✗ Branch 7 not taken.
 601 _receiver[Rover].e_vel = _kalmanFilter.x.segment<3>(States::Vel);
2516
1/2
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
 601 _receiver[Rover].lla_posMarker = trafo::ecef2lla_WGS84(_receiver[Rover].e_posMarker);
2517
2518
1/2
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
 601 if (_kalmanFilter.isPreUpdateSaved()) { _kalmanFilter.discardPreUpdate(); }
2519 }
2520 else
2521 {
2522 ✗ std::string text = _kalmanFilter.z.rows() == 0
2523 ✗ ? std::string("Update rejected - no observations left")
2524 ✗ : fmt::format("Update rejected - position change due to update is {:.2f} [m] > {:.2f} [m]", solutionValid.dx, solutionValid.threshold);
2525
2526 ✗ LOG_WARN("{}: [{}] {}", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), text);
2527 ✗ addEventToGui(rtkSol, text);
2528 ✗ if (_kalmanFilter.isPreUpdateSaved()) { _kalmanFilter.restorePreUpdate(); }
2529 ✗ }
2530
2531 601 return solutionValid;
2532 }
2533
2534 601 bool RealTimeKinematic::resolveAmbiguities(size_t nCarrierMeasUniqueSatellite, const std::shared_ptr<RtkSolution>& rtkSol)
2535 {
2536 601 std::vector<States::StateKeyType> ambKeys;
2537 601 std::vector<Meas::MeasKeyTypes> ambMeasKeys;
2538
2/2
✓ Branch 2 taken 23579 times.
✓ Branch 3 taken 601 times.
 24180 for (size_t i = States::KFStates_COUNT; i < _kalmanFilter.x.rowKeys().size(); i++) // 0-2 Pos, 3-5 Vel
2539 {
2540
2/4
✓ Branch 2 taken 23579 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23579 times.
✗ Branch 6 not taken.
 23579 if (const auto* ambDD = std::get_if<States::AmbiguityDD>(&_kalmanFilter.x.rowKeys().at(i)))
2541 {
2542
1/2
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
 23579 ambKeys.emplace_back(*ambDD);
2543
1/2
✓ Branch 1 taken 23579 times.
✗ Branch 2 not taken.
 23579 ambMeasKeys.emplace_back(*ambDD);
2544 }
2545 }
2546 601 size_t nAmbs = ambKeys.size();
2547
2548
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 601 times.
 601 if (nCarrierMeasUniqueSatellite < _nMinSatForAmbFix)
2549 {
2550 LOG_TRACE("{}: [{}] Not fixing ambiguities because only {} satellites but minimum {} needed.",
2551 nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), nCarrierMeasUniqueSatellite, _nMinSatForAmbFix);
2552 ✗ if (!_nMinSatForAmbFixTriggered)
2553 {
2554 ✗ addEventToGui(rtkSol, fmt::format("Not fixing ambiguities anymore because\nonly {} satellites but minimum {} needed.", nCarrierMeasUniqueSatellite, _nMinSatForAmbFix));
2555 }
2556 ✗ _nMinSatForAmbFixTriggered = true;
2557
2558 ✗ return false;
2559 }
2560
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 601 times.
 601 if (_nMinSatForAmbFixTriggered)
2561 {
2562 ✗ _nMinSatForAmbFixTriggered = false;
2563 ✗ addEventToGui(rtkSol, fmt::format("Resuming ambiguity fixing as\nminimum amount of satellites reached."));
2564 }
2565
2566
3/6
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 601 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 601 times.
✗ Branch 10 not taken.
 601 if (double posVar = _kalmanFilter.P(States::Pos, States::Pos).diagonal().sum() / 3.0;
2567
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 601 times.
 601 posVar > _maxPosVar)
2568 {
2569 LOG_TRACE("{}: [{}] Not fixing ambiguities because position variance is {:.4f}m which is higher than {:.4f}m",
2570 nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), posVar, _maxPosVar);
2571 ✗ if (!_maxPosVarTriggered)
2572 {
2573 ✗ addEventToGui(rtkSol, fmt::format("Not fixing ambiguities anymore because\nposition variance is {:.4f}m^2 which is higher than {:.4f}m^2.", posVar, _maxPosVar));
2574 }
2575 ✗ _maxPosVarTriggered = true;
2576 ✗ return false;
2577 }
2578
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 601 times.
 601 else if (_maxPosVarTriggered) // NOLINT(readability-else-after-return,llvm-else-after-return)
2579 {
2580 ✗ _maxPosVarTriggered = false;
2581 ✗ addEventToGui(rtkSol, fmt::format("Resuming ambiguity fixing as\nposition variance smaller than limit."));
2582 }
2583
2584
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 601 times.
 601 if (ambKeys.empty()) { return false; } // No ambiguities to estimate
2585 1202 if (nCarrierMeasUniqueSatellite >= _nMinSatForAmbHold
2586
6/8
✓ Branch 0 taken 601 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 598 times.
✓ Branch 7 taken 3 times.
✓ Branch 8 taken 598 times.
✓ Branch 9 taken 3 times.
 601 && _ambiguityResolutionStrategy == AmbiguityResolutionStrategy::FixAndHold && ambKeys.size() == _ambiguitiesHold.size()) // No new ambiguities to estimate
2587 {
2588 #if LOG_LEVEL <= LOG_LEVEL_DATA
2589 {
2590 std::set<SatSigId> ambStates;
2591 for (const auto& key : ambKeys)
2592 {
2593 auto amb = std::get<States::AmbiguityDD>(key);
2594 if (ambStates.contains(amb.satSigId))
2595 {
2596 LOG_CRITICAL("{}: The KF state has [{}] more than once.", nameId(), amb);
2597 }
2598 ambStates.insert(amb.satSigId);
2599 }
2600 LOG_DATA("{}: ambKeys: [{}]", nameId(), fmt::join(ambStates, ", "));
2601
2602 std::set<SatSigId> ambHold;
2603 for (const auto& hold : _ambiguitiesHold)
2604 {
2605 if (ambHold.contains(hold.first))
2606 {
2607 LOG_CRITICAL("{}: _ambiguitiesHold has [{}] more than once.", nameId(), hold.first);
2608 }
2609 ambHold.insert(hold.first);
2610 }
2611 LOG_DATA("{}: ambHold: [{}]", nameId(), fmt::join(ambHold, ", "));
2612
2613 if (ambStates != ambHold)
2614 {
2615 std::vector<SatSigId> diff;
2616 std::set_difference(ambStates.begin(), ambStates.end(), ambHold.begin(), ambHold.end(), std::back_inserter(diff)); // NOLINT(boost-use-ranges,modernize-use-ranges) // There is no ranges::set_difference
2617 LOG_DATA("{}: Amb not in hold: [{}]", nameId(), fmt::join(diff, ", "));
2618 diff.clear();
2619 std::set_difference(ambHold.begin(), ambHold.end(), ambStates.begin(), ambStates.end(), std::back_inserter(diff)); // NOLINT(boost-use-ranges,modernize-use-ranges) // There is no ranges::set_difference
2620 LOG_DATA("{}: Amb not in states: [{}]", nameId(), fmt::join(diff, ", "));
2621 }
2622 }
2623 #endif
2624
2/4
✓ Branch 2 taken 598 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 598 times.
✗ Branch 6 not taken.
 598 Eigen::VectorXd fixedAmb = Eigen::VectorXd::Zero(static_cast<int>(ambKeys.size()));
2625 // Apply exact integers (after update it is still float in the late digits)
2626
2/2
✓ Branch 1 taken 23462 times.
✓ Branch 2 taken 598 times.
 24060 for (size_t k = 0; k < ambKeys.size(); k++)
2627 {
2628
1/2
✓ Branch 1 taken 23462 times.
✗ Branch 2 not taken.
 23462 const auto& key = ambKeys.at(k);
2629
1/2
✓ Branch 1 taken 23462 times.
✗ Branch 2 not taken.
 23462 auto satSigId = std::get<States::AmbiguityDD>(key).satSigId;
2630 LOG_DATA("{}: FixAndHold: [{}] Holding {} to {} (after update)", nameId(), satSigId, _kalmanFilter.x.hasRow(key), _ambiguitiesHold.contains(satSigId));
2631 LOG_DATA("{}: FixAndHold: [{}] Holding {} to {} (after update)", nameId(), satSigId, _kalmanFilter.x(key), _ambiguitiesHold.at(satSigId));
2632
2/4
✓ Branch 1 taken 23462 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23462 times.
✗ Branch 5 not taken.
 23462 fixedAmb(static_cast<int>(k)) = _ambiguitiesHold.at(satSigId);
2633 }
2634 598 rtkSol->nAmbiguitiesFixed = ambKeys.size() + 1; // + 1 to also count the pivot
2635
1/2
✓ Branch 1 taken 598 times.
✗ Branch 2 not taken.
 598 applyFixedAmbiguities(fixedAmb, ambKeys, ambMeasKeys);
2636
2637
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 598 times.
 598 if (_nMinSatForAmbHoldTriggered)
2638 {
2639 ✗ _nMinSatForAmbHoldTriggered = false;
2640 ✗ addEventToGui(rtkSol, fmt::format("Resuming ambiguity holding as\nminimum amount of satellites reached."));
2641 }
2642
2643 598 return true;
2644 598 }
2645
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 3 if (nCarrierMeasUniqueSatellite < _nMinSatForAmbHold && _ambiguityResolutionStrategy == AmbiguityResolutionStrategy::FixAndHold)
2646 {
2647 LOG_TRACE("{}: [{}] Not holding ambiguities because only {} satellites but minimum {} needed.",
2648 nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), nCarrierMeasUniqueSatellite, _nMinSatForAmbHold);
2649 ✗ if (!_nMinSatForAmbHoldTriggered)
2650 {
2651 ✗ addEventToGui(rtkSol, fmt::format("Not holding ambiguities anymore because\nonly {} satellites but minimum {} needed.", nCarrierMeasUniqueSatellite, _nMinSatForAmbHold));
2652 }
2653 ✗ _nMinSatForAmbHoldTriggered = true;
2654 }
2655
2656 LOG_DATA("{}: [{}] Estimating ambiguities", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST));
2657
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 auto floatKeys = States::PosVel;
2658 3 size_t partialFixTries = 0;
2659 do {
2660 LOG_DATA("{}: [{}] floatKeys = {}", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), fmt::join(floatKeys, ", "));
2661 LOG_DATA("{}: [{}] ambKeys = {}", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), fmt::join(ambKeys, ", "));
2662
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 6 auto fixed = ResolveAmbiguities(_kalmanFilter.x(ambKeys), _kalmanFilter.P(ambKeys, ambKeys),
2663
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 6 _kalmanFilter.x(floatKeys), _kalmanFilter.P(floatKeys, floatKeys),
2664
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 6 _kalmanFilter.P(ambKeys, floatKeys), _kalmanFilter.P(floatKeys, ambKeys),
2665
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 9 _ambiguityResolutionParameters, nameId());
2666 3 rtkSol->ambiguityResolutionFailure = fixed.failure;
2667 3 rtkSol->ambiguityCriticalValueRatio = fixed.ambiguityCriticalValueRatio;
2668
2669
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 3 if (fixed.failure == AmbiguityResolutionFailure::None)
2670 {
2671
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 _kalmanFilter.x(floatKeys) = fixed.b;
2672 LOG_DATA("{}: x(fixed.b) =\n{}", nameId(), _kalmanFilter.x.transposed());
2673
2674
2/4
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 if (_applyFixedAmbiguitiesWithUpdate) { applyFixedAmbiguities(fixed.fixedAmb.front().a, ambKeys, ambMeasKeys); }
2675 else
2676 {
2677 ✗ _kalmanFilter.x(ambKeys) = fixed.fixedAmb.front().a;
2678 // _kalmanFilter.P(floatKeys, floatKeys) = fixed.Qb;
2679 // LOG_DATA("{}: P(fixed.Qb) =\n{}", nameId(), _kalmanFilter.P);
2680 }
2681
2682 LOG_DATA("{}: dx_ecef (fix, update - fix ) = {}", nameId(), (_kalmanFilter.x.segment<3>(States::Pos) - _receiver[Rover].e_posMarker).transpose());
2683 LOG_DATA("{}: dv_ecef (fix, update - fix ) = {}", nameId(), (_kalmanFilter.x.segment<3>(States::Vel) - _receiver[Rover].e_vel).transpose());
2684
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 _receiver[Rover].e_posMarker = _kalmanFilter.x.segment<3>(States::Pos);
2685
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 _receiver[Rover].e_vel = _kalmanFilter.x.segment<3>(States::Vel);
2686
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 _receiver[Rover].lla_posMarker = trafo::ecef2lla_WGS84(_receiver[Rover].e_posMarker);
2687
2688 3 rtkSol->ambiguityCriticalValueRatio = fixed.ambiguityCriticalValueRatio;
2689 3 rtkSol->nAmbiguitiesFixed = fixed.nFixed + 1; // + 1 to also count the pivot
2690
2691
2/2
✓ Branch 4 taken 117 times.
✓ Branch 5 taken 3 times.
 120 for (const auto& key_ : ambKeys)
2692 {
2693
1/2
✓ Branch 1 taken 117 times.
✗ Branch 2 not taken.
 117 const auto key = std::get<States::AmbiguityDD>(key_);
2694
2695 234 if (_ambiguityResolutionStrategy == AmbiguityResolutionStrategy::FixAndHold
2696
3/4
✓ Branch 1 taken 117 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 73 times.
✓ Branch 4 taken 44 times.
 117 && _ambiguitiesHold.contains(key.satSigId)
2697
3/6
✓ Branch 2 taken 73 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 73 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 73 times.
 73 && std::abs(_kalmanFilter.x(key) - _ambiguitiesHold.at(key.satSigId)) > 0.1
2698
2/8
✓ Branch 0 taken 117 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 117 times.
 234 && fixed.nFixed == nAmbs && partialFixTries == 0)
2699 {
2700 ✗ LOG_WARN("{}: Ambiguity [{}] changed from {} to {} (despite being FixAndHold)", nameId(), key.satSigId,
2701 _kalmanFilter.x(key), _ambiguitiesHold.at(key.satSigId));
2702 }
2703
2/4
✓ Branch 2 taken 117 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 117 times.
✗ Branch 6 not taken.
 117 _ambiguitiesHold[key.satSigId] = _kalmanFilter.x(key);
2704 }
2705
2706 3 return partialFixTries == 0;
2707 }
2708 LOG_DATA("{}: Fixing failed. partialFixTries = {}", nameId(), partialFixTries);
2709
2710 ✗ if (_ambiguityResolutionParameters.partialFixing)
2711 {
2712 ✗ int maxAmb = 0;
2713 ✗ _kalmanFilter.P(ambKeys, ambKeys).diagonal().maxCoeff(&maxAmb);
2714
2715 LOG_DATA("{}: [{}] Trying partial fixing by not fixing [{}] with highest variance of {}",
2716 nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST),
2717 ambKeys.at(static_cast<size_t>(maxAmb)),
2718 _kalmanFilter.P(ambKeys.at(static_cast<size_t>(maxAmb)), ambKeys.at(static_cast<size_t>(maxAmb))));
2719 ✗ floatKeys.push_back(ambKeys.at(static_cast<size_t>(maxAmb)));
2720 ✗ ambKeys.erase(std::next(ambKeys.begin(), maxAmb));
2721 ✗ ambMeasKeys.erase(std::next(ambMeasKeys.begin(), maxAmb));
2722 ✗ partialFixTries++;
2723 }
2724
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 3 } while (_ambiguityResolutionParameters.partialFixing && ambKeys.size() > 1 && partialFixTries < 6);
2725
2726 ✗ if (_ambiguityResolutionParameters.searchAlgorithm != AmbiguityResolutionParameters::SearchAlgorithm::None)
2727 {
2728 LOG_TRACE("{}: [{}] Ambiguity resolution failed", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST));
2729 ✗ _ambiguitiesHold.clear();
2730 }
2731
2732 ✗ return false;
2733 601 }
2734
2735 601 void RealTimeKinematic::applyFixedAmbiguities(const Eigen::VectorXd& fixedAmb, const std::vector<States::StateKeyType>& ambKeys, const std::vector<Meas::MeasKeyTypes>& ambMeasKeys)
2736 {
2737 #if LOG_LEVEL <= LOG_LEVEL_DATA
2738 if (((_kalmanFilter.x(ambKeys).array() * 1e2).round() * 1e-2).matrix() != fixedAmb)
2739 {
2740 auto ambPrint = KeyedMatrixXd<States::StateKeyType>(
2741 (Eigen::MatrixXd(static_cast<int>(ambKeys.size()), 2) << _kalmanFilter.x(ambKeys), fixedAmb).finished(),
2742 ambKeys, std::vector<States::StateKeyType>{ States::AmbiguityDD(SatSigId(Code::G1C, 200)), States::AmbiguityDD(SatSigId(Code::G1C, 201)) });
2743 LOG_DATA("{}: [{}] Ambiguity estimate changed (relative to pivot) (200 = prev, 201 = new)\n{}", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), ambPrint.transposed());
2744 LOG_DATA("P(amb) =\n{}", _kalmanFilter.P(ambKeys, ambKeys));
2745 }
2746 #endif
2747
2748 // Make a KF update with the fixed ambiguities as observation in order to adapt the P matrix
2749
1/2
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
 601 _kalmanFilter.setMeasurements(ambMeasKeys);
2750
2751 // Update the Measurement sensitivity Matrix (𝐇), the Measurement noise covariance matrix (𝐑) and the Measurement vector (𝐳)
2752
4/8
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 601 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 601 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 601 times.
✗ Branch 13 not taken.
 601 _kalmanFilter.z.segment(ambMeasKeys) = fixedAmb - _kalmanFilter.x(ambKeys);
2753
2/4
✓ Branch 3 taken 601 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 601 times.
✗ Branch 7 not taken.
 601 _kalmanFilter.H(ambMeasKeys, ambKeys).setIdentity();
2754
2755 // R matrix
2756
2/4
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 601 times.
✗ Branch 6 not taken.
 601 _kalmanFilter.R(all, all).diagonal().setConstant(_ambFixUpdateVariance);
2757
2758 LOG_DATA("{}: z =\n{}", nameId(), _kalmanFilter.z.transposed());
2759 LOG_DATA("{}: H =\n{}", nameId(), _kalmanFilter.H);
2760 LOG_DATA("{}: R =\n{}", nameId(), _kalmanFilter.R);
2761
2762 601 _kalmanFilter.correctWithMeasurementInnovation();
2763
2764 LOG_DATA("{}: x (fix, update , t = {}) = (Ambiguity update)\n{}", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), _kalmanFilter.x.transposed());
2765 // Apply the integer values, otherwise values not exact
2766
2/4
✓ Branch 2 taken 601 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 601 times.
✗ Branch 6 not taken.
 601 _kalmanFilter.x(ambKeys) = fixedAmb;
2767 LOG_DATA("{}: x (fix, update , t = {}) = (Ambiguity integers)\n{}", nameId(), _receiver[Rover].gnssObs->insTime.toYMDHMS(GPST), _kalmanFilter.x.transposed());
2768 601 }
2769
2770 1202 const char* to_string(const RealTimeKinematic::ReceiverType& receiver)
2771 {
2772
2/4
✓ Branch 0 taken 601 times.
✓ Branch 1 taken 601 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 1202 switch (receiver)
2773 {
2774 601 case RealTimeKinematic::ReceiverType::Base:
2775 601 return "Base";
2776 601 case RealTimeKinematic::ReceiverType::Rover:
2777 601 return "Rover";
2778 ✗ case RealTimeKinematic::ReceiverType::ReceiverType_COUNT:
2779 ✗ return "";
2780 }
2781 ✗ return "";
2782 }
2783
2784 } // namespace NAV
2785