INSTINCT Code Coverage Report

Directory: src/
File: Navigation/GNSS/Ambiguity/CycleSlipDetector.cpp
Date: 2025-07-19 10:51:51
Exec Total Coverage
Lines: 59 164 36.0%
Functions: 5 12 41.7%
Branches: 52 290 17.9%
Line Branch Exec Source
1 // This file is part of INSTINCT, the INS Toolkit for Integrated
2 // Navigation Concepts and Training by the Institute of Navigation of
3 // the University of Stuttgart, Germany.
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9 /// @file CycleSlipDetector.cpp
10 /// @brief Combination of different cycle-slip detection algorithms
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2023-11-10
13
14 #include "CycleSlipDetector.hpp"
15
16 #include "Navigation/Constants.hpp"
17 #include "util/Logger.hpp"
18 #include "internal/gui/widgets/HelpMarker.hpp"
19 #include <fmt/core.h>
20
21 namespace NAV
22 {
23
24 1202 std::vector<CycleSlipDetector::Result> CycleSlipDetector::checkForCycleSlip(InsTime insTime, const std::vector<SatelliteObservation>& satObs, [[maybe_unused]] const std::string& nameId)
25 {
26 1202 std::vector<Result> cycleSlips;
27 1202 std::vector<SatSigId> resetThisEpoch;
28
29 78760 auto searchCycleSlipAlreadyFound = [&cycleSlips](const SatSigId& satSigId) {
30 78760 return std::ranges::find_if(cycleSlips, [&](const Result& cycleSlip) {
31 ✗ if (const auto* s = std::get_if<CycleSlipLossOfLockIndicator>(&cycleSlip)) { return s->signal == satSigId; }
32 ✗ if (const auto* s = std::get_if<CycleSlipSingleFrequency>(&cycleSlip)) { return s->signal == satSigId; }
33 ✗ return false;
34 78760 });
35 1202 };
36
37
2/2
✓ Branch 5 taken 16120 times.
✓ Branch 6 taken 1202 times.
 17322 for (const auto& obs : satObs)
38 {
39 16120 std::string signals;
40
2/2
✓ Branch 4 taken 55572 times.
✓ Branch 5 taken 16120 times.
 71692 for (const auto& signal : obs.signals)
41 {
42
3/4
✓ Branch 1 taken 39452 times.
✓ Branch 2 taken 16120 times.
✓ Branch 4 taken 39452 times.
✗ Branch 5 not taken.
 55572 if (!signals.empty()) { signals += ", "; }
43
2/4
✓ Branch 2 taken 55572 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 55572 times.
✗ Branch 6 not taken.
 55572 signals += fmt::format("{}", SatSigId(signal.code, obs.satId.satNum));
44 }
45 LOG_DATA("{}: [{}] {}", nameId, insTime.toYMDHMS(GPST), signals);
46 16120 }
47
48
2/2
✓ Branch 5 taken 16120 times.
✓ Branch 6 taken 1202 times.
 17322 for (const auto& obs : satObs)
49 {
50 LOG_DATA("{}: [{}] Checking [{}]", nameId, insTime.toYMDHMS(GPST), obs.satId);
51
2/2
✓ Branch 5 taken 55572 times.
✓ Branch 6 taken 16120 times.
 71692 for (const auto& signal : obs.signals)
52 {
53 55572 auto satSigId = SatSigId(signal.code, obs.satId.satNum);
54
1/2
✓ Branch 0 taken 55572 times.
✗ Branch 1 not taken.
 55572 if (_enableLLICheck)
55 {
56
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 55572 times.
 55572 if (signal.measurement.LLI)
57 {
58 LOG_DATA("{}: [{}] Cycle-slip detected, due to LLI set", nameId, satSigId);
59 ✗ cycleSlips.emplace_back(CycleSlipLossOfLockIndicator{ satSigId });
60 ✗ resetSignal(satSigId);
61 }
62 else
63 {
64 LOG_DATA("{}: [{}] LLI check passed", nameId, SatSigId(signal.code, obs.satId.satNum));
65 }
66 }
67
2/4
✓ Branch 1 taken 55572 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 55572 times.
✗ Branch 5 not taken.
 55572 auto lambda = InsConst::C / satSigId.freq().getFrequency(obs.freqNum);
68
69 111144 auto result = _singleFrequencyDetector.checkForCycleSlip(satSigId, insTime,
70 55572 signal.measurement.value * lambda,
71
1/2
✓ Branch 1 taken 55572 times.
✗ Branch 2 not taken.
 55572 lambda * _singleFrequencyThresholdPercentage);
72
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 55572 times.
 55572 if (result == PolynomialCycleSlipDetectorResult::CycleSlip)
73 {
74 ✗ if (searchCycleSlipAlreadyFound(satSigId) == cycleSlips.end())
75 {
76 LOG_DATA("{}: [{}] Cycle-slip detected, due to single frequency check", nameId, satSigId);
77 ✗ cycleSlips.emplace_back(CycleSlipSingleFrequency{ satSigId });
78 ✗ resetThisEpoch.push_back(satSigId);
79 }
80 else
81 {
82 LOG_DATA("{}: [{}] Cycle-slip detected, due to single frequency check, but already found due to LLI.", nameId, satSigId);
83 }
84 }
85 else if (result == PolynomialCycleSlipDetectorResult::NoCycleSlip)
86 {
87 LOG_DATA("{}: [{}] Single Frequency check passed", nameId, satSigId);
88 }
89 else if (result == PolynomialCycleSlipDetectorResult::LessDataThanWindowSize)
90 {
91 LOG_DATA("{}: [{}] Single Frequency check skipped, because not enough data", nameId, satSigId);
92 }
93 }
94
95
1/2
✓ Branch 1 taken 16120 times.
✗ Branch 2 not taken.
 16120 if (obs.signals.size() >= 2)
96 {
97 // Signal with largest frequency, e.g. L1
98
1/2
✓ Branch 1 taken 16120 times.
✗ Branch 2 not taken.
 16120 auto signal1 = std::ranges::max_element(obs.signals, [&obs](const SatelliteObservation::Signal& s1, const SatelliteObservation::Signal& s2) {
99
4/8
✓ Branch 1 taken 39452 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39452 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39452 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 39452 times.
✗ Branch 11 not taken.
 39452 return s1.code.getFrequency().getFrequency(obs.freqNum) < s2.code.getFrequency().getFrequency(obs.freqNum);
100 });
101 // Signal with lower frequency, e.g. L2/L5
102
2/2
✓ Branch 5 taken 55572 times.
✓ Branch 6 taken 16120 times.
 71692 for (const auto& signal2 : obs.signals)
103 {
104
4/6
✓ Branch 1 taken 55572 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 55572 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16120 times.
✓ Branch 9 taken 39452 times.
 55572 if (signal1->code.getFrequency() == signal2.code.getFrequency()) { continue; }
105
106 39452 auto satSigId1 = SatSigId(signal1->code, obs.satId.satNum);
107 39452 auto satSigId2 = SatSigId(signal2.code, obs.satId.satNum);
108
109 39452 auto key = DualFrequencyCombination{ .satId = obs.satId, .sig1 = signal1->code, .sig2 = signal2.code };
110
2/4
✓ Branch 1 taken 39452 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39452 times.
✗ Branch 5 not taken.
 39452 auto lambda1 = InsConst::C / satSigId1.freq().getFrequency(obs.freqNum);
111
2/4
✓ Branch 1 taken 39452 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39452 times.
✗ Branch 5 not taken.
 39452 auto lambda2 = InsConst::C / satSigId2.freq().getFrequency(obs.freqNum);
112
113 118356 auto result = _dualFrequencyDetector.checkForCycleSlip(key, insTime,
114
1/2
✓ Branch 1 taken 39452 times.
✗ Branch 2 not taken.
 39452 signal1->measurement.value * lambda1 - signal2.measurement.value * lambda2,
115 39452 lambda1 * _dualFrequencyThresholdPercentage);
116
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 39452 times.
 39452 if (result == PolynomialCycleSlipDetectorResult::CycleSlip)
117 {
118 ✗ auto sat1Slip = searchCycleSlipAlreadyFound(satSigId1);
119 ✗ auto sat2Slip = searchCycleSlipAlreadyFound(satSigId2);
120 ✗ if ((sat1Slip == cycleSlips.end() && sat2Slip != cycleSlips.end())
121 ✗ || (sat1Slip != cycleSlips.end() && sat2Slip == cycleSlips.end()))
122 {
123 ✗ auto satSigIdSlip = sat1Slip == cycleSlips.end() ? satSigId2 : satSigId1;
124 LOG_DATA("{}: [{} / {}] Cycle-slip detected, due to dual frequency check. But slip was already found in [{}]. Assuming not both slipped.",
125 nameId, satSigId1, satSigId2, satSigIdSlip);
126 ✗ resetSignal(satSigIdSlip);
127 ✗ resetThisEpoch.push_back(satSigIdSlip);
128 ✗ std::erase(resetThisEpoch, satSigIdSlip);
129 ✗ _singleFrequencyDetector.addMeasurement(satSigIdSlip, insTime,
130 ✗ sat1Slip == cycleSlips.end() ? signal2.measurement.value * lambda2 : signal1->measurement.value * lambda1);
131 }
132 ✗ else if (sat1Slip != cycleSlips.end() && sat2Slip != cycleSlips.end())
133 {
134 LOG_DATA("{}: [{} / {}] Cycle-slip detected, due to dual frequency check. But both slips were already found.",
135 nameId, satSigId1, satSigId2);
136 ✗ resetSignal(satSigId1);
137 ✗ resetSignal(satSigId2);
138 ✗ std::erase(resetThisEpoch, satSigId1);
139 ✗ std::erase(resetThisEpoch, satSigId2);
140 ✗ _singleFrequencyDetector.addMeasurement(satSigId1, insTime, signal1->measurement.value * lambda1);
141 ✗ _singleFrequencyDetector.addMeasurement(satSigId2, insTime, signal2.measurement.value * lambda2);
142 }
143 else
144 {
145 LOG_DATA("{}: [{} / {}] Cycle-slip detected, due to dual frequency check", nameId, satSigId1, satSigId2);
146 ✗ cycleSlips.emplace_back(CycleSlipDualFrequency{ satSigId1, satSigId2 });
147 ✗ resetSignal(satSigId1);
148 ✗ resetSignal(satSigId2);
149 ✗ std::erase(resetThisEpoch, satSigId1);
150 ✗ std::erase(resetThisEpoch, satSigId2);
151 ✗ _singleFrequencyDetector.addMeasurement(satSigId1, insTime, signal1->measurement.value * lambda1);
152 ✗ _singleFrequencyDetector.addMeasurement(satSigId2, insTime, signal2.measurement.value * lambda2);
153 }
154 ✗ _dualFrequencyDetector.addMeasurement(key, insTime, signal1->measurement.value * lambda1 - signal2.measurement.value * lambda2);
155 }
156
2/2
✓ Branch 0 taken 39380 times.
✓ Branch 1 taken 72 times.
 39452 else if (result == PolynomialCycleSlipDetectorResult::NoCycleSlip)
157 {
158
1/2
✓ Branch 1 taken 39380 times.
✗ Branch 2 not taken.
 39380 if (auto slip = searchCycleSlipAlreadyFound(satSigId1);
159
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 39380 times.
 39380 slip != cycleSlips.end())
160 {
161 LOG_DATA("{}: [{}] Cycle-slip detected, but dual frequency check could not detect it. Removing previous detection.", nameId, satSigId1);
162 ✗ cycleSlips.erase(slip);
163 ✗ std::erase(resetThisEpoch, satSigId1);
164 }
165
1/2
✓ Branch 1 taken 39380 times.
✗ Branch 2 not taken.
 39380 if (auto slip = searchCycleSlipAlreadyFound(satSigId2);
166
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 39380 times.
 39380 slip != cycleSlips.end())
167 {
168 LOG_DATA("{}: [{}] Cycle-slip detected, but dual frequency check could not detect it. Removing previous detection.", nameId, satSigId2);
169 ✗ cycleSlips.erase(slip);
170 ✗ std::erase(resetThisEpoch, satSigId2);
171 }
172 LOG_DATA("{}: [{} / {}] Dual Frequency check passed", nameId, satSigId1, satSigId2);
173 }
174 else if (result == PolynomialCycleSlipDetectorResult::LessDataThanWindowSize)
175 {
176 LOG_DATA("{}: [{} / {}] Dual Frequency check skipped, because not enough data", nameId, satSigId1, satSigId2);
177 }
178 }
179 }
180
2/2
✓ Branch 5 taken 55572 times.
✓ Branch 6 taken 16120 times.
 71692 for (const auto& signal : obs.signals)
181 {
182 55572 auto satSigId = SatSigId(signal.code, obs.satId.satNum);
183
1/2
✓ Branch 1 taken 55572 times.
✗ Branch 2 not taken.
 55572 auto iter = std::ranges::find(resetThisEpoch, satSigId);
184
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 55572 times.
 55572 if (iter != resetThisEpoch.end())
185 {
186 ✗ resetSignal(satSigId);
187 ✗ auto lambda = InsConst::C / satSigId.freq().getFrequency(obs.freqNum);
188 ✗ _singleFrequencyDetector.addMeasurement(satSigId, insTime, signal.measurement.value * lambda);
189 }
190 }
191 }
192
193 2404 return cycleSlips;
194 1202 }
195
196 6 void CycleSlipDetector::reset()
197 {
198 6 _singleFrequencyDetector.clear();
199 6 _dualFrequencyDetector.clear();
200 6 }
201
202 ✗ void CycleSlipDetector::resetSignal(const SatSigId& satSigId)
203 {
204 ✗ _singleFrequencyDetector.reset(satSigId);
205 ✗ std::erase_if(_dualFrequencyDetector._detectors, [&satSigId](const auto& detector) {
206 ✗ return detector.first.satId == satSigId.toSatId()
207 ✗ && (detector.first.sig1 == satSigId.code || detector.first.sig2 == satSigId.code);
208 });
209 ✗ }
210
211 ✗ bool CycleSlipDetectorGui(const char* label, CycleSlipDetector& cycleSlipDetector, float width, bool dualFrequencyAvailable)
212 {
213 ✗ bool changed = false;
214 ✗ if (ImGui::Checkbox(fmt::format("LLI check##{}", label).c_str(), &cycleSlipDetector._enableLLICheck))
215 {
216 ✗ changed = true;
217 }
218 ✗ ImGui::SameLine();
219 ✗ if (!cycleSlipDetector._singleFrequencyDetector.isEnabled()) { ImGui::PushStyleColor(ImGuiCol_Button, ImGui::GetStyle().Colors[ImGuiCol_TextDisabled]); }
220 ✗ if (ImGui::Button(fmt::format("Single Frequency detector##{}", label).c_str()))
221 {
222 ✗ ImGui::OpenPopup(fmt::format("Single Frequency detector##Popup - {}", label).c_str());
223 }
224 ✗ if (!cycleSlipDetector._singleFrequencyDetector.isEnabled()) { ImGui::PopStyleColor(); }
225
226 ✗ if (ImGui::BeginPopup(fmt::format("Single Frequency detector##Popup - {}", label).c_str()))
227 {
228 ✗ if (PolynomialCycleSlipDetectorGui(fmt::format("Single Frequency detector {}", label).c_str(),
229 ✗ cycleSlipDetector._singleFrequencyDetector, width))
230 {
231 ✗ changed = true;
232 }
233 ✗ ImGui::SetNextItemWidth(width);
234 ✗ if (double val = cycleSlipDetector._singleFrequencyThresholdPercentage * 100.0;
235 ✗ ImGui::DragDouble(fmt::format("Threshold##single {}", label).c_str(), &val, 1.0F,
236 1.0, std::numeric_limits<double>::max(), "%.2f %%"))
237 {
238 ✗ cycleSlipDetector._singleFrequencyThresholdPercentage = val / 100.0;
239 ✗ changed = true;
240 }
241 ✗ ImGui::SameLine();
242 ✗ gui::widgets::HelpMarker("As percentage of the wavelength of the signals used.");
243
244 ✗ ImGui::EndPopup();
245 }
246 ✗ ImGui::SameLine();
247 ✗ if (!dualFrequencyAvailable || !cycleSlipDetector._dualFrequencyDetector.isEnabled()) { ImGui::PushStyleColor(ImGuiCol_Button, ImGui::GetStyle().Colors[ImGuiCol_TextDisabled]); }
248 ✗ if (ImGui::Button(fmt::format("Dual Frequency detector##{}", label).c_str()))
249 {
250 ✗ ImGui::OpenPopup(fmt::format("Dual Frequency detector##Popup - {}", label).c_str());
251 }
252 ✗ if (!dualFrequencyAvailable && ImGui::IsItemHovered()) { ImGui::SetTooltip("Dual frequency not available due to filter settings."); }
253 ✗ if (!dualFrequencyAvailable || !cycleSlipDetector._dualFrequencyDetector.isEnabled()) { ImGui::PopStyleColor(); }
254 ✗ if (ImGui::BeginPopup(fmt::format("Dual Frequency detector##Popup - {}", label).c_str()))
255 {
256 ✗ if (PolynomialCycleSlipDetectorGui(fmt::format("Dual Frequency detector {}", label).c_str(),
257 ✗ cycleSlipDetector._dualFrequencyDetector, width))
258 {
259 ✗ changed = true;
260 }
261 ✗ ImGui::SetNextItemWidth(width);
262 ✗ if (double val = cycleSlipDetector._dualFrequencyThresholdPercentage * 100.0;
263 ✗ ImGui::DragDouble(fmt::format("Threshold##dual {}", label).c_str(), &val, 1.0F,
264 1.0, std::numeric_limits<double>::max(), "%.2f %%"))
265 {
266 ✗ cycleSlipDetector._dualFrequencyThresholdPercentage = val / 100.0;
267 ✗ changed = true;
268 }
269 ✗ ImGui::SameLine();
270 ✗ gui::widgets::HelpMarker("As percentage of the smallest wavelength of the signals used.");
271
272 ✗ ImGui::EndPopup();
273 }
274
275 ✗ return changed;
276 }
277
278 ✗ void to_json(json& j, const CycleSlipDetector& data)
279 {
280 ✗ j = json{
281 ✗ { "enableLLICheck", data._enableLLICheck },
282 ✗ { "singleFrequencyThresholdPercentage", data._singleFrequencyThresholdPercentage },
283 ✗ { "dualFrequencyThresholdPercentage", data._dualFrequencyThresholdPercentage },
284 ✗ { "singleFrequencyDetector", data._singleFrequencyDetector },
285 ✗ { "dualFrequencyDetector", data._dualFrequencyDetector },
286 ✗ };
287 ✗ }
288 2 void from_json(const json& j, CycleSlipDetector& data)
289 {
290
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("enableLLICheck")) { j.at("enableLLICheck").get_to(data._enableLLICheck); }
291
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("singleFrequencyThresholdPercentage")) { j.at("singleFrequencyThresholdPercentage").get_to(data._singleFrequencyThresholdPercentage); }
292
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("dualFrequencyThresholdPercentage")) { j.at("dualFrequencyThresholdPercentage").get_to(data._dualFrequencyThresholdPercentage); }
293
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("singleFrequencyDetector")) { j.at("singleFrequencyDetector").get_to(data._singleFrequencyDetector); }
294
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("dualFrequencyDetector")) { j.at("dualFrequencyDetector").get_to(data._dualFrequencyDetector); }
295 2 }
296
297 ✗ std::string to_string(const CycleSlipDetector::Result& cycleSlip)
298 {
299 ✗ if (const auto* s = std::get_if<CycleSlipDetector::CycleSlipLossOfLockIndicator>(&cycleSlip))
300 {
301 ✗ return fmt::format("Cycle-slip [{}] ({})", s->signal, "LLI set");
302 }
303 ✗ if (const auto* s = std::get_if<CycleSlipDetector::CycleSlipSingleFrequency>(&cycleSlip))
304 {
305 ✗ return fmt::format("Cycle-slip [{}] ({})", s->signal, "single frequency check");
306 }
307 ✗ if (const auto* s = std::get_if<CycleSlipDetector::CycleSlipDualFrequency>(&cycleSlip))
308 {
309 ✗ return fmt::format("Cycle-slip [{}] & [{}] ({})", s->signals[0], s->signals[1], "dual frequency check");
310 }
311
312 ✗ return "";
313 }
314
315 } // namespace NAV
316
317 ✗ std::ostream& operator<<(std::ostream& os, const NAV::CycleSlipDetector::Result& obj)
318 {
319 ✗ return os << fmt::format("{}", obj);
320 }
321