INSTINCT Code Coverage Report

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		✗	std::vector<CycleSlipDetector::Result> CycleSlipDetector::checkForCycleSlip(InsTime insTime, const std::vector<SatelliteObservation>& satObs, [[maybe_unused]] const std::string& nameId)
25			{
26		✗	std::vector<Result> cycleSlips;
27		✗	std::vector<SatSigId> resetThisEpoch;
28
29		✗	auto searchCycleSlipAlreadyFound = [&cycleSlips](const SatSigId& satSigId) {
30		✗	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		✗	});
35		✗	};
36
37		✗	for (const auto& obs : satObs)
38			{
39		✗	std::string signals;
40		✗	for (const auto& signal : obs.signals)
41			{
42		✗	if (!signals.empty()) { signals += ", "; }
43		✗	signals += fmt::format("{}", SatSigId(signal.code, obs.satId.satNum));
44			}
45			LOG_DATA("{}: [{}] {}", nameId, insTime.toYMDHMS(GPST), signals);
46		✗	}
47
48		✗	for (const auto& obs : satObs)
49			{
50			LOG_DATA("{}: [{}] Checking [{}]", nameId, insTime.toYMDHMS(GPST), obs.satId);
51		✗	for (const auto& signal : obs.signals)
52			{
53		✗	auto satSigId = SatSigId(signal.code, obs.satId.satNum);
54		✗	if (_enableLLICheck)
55			{
56		✗	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		✗	auto lambda = InsConst::C / satSigId.freq().getFrequency(obs.freqNum);
68
69		✗	auto result = _singleFrequencyDetector.checkForCycleSlip(satSigId, insTime,
70		✗	signal.measurement.value * lambda,
71		✗	lambda * _singleFrequencyThresholdPercentage);
72		✗	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		✗	if (obs.signals.size() >= 2)
96			{
97			// Signal with largest frequency, e.g. L1
98		✗	auto signal1 = std::ranges::max_element(obs.signals, [&obs](const SatelliteObservation::Signal& s1, const SatelliteObservation::Signal& s2) {
99		✗	return s1.code.getFrequency().getFrequency(obs.freqNum) < s2.code.getFrequency().getFrequency(obs.freqNum);
100			});
101			// Signal with lower frequency, e.g. L2/L5
102		✗	for (const auto& signal2 : obs.signals)
103			{
104		✗	if (signal1->code.getFrequency() == signal2.code.getFrequency()) { continue; }
105
106		✗	auto satSigId1 = SatSigId(signal1->code, obs.satId.satNum);
107		✗	auto satSigId2 = SatSigId(signal2.code, obs.satId.satNum);
108
109		✗	auto key = DualFrequencyCombination{ .satId = obs.satId, .sig1 = signal1->code, .sig2 = signal2.code };
110		✗	auto lambda1 = InsConst::C / satSigId1.freq().getFrequency(obs.freqNum);
111		✗	auto lambda2 = InsConst::C / satSigId2.freq().getFrequency(obs.freqNum);
112
113		✗	auto result = _dualFrequencyDetector.checkForCycleSlip(key, insTime,
114		✗	signal1->measurement.value * lambda1 - signal2.measurement.value * lambda2,
115		✗	lambda1 * _dualFrequencyThresholdPercentage);
116		✗	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		✗	else if (result == PolynomialCycleSlipDetectorResult::NoCycleSlip)
157			{
158		✗	if (auto slip = searchCycleSlipAlreadyFound(satSigId1);
159		✗	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		✗	if (auto slip = searchCycleSlipAlreadyFound(satSigId2);
166		✗	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		✗	for (const auto& signal : obs.signals)
181			{
182		✗	auto satSigId = SatSigId(signal.code, obs.satId.satNum);
183		✗	auto iter = std::ranges::find(resetThisEpoch, satSigId);
184		✗	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		✗	return cycleSlips;
194		✗	}
195
196		✗	void CycleSlipDetector::reset()
197			{
198		✗	_singleFrequencyDetector.clear();
199		✗	_dualFrequencyDetector.clear();
200		✗	}
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		✗	void from_json(const json& j, CycleSlipDetector& data)
289			{
290		✗	if (j.contains("enableLLICheck")) { j.at("enableLLICheck").get_to(data._enableLLICheck); }
291		✗	if (j.contains("singleFrequencyThresholdPercentage")) { j.at("singleFrequencyThresholdPercentage").get_to(data._singleFrequencyThresholdPercentage); }
292		✗	if (j.contains("dualFrequencyThresholdPercentage")) { j.at("dualFrequencyThresholdPercentage").get_to(data._dualFrequencyThresholdPercentage); }
293		✗	if (j.contains("singleFrequencyDetector")) { j.at("singleFrequencyDetector").get_to(data._singleFrequencyDetector); }
294		✗	if (j.contains("dualFrequencyDetector")) { j.at("dualFrequencyDetector").get_to(data._dualFrequencyDetector); }
295		✗	}
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