INSTINCT Code Coverage Report

Directory: src/
File: Nodes/DataProcessor/ErrorModel/ErrorModel.cpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 262 624 42.0%
Functions: 12 46 26.1%
Branches: 420 1708 24.6%
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 "ErrorModel.hpp"
10
11 #include "NodeRegistry.hpp"
12 #include <algorithm>
13 #include <imgui.h>
14 #include "Navigation/INS/Units.hpp"
15 #include "internal/NodeManager.hpp"
16 namespace nm = NAV::NodeManager;
17 #include "internal/FlowManager.hpp"
18
19 #include "NodeData/IMU/ImuObsSimulated.hpp"
20 #include "NodeData/IMU/ImuObsWDelta.hpp"
21
22 #include "internal/gui/widgets/HelpMarker.hpp"
23 #include "internal/gui/widgets/imgui_ex.hpp"
24 #include "internal/gui/widgets/InputWithUnit.hpp"
25 #include "internal/gui/NodeEditorApplication.hpp"
26
27 #include "Navigation/Transformations/CoordinateFrames.hpp"
28 #include "Navigation/Transformations/Units.hpp"
29 #include "Navigation/GNSS/Functions.hpp"
30
31 #include "util/Eigen.hpp"
32 #include "util/StringUtil.hpp"
33
34 #include <imgui_internal.h>
35 #include <limits>
36 #include <set>
37 #include <type_traits>
38
39 // ---------------------------------------------------------- Private variabels ------------------------------------------------------------
40
41 namespace NAV
42 {
43 /// List of supported data identifiers
44 const std::vector<std::string> supportedDataIdentifier{ ImuObs::type(), ImuObsWDelta::type(), PosVelAtt::type(), GnssObs::type() };
45
46 } // namespace NAV
47
48 // ---------------------------------------------------------- Member functions -------------------------------------------------------------
49
50 120 NAV::ErrorModel::ErrorModel()
51
56/112
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 120 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 120 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 120 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 120 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 120 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 120 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 120 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 120 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 120 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 120 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 120 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 120 times.
✗ Branch 40 not taken.
✓ Branch 42 taken 120 times.
✗ Branch 43 not taken.
✓ Branch 45 taken 120 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 120 times.
✗ Branch 49 not taken.
✓ Branch 51 taken 120 times.
✗ Branch 52 not taken.
✓ Branch 54 taken 120 times.
✗ Branch 55 not taken.
✓ Branch 57 taken 120 times.
✗ Branch 58 not taken.
✓ Branch 60 taken 120 times.
✗ Branch 61 not taken.
✓ Branch 63 taken 120 times.
✗ Branch 64 not taken.
✓ Branch 66 taken 120 times.
✗ Branch 67 not taken.
✓ Branch 69 taken 120 times.
✗ Branch 70 not taken.
✓ Branch 72 taken 120 times.
✗ Branch 73 not taken.
✓ Branch 75 taken 120 times.
✗ Branch 76 not taken.
✓ Branch 78 taken 120 times.
✗ Branch 79 not taken.
✓ Branch 81 taken 120 times.
✗ Branch 82 not taken.
✓ Branch 84 taken 120 times.
✗ Branch 85 not taken.
✓ Branch 87 taken 120 times.
✗ Branch 88 not taken.
✓ Branch 90 taken 120 times.
✗ Branch 91 not taken.
✓ Branch 93 taken 120 times.
✗ Branch 94 not taken.
✓ Branch 96 taken 120 times.
✗ Branch 97 not taken.
✓ Branch 99 taken 120 times.
✗ Branch 100 not taken.
✓ Branch 102 taken 120 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 120 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 120 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 120 times.
✗ Branch 112 not taken.
✓ Branch 114 taken 120 times.
✗ Branch 115 not taken.
✓ Branch 117 taken 120 times.
✗ Branch 118 not taken.
✓ Branch 120 taken 120 times.
✗ Branch 121 not taken.
✓ Branch 123 taken 120 times.
✗ Branch 124 not taken.
✓ Branch 126 taken 120 times.
✗ Branch 127 not taken.
✓ Branch 129 taken 120 times.
✗ Branch 130 not taken.
✓ Branch 132 taken 120 times.
✗ Branch 133 not taken.
✓ Branch 135 taken 120 times.
✗ Branch 136 not taken.
✓ Branch 138 taken 120 times.
✗ Branch 139 not taken.
✓ Branch 141 taken 120 times.
✗ Branch 142 not taken.
✓ Branch 144 taken 120 times.
✗ Branch 145 not taken.
✓ Branch 147 taken 120 times.
✗ Branch 148 not taken.
✓ Branch 150 taken 120 times.
✗ Branch 151 not taken.
✓ Branch 153 taken 120 times.
✗ Branch 154 not taken.
✓ Branch 156 taken 120 times.
✗ Branch 157 not taken.
✓ Branch 159 taken 120 times.
✗ Branch 160 not taken.
✓ Branch 162 taken 120 times.
✗ Branch 163 not taken.
✓ Branch 165 taken 120 times.
✗ Branch 166 not taken.
✓ Branch 170 taken 120 times.
✗ Branch 171 not taken.
 120 : Node(typeStatic())
52 {
53 LOG_TRACE("{}: called", name);
54 120 _hasConfig = true;
55 120 _guiConfigDefaultWindowSize = { 812, 530 };
56
57
1/2
✓ Branch 2 taken 120 times.
✗ Branch 3 not taken.
 120 nm::CreateInputPin(this, "True", Pin::Type::Flow, supportedDataIdentifier, &ErrorModel::receiveObs);
58
59
1/2
✓ Branch 3 taken 120 times.
✗ Branch 4 not taken.
 120 nm::CreateOutputPin(this, "Biased", Pin::Type::Flow, supportedDataIdentifier);
60
61
1/2
✓ Branch 4 taken 120 times.
✗ Branch 5 not taken.
 120 std::mt19937_64 gen(static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()));
62
1/2
✓ Branch 2 taken 120 times.
✗ Branch 3 not taken.
 120 std::uniform_int_distribution<uint64_t> dist(0, std::numeric_limits<uint64_t>::max() / 2);
63
64
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 120 _imuAccelerometerRng.seed = dist(gen);
65
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 120 _imuGyroscopeRng.seed = dist(gen);
66
67
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 120 _imuAccelerometerRWRng.seed = dist(gen);
68
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 120 _imuGyroscopeRWRng.seed = dist(gen);
69
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 120 _imuAccelerometerIRWRng.seed = dist(gen);
70
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 120 _imuGyroscopeIRWRng.seed = dist(gen);
71
72
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 120 _positionRng.seed = dist(gen);
73
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 120 _velocityRng.seed = dist(gen);
74
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 120 _attitudeRng.seed = dist(gen);
75
76
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 120 _pseudorangeRng.seed = dist(gen);
77
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 120 _carrierPhaseRng.seed = dist(gen);
78
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 120 _dopplerRng.seed = dist(gen);
79
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 120 _ambiguityRng.seed = dist(gen);
80
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 120 _cycleSlipRng.seed = dist(gen);
81 120 }
82
83 256 NAV::ErrorModel::~ErrorModel()
84 {
85 LOG_TRACE("{}: called", nameId());
86 256 }
87
88 464 std::string NAV::ErrorModel::typeStatic()
89 {
90
1/2
✓ Branch 1 taken 232 times.
✗ Branch 2 not taken.
 464 return "ErrorModel";
91 }
92
93 ✗ std::string NAV::ErrorModel::type() const
94 {
95 ✗ return typeStatic();
96 }
97
98 112 std::string NAV::ErrorModel::category()
99 {
100
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 224 return "Data Processor";
101 }
102
103 ✗ void NAV::ErrorModel::guiConfig()
104 {
105 ✗ if (outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier.size() != 1)
106 {
107 ✗ ImGui::TextUnformatted("Please connect the input pin to show the options");
108 ✗ return;
109 }
110
111 ✗ float itemWidth = 470 * gui::NodeEditorApplication::windowFontRatio();
112 ✗ float unitWidth = 180 * gui::NodeEditorApplication::windowFontRatio();
113
114 ✗ auto inputDoubleWithUnit = [&](const char* title, double& data, auto& unit, const char* combo_items_separated_by_zeros, const char* format) {
115 ✗ if (auto response = gui::widgets::InputDoubleWithUnit(fmt::format("{}##{}", title, size_t(id)).c_str(), itemWidth, unitWidth,
116 &data, unit, combo_items_separated_by_zeros, 0.0, 0.0,
117 format, ImGuiInputTextFlags_CharsScientific))
118 {
119 ✗ if (response == gui::widgets::InputWithUnitChange_Input) { LOG_DEBUG("{}: {} changed to {}", nameId(), title, data); }
120 ✗ if (response == gui::widgets::InputWithUnitChange_Unit) { LOG_DEBUG("{}: {} unit changed to {}", nameId(), title, fmt::underlying(unit)); }
121 ✗ flow::ApplyChanges();
122 }
123 ✗ };
124
125 ✗ auto inputVector3WithUnit = [&](const char* title, Eigen::Vector3d& data, auto& unit, const char* combo_items_separated_by_zeros, const char* format) {
126 ✗ if (auto response = gui::widgets::InputDouble3WithUnit(fmt::format("{}##{}", title, size_t(id)).c_str(), itemWidth, unitWidth,
127 data.data(), unit, combo_items_separated_by_zeros,
128 format, ImGuiInputTextFlags_CharsScientific))
129 {
130 ✗ if (response == gui::widgets::InputWithUnitChange_Input) { LOG_DEBUG("{}: {} changed to {}", nameId(), title, data.transpose()); }
131 ✗ if (response == gui::widgets::InputWithUnitChange_Unit) { LOG_DEBUG("{}: {} unit changed to {}", nameId(), title, fmt::underlying(unit)); }
132 ✗ flow::ApplyChanges();
133 }
134 ✗ };
135
136 ✗ auto rngInput = [&](const char* title, RandomNumberGenerator& rng) {
137 ✗ float currentCursorX = ImGui::GetCursorPosX();
138 ✗ if (ImGui::Checkbox(fmt::format("##rng.useSeed {} {}", title, size_t(id)).c_str(), &rng.useSeed))
139 {
140 ✗ LOG_DEBUG("{}: {} rng.useSeed changed to {}", nameId(), title, rng.useSeed);
141 ✗ flow::ApplyChanges();
142 }
143 ✗ if (ImGui::IsItemHovered()) { ImGui::SetTooltip("Use seed?"); }
144 ✗ ImGui::SameLine();
145 ✗ if (!rng.useSeed)
146 {
147 ✗ ImGui::BeginDisabled();
148 }
149 ✗ ImGui::SetNextItemWidth(itemWidth - (ImGui::GetCursorPosX() - currentCursorX));
150 ✗ if (ImGui::SliderULong(fmt::format("{} Seed##{}", title, size_t(id)).c_str(), &rng.seed, 0, std::numeric_limits<uint64_t>::max() / 2, "%lu"))
151 {
152 ✗ LOG_DEBUG("{}: {} rng.seed changed to {}", nameId(), title, rng.seed);
153 ✗ flow::ApplyChanges();
154 }
155 ✗ if (!rng.useSeed)
156 {
157 ✗ ImGui::EndDisabled();
158 }
159 ✗ };
160
161 ✗ auto noiseGuiInput = [&]<typename T>(const char* title, T& data, auto& unit, const char* combo_items_separated_by_zeros, const char* format, RandomNumberGenerator& rng) {
162 ✗ if constexpr (std::is_same_v<T, double>) { inputDoubleWithUnit(title, data, unit, combo_items_separated_by_zeros, format); }
163 ✗ else if constexpr (std::is_same_v<T, Eigen::Vector3d>) { inputVector3WithUnit(title, data, unit, combo_items_separated_by_zeros, format); }
164
165 ✗ rngInput(title, rng);
166 ✗ };
167
168 ✗ if (_inputType == InputType::ImuObs || _inputType == InputType::ImuObsWDelta || _inputType == InputType::PosVelAtt)
169 {
170 ✗ ImGui::TextUnformatted("Offsets:");
171 ✗ ImGui::Indent();
172 {
173 ✗ if (_inputType == InputType::ImuObs || _inputType == InputType::ImuObsWDelta)
174 {
175 ✗ inputVector3WithUnit("Accelerometer Bias (platform)", _imuAccelerometerBias_p, _imuAccelerometerBiasUnit, MakeComboItems<Units::ImuAccelerometerUnits>().c_str(), "%.2g");
176 ✗ inputVector3WithUnit("Gyroscope Bias (platform)", _imuGyroscopeBias_p, _imuGyroscopeBiasUnit, MakeComboItems<Units::ImuGyroscopeUnits>().c_str(), "%.2g");
177 }
178 ✗ else if (_inputType == InputType::PosVelAtt)
179 {
180 ✗ inputVector3WithUnit(fmt::format("Position Bias ({})", _positionBiasUnit == PositionBiasUnits::meter ? "NED" : "LatLonAlt").c_str(),
181 ✗ _positionBias, _positionBiasUnit, "m, m, m\0\0", "%.2g");
182 ✗ inputVector3WithUnit("Velocity Bias (NED)", _velocityBias, _velocityBiasUnit, "m/s\0\0", "%.2g");
183 ✗ inputVector3WithUnit("RollPitchYaw Bias", _attitudeBias, _attitudeBiasUnit, "rad\0deg\0\0", "%.2g");
184 }
185 }
186 ✗ ImGui::Unindent();
187 }
188
189 ✗ if (_inputType == InputType::ImuObs || _inputType == InputType::ImuObsWDelta || _inputType == InputType::PosVelAtt || _inputType == InputType::GnssObs)
190 {
191 ✗ ImGui::TextUnformatted("Measurement noise:");
192 ✗ ImGui::Indent();
193 {
194 ✗ if (_inputType == InputType::ImuObs || _inputType == InputType::ImuObsWDelta)
195 {
196 ✗ noiseGuiInput("Accelerometer Noise (Std. dev)", _imuAccelerometerNoise, _imuAccelerometerNoiseUnit, MakeComboItems<Units::ImuAccelerometerNoiseUnits>().c_str(), "%.2g", _imuAccelerometerRng);
197 ✗ noiseGuiInput("Gyroscope Noise (Std. dev)", _imuGyroscopeNoise, _imuGyroscopeNoiseUnit, MakeComboItems<Units::ImuGyroscopeNoiseUnits>().c_str(), "%.2g", _imuGyroscopeRng);
198 }
199 ✗ else if (_inputType == InputType::PosVelAtt)
200 {
201 ✗ noiseGuiInput(fmt::format("Position Noise ({})", _positionNoiseUnit == PositionNoiseUnits::meter
202 ✗ ? "Standard deviation"
203 : "Variance")
204 .c_str(),
205 ✗ _positionNoise, _positionNoiseUnit, "m, m, m\0m^2, m^2, m^2\0\0",
206 ✗ "%.2g", _positionRng);
207 ✗ noiseGuiInput(fmt::format("Velocity Noise ({})", _velocityNoiseUnit == VelocityNoiseUnits::m_s ? "Standard deviation"
208 : "Variance")
209 .c_str(),
210 ✗ _velocityNoise, _velocityNoiseUnit, "m/s\0m^2/s^2\0\0", "%.2g", _velocityRng);
211 ✗ noiseGuiInput(fmt::format("Attitude Noise ({})", _attitudeNoiseUnit == AttitudeNoiseUnits::rad || _attitudeNoiseUnit == AttitudeNoiseUnits::deg
212 ✗ ? "Standard deviation"
213 : "Variance")
214 .c_str(),
215 ✗ _attitudeNoise, _attitudeNoiseUnit, "rad\0deg\0rad^2\0deg^2\0\0", "%.2g", _attitudeRng);
216 }
217 ✗ else if (_inputType == InputType::GnssObs)
218 {
219 ✗ noiseGuiInput("Pseudorange Noise", _gui_pseudorangeNoise, _gui_pseudorangeNoiseUnit, "m\0\0", "%.3g", _pseudorangeRng);
220 ✗ noiseGuiInput("Carrier-phase Noise", _gui_carrierPhaseNoise, _gui_carrierPhaseNoiseUnit, "m\0\0", "%.3g", _carrierPhaseRng);
221 ✗ noiseGuiInput("Doppler/Range-rate Noise", _gui_dopplerNoise, _gui_dopplerNoiseUnit, "m/s\0\0", "%.3g", _dopplerRng);
222 }
223 }
224 ✗ ImGui::Unindent();
225 }
226 ✗ if (_inputType == InputType::ImuObs || _inputType == InputType::ImuObsWDelta)
227 {
228 ✗ ImGui::TextUnformatted("Random walk noise:");
229 ✗ ImGui::Indent();
230 ✗ noiseGuiInput("Accelerometer RW (Std. dev)", _imuAccelerometerRW, _imuAccelerometerRWUnit, MakeComboItems<Units::ImuAccelerometerNoiseUnits>().c_str(), "%.2g", _imuAccelerometerRWRng);
231 ✗ noiseGuiInput("Gyroscope RW (Std. dev)", _imuGyroscopeRW, _imuGyroscopeRWUnit, MakeComboItems<Units::ImuGyroscopeNoiseUnits>().c_str(), "%.2g", _imuGyroscopeRWRng);
232 ✗ ImGui::Unindent();
233 ✗ ImGui::TextUnformatted("Integrated Random walk noise:");
234 ✗ ImGui::Indent();
235 ✗ noiseGuiInput("Accelerometer IRW (Std. dev)", _imuAccelerometerIRW, _imuAccelerometerIRWUnit, MakeComboItems<Units::ImuAccelerometerIRWUnits>().c_str(), "%.2g", _imuAccelerometerIRWRng);
236 ✗ noiseGuiInput("Gyroscope IRW (Std. dev)", _imuGyroscopeIRW, _imuGyroscopeIRWUnit, MakeComboItems<Units::ImuGyroscopeIRWUnits>().c_str(), "%.2g", _imuGyroscopeIRWRng);
237 ✗ ImGui::Unindent();
238 }
239
240 ✗ if (_inputType == InputType::GnssObs)
241 {
242 ✗ ImGui::TextUnformatted("Ambiguities:");
243 ✗ ImGui::Indent();
244 {
245 ✗ ImGui::SetNextItemWidth((itemWidth - ImGui::GetStyle().ItemInnerSpacing.x) / 2.0F);
246 ✗ if (ImGui::InputIntL(fmt::format("##Ambiguity Bounds lower {}", size_t(id)).c_str(), _gui_ambiguityLimits.data(),
247 ✗ std::numeric_limits<int>::lowest(), _gui_ambiguityLimits[1], 0, 0))
248 {
249 ✗ LOG_DEBUG("{}: Ambiguity lower bound changed to {}", nameId(), _gui_ambiguityLimits[0]);
250 ✗ flow::ApplyChanges();
251 }
252 ✗ ImGui::SameLine();
253 ✗ ImGui::SetCursorPosX(ImGui::GetCursorPosX() - ImGui::GetStyle().ItemSpacing.x + ImGui::GetStyle().ItemInnerSpacing.x);
254 ✗ ImGui::SetNextItemWidth((itemWidth - ImGui::GetStyle().ItemInnerSpacing.x) / 2.0F);
255 ✗ if (ImGui::InputIntL(fmt::format("Ambiguity Bounds##Ambiguity Bounds upper{}", size_t(id)).c_str(), &_gui_ambiguityLimits[1],
256 ✗ _gui_ambiguityLimits[0], std::numeric_limits<int>::max(), 0, 0))
257 {
258 ✗ LOG_DEBUG("{}: Ambiguity upper bound changed to {}", nameId(), _gui_ambiguityLimits[1]);
259 ✗ flow::ApplyChanges();
260 }
261 ✗ rngInput("Ambiguity", _ambiguityRng);
262 }
263 ✗ ImGui::Unindent();
264
265 ✗ ImGui::TextUnformatted("Cycle-slip:");
266 ✗ ImGui::Indent();
267 {
268 ✗ noiseGuiInput("Frequency", _gui_cycleSlipFrequency, _gui_cycleSlipFrequencyUnit, "/ day\0/ hour\0/ minute\0\0", "%.2g", _cycleSlipRng);
269
270 ✗ if (auto response = gui::widgets::SliderDoubleWithUnit(fmt::format("Detection probability (LLI)##{}", size_t(id)).c_str(), itemWidth, unitWidth,
271 &_gui_cycleSlipDetectionProbability, 0.0, 100.0,
272 ✗ _gui_cycleSlipDetectionProbabilityUnit, "%\0\0", "%.2f"))
273 {
274 ✗ if (response == gui::widgets::InputWithUnitChange_Input) { LOG_DEBUG("{}: Detection probability (LLI) changed to {}", nameId(), _gui_cycleSlipDetectionProbability); }
275 ✗ if (response == gui::widgets::InputWithUnitChange_Unit) { LOG_DEBUG("{}: Detection probability (LLI) unit changed to {}", nameId(), fmt::underlying(_gui_cycleSlipDetectionProbabilityUnit)); }
276 ✗ flow::ApplyChanges();
277 }
278 ✗ ImGui::SameLine();
279 ✗ gui::widgets::HelpMarker("The chance that the Lock-of-Loss (LLI) indicator is set, when a cycle-slip occurs");
280
281 ✗ ImGui::SetNextItemWidth(itemWidth);
282 ✗ if (ImGui::DragInt(fmt::format("Ambiguity Range to slip to##{}", size_t(id)).c_str(), &_gui_cycleSlipRange,
283 1.0F, 1, std::numeric_limits<int>::max(), "+/- %d"))
284 {
285 ✗ LOG_DEBUG("{}: Cycle-slip ambiguity range changed to {}", nameId(), _gui_cycleSlipRange);
286 ✗ flow::ApplyChanges();
287 }
288 ✗ ImGui::SameLine();
289 ✗ gui::widgets::HelpMarker("If the ambiguity range is set, the cycle-slips are bound by it too");
290
291 ✗ ImGui::SetNextItemWidth(itemWidth);
292 ✗ if (ShowFrequencySelector(fmt::format("Satellite Frequencies##{}", size_t(id)).c_str(), _filterFreq))
293 {
294 ✗ flow::ApplyChanges();
295 }
296 ✗ ImGui::SameLine();
297 ✗ gui::widgets::HelpMarker("Frequency and code selection only affects the cycle-slip simulation");
298
299 ✗ ImGui::SetNextItemWidth(itemWidth);
300 ✗ if (ShowCodeSelector(fmt::format("Signal Codes##{}", size_t(id)).c_str(), _filterCode, _filterFreq))
301 {
302 ✗ flow::ApplyChanges();
303 }
304 }
305 ✗ ImGui::Unindent();
306
307 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
308 ✗ if (ImGui::TreeNode(fmt::format("Simulated Ambiguities and Cycle-slips##{}", size_t(id)).c_str()))
309 {
310 ✗ auto nAmb = static_cast<int>(_ambiguities.size());
311 ✗ if (nAmb > 0)
312 {
313 ✗ std::set<InsTime> ambiguityTimes;
314 ✗ for (int i = 0; i < nAmb; i++)
315 {
316 ✗ const auto& ambVec = std::next(_ambiguities.begin(), i)->second;
317 ✗ for (const auto& amb : ambVec)
318 {
319 ✗ ambiguityTimes.insert(amb.first);
320 }
321 }
322 ✗ if (ambiguityTimes.size() < 64 - 1)
323 {
324 ✗ ImGui::PushFont(Application::MonoFont());
325
326 ✗ if (ImGui::BeginTable(fmt::format("Ambiguities##{}", size_t(id)).c_str(), static_cast<int>(ambiguityTimes.size() + 1),
327 ImGuiTableFlags_Borders | ImGuiTableFlags_NoHostExtendX | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_ScrollX | ImGuiTableFlags_ScrollY,
328 ✗ ImVec2(0.0F, 300.0F * gui::NodeEditorApplication::monoFontRatio())))
329 {
330 ✗ ImGui::TableSetupColumn("");
331 ✗ for (const auto& time : ambiguityTimes)
332 {
333 ✗ auto t = time.toYMDHMS(GPST);
334 ✗ t.sec = std::round(t.sec * 1e2) / 1e2;
335 ✗ ImGui::TableSetupColumn(str::replaceAll_copy(fmt::format("{}", t), " ", "\n").c_str());
336 }
337 ✗ ImGui::TableSetupScrollFreeze(1, 1);
338 ✗ ImGui::TableHeadersRow();
339
340 ✗ for (int i = 0; i < nAmb; i++)
341 {
342 ✗ ImGui::TableNextRow();
343 ✗ ImGui::TableNextColumn();
344 ✗ const auto& ambiguities = std::next(_ambiguities.begin(), i);
345 ✗ if (ambiguities == _ambiguities.end()) { break; }
346 ✗ ImGui::TextUnformatted(fmt::format("{}", ambiguities->first).c_str());
347 ✗ ImGui::TableSetBgColor(ImGuiTableBgTarget_CellBg, ImGui::GetColorU32(ImGui::GetStyle().Colors[ImGuiCol_TableHeaderBg]));
348
349 ✗ for (const auto& time : ambiguityTimes)
350 {
351 ✗ ImGui::TableNextColumn();
352 ✗ auto iter = std::ranges::find_if(ambiguities->second, [&time](const auto& timeAndAmb) {
353 ✗ return timeAndAmb.first == time;
354 });
355 ✗ if (iter != ambiguities->second.end())
356 {
357 ✗ ImVec4 color;
358 ✗ auto cycleIter = std::ranges::find_if(_cycleSlips, [&](const CycleSlipInfo& cycleSlip) {
359 ✗ return cycleSlip.time == time && cycleSlip.satSigId == ambiguities->first;
360 });
361 ✗ if (cycleIter != _cycleSlips.end()) { color = ImColor(240, 128, 128); }
362 ✗ else if (ambiguities->second.back().first == time) { color = ImGui::GetStyle().Colors[ImGuiCol_Text]; }
363 ✗ else { color = ImGui::GetStyle().Colors[ImGuiCol_TextDisabled]; }
364 ✗ ImGui::TextColored(color, "%s%s",
365 ✗ fmt::format("{}", iter->second).c_str(),
366 ✗ cycleIter != _cycleSlips.end() && cycleIter->LLI ? " (LLI)" : "");
367 ✗ if (ImGui::IsItemHovered() && cycleIter != _cycleSlips.end()) { ImGui::SetTooltip("Cycle-slip"); }
368 }
369 ✗ else if (time < ambiguities->second.front().first)
370 {
371 ✗ ImGui::TableSetBgColor(ImGuiTableBgTarget_CellBg, ImColor(70, 70, 70));
372 }
373 }
374 }
375
376 ✗ ImGui::EndTable();
377 }
378 ✗ ImGui::PopFont();
379 }
380 else
381 {
382 ✗ ImGui::TextColored(ImColor(255, 0, 0), "More than 64 timestamps for ambiguities, which cannot be displayed in a table");
383 }
384 ✗ }
385
386 ✗ ImGui::TreePop();
387 }
388 }
389 }
390
391 ✗ json NAV::ErrorModel::save() const
392 {
393 LOG_TRACE("{}: called", nameId());
394
395 ✗ json j;
396
397 ✗ j["imuAccelerometerBiasUnit"] = _imuAccelerometerBiasUnit;
398 ✗ j["imuAccelerometerBias_p"] = _imuAccelerometerBias_p;
399 ✗ j["imuGyroscopeBiasUnit"] = _imuGyroscopeBiasUnit;
400 ✗ j["imuGyroscopeBias_p"] = _imuGyroscopeBias_p;
401 ✗ j["imuAccelerometerNoiseUnit"] = _imuAccelerometerNoiseUnit;
402 ✗ j["imuAccelerometerNoise"] = _imuAccelerometerNoise;
403 ✗ j["imuAccelerometerRng"] = _imuAccelerometerRng;
404 ✗ j["imuGyroscopeNoiseUnit"] = _imuGyroscopeNoiseUnit;
405 ✗ j["imuGyroscopeNoise"] = _imuGyroscopeNoise;
406 ✗ j["imuGyroscopeRng"] = _imuGyroscopeRng;
407 ✗ j["imuAccelerometerRWUnit"] = _imuAccelerometerRWUnit;
408 ✗ j["imuAccelerometerRW"] = _imuAccelerometerRW;
409 ✗ j["imuAccelerometerRWRng"] = _imuAccelerometerRWRng;
410
411 ✗ j["imuGyroscopeRWUnit"] = _imuGyroscopeRWUnit;
412 ✗ j["imuGyroscopeRW"] = _imuGyroscopeRW;
413 ✗ j["imuGyroscopeRWRng"] = _imuGyroscopeRWRng;
414 ✗ j["imuAccelerometerIRWUnit"] = _imuAccelerometerIRWUnit;
415 ✗ j["imuAccelerometerIRW"] = _imuAccelerometerIRW;
416 ✗ j["imuAccelerometerIRWRng"] = _imuAccelerometerIRWRng;
417 ✗ j["imuGyroscopeIRWUnit"] = _imuGyroscopeIRWUnit;
418 ✗ j["imuGyroscopeIRW"] = _imuGyroscopeIRW;
419 ✗ j["imuGyroscopeIRWRng"] = _imuGyroscopeIRWRng;
420
421 // #########################################################################################################################################
422 ✗ j["positionBiasUnit"] = _positionBiasUnit;
423 ✗ j["positionBias"] = _positionBias;
424 ✗ j["velocityBiasUnit"] = _velocityBiasUnit;
425 ✗ j["velocityBias"] = _velocityBias;
426 ✗ j["attitudeBiasUnit"] = _attitudeBiasUnit;
427 ✗ j["attitudeBias"] = _attitudeBias;
428 ✗ j["positionNoiseUnit"] = _positionNoiseUnit;
429 ✗ j["positionNoise"] = _positionNoise;
430 ✗ j["positionRng"] = _positionRng;
431 ✗ j["velocityNoiseUnit"] = _velocityNoiseUnit;
432 ✗ j["velocityNoise"] = _velocityNoise;
433 ✗ j["velocityRng"] = _velocityRng;
434 ✗ j["attitudeNoiseUnit"] = _attitudeNoiseUnit;
435 ✗ j["attitudeNoise"] = _attitudeNoise;
436 ✗ j["attitudeRng"] = _attitudeRng;
437 // #########################################################################################################################################
438 ✗ j["pseudorangeNoiseUnit"] = _gui_pseudorangeNoiseUnit;
439 ✗ j["pseudorangeNoise"] = _gui_pseudorangeNoise;
440 ✗ j["pseudorangeRng"] = _pseudorangeRng;
441 ✗ j["carrierPhaseNoiseUnit"] = _gui_carrierPhaseNoiseUnit;
442 ✗ j["carrierPhaseNoise"] = _gui_carrierPhaseNoise;
443 ✗ j["carrierPhaseRng"] = _carrierPhaseRng;
444 ✗ j["dopplerNoiseUnit"] = _gui_dopplerNoiseUnit;
445 ✗ j["dopplerNoise"] = _gui_dopplerNoise;
446 ✗ j["dopplerRng"] = _dopplerRng;
447 ✗ j["ambiguityLimits"] = _gui_ambiguityLimits;
448 ✗ j["ambiguityRng"] = _ambiguityRng;
449 ✗ j["cycleSlipFrequencyUnit"] = _gui_cycleSlipFrequencyUnit;
450 ✗ j["cycleSlipFrequency"] = _gui_cycleSlipFrequency;
451 ✗ j["cycleSlipRange"] = _gui_cycleSlipRange;
452 ✗ j["cycleSlipDetectionProbabilityUnit"] = _gui_cycleSlipDetectionProbabilityUnit;
453 ✗ j["cycleSlipDetectionProbability"] = _gui_cycleSlipDetectionProbability;
454 ✗ j["cycleSlipRng"] = _cycleSlipRng;
455
456 ✗ j["filterFreq"] = Frequency_(_filterFreq);
457 ✗ j["filterCode"] = _filterCode;
458
459 ✗ return j;
460 ✗ }
461
462 8 void NAV::ErrorModel::restore(json const& j)
463 {
464 LOG_TRACE("{}: called", nameId());
465
466
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuAccelerometerBiasUnit")) { j.at("imuAccelerometerBiasUnit").get_to(_imuAccelerometerBiasUnit); }
467
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuAccelerometerBias_p")) { j.at("imuAccelerometerBias_p").get_to(_imuAccelerometerBias_p); }
468
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuGyroscopeBiasUnit")) { j.at("imuGyroscopeBiasUnit").get_to(_imuGyroscopeBiasUnit); }
469
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuGyroscopeBias_p")) { j.at("imuGyroscopeBias_p").get_to(_imuGyroscopeBias_p); }
470
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuAccelerometerNoiseUnit")) { j.at("imuAccelerometerNoiseUnit").get_to(_imuAccelerometerNoiseUnit); }
471
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuAccelerometerNoise")) { j.at("imuAccelerometerNoise").get_to(_imuAccelerometerNoise); }
472
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuAccelerometerRng")) { j.at("imuAccelerometerRng").get_to(_imuAccelerometerRng); }
473
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuGyroscopeNoiseUnit")) { j.at("imuGyroscopeNoiseUnit").get_to(_imuGyroscopeNoiseUnit); }
474
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuGyroscopeNoise")) { j.at("imuGyroscopeNoise").get_to(_imuGyroscopeNoise); }
475
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuGyroscopeRng")) { j.at("imuGyroscopeRng").get_to(_imuGyroscopeRng); }
476
477
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuAccelerometerRWUnit")) { j.at("imuAccelerometerRWUnit").get_to(_imuAccelerometerRWUnit); }
478
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuAccelerometerRW")) { j.at("imuAccelerometerRW").get_to(_imuAccelerometerRW); }
479
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuAccelerometerRWRng")) { j.at("imuAccelerometerRWRng").get_to(_imuAccelerometerRWRng); }
480
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuGyroscopeRWUnit")) { j.at("imuGyroscopeRWUnit").get_to(_imuGyroscopeRWUnit); }
481
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuGyroscopeRW")) { j.at("imuGyroscopeRW").get_to(_imuGyroscopeRW); }
482
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuGyroscopeRWRng")) { j.at("imuGyroscopeRWRng").get_to(_imuGyroscopeRWRng); }
483
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuAccelerometerIRWUnit")) { j.at("imuAccelerometerIRWUnit").get_to(_imuAccelerometerIRWUnit); }
484
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuAccelerometerIRW")) { j.at("imuAccelerometerIRW").get_to(_imuAccelerometerIRW); }
485
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuAccelerometerIRWRng")) { j.at("imuAccelerometerIRWRng").get_to(_imuAccelerometerIRWRng); }
486
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuGyroscopeIRWUnit")) { j.at("imuGyroscopeIRWUnit").get_to(_imuGyroscopeIRWUnit); }
487
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuGyroscopeIRW")) { j.at("imuGyroscopeIRW").get_to(_imuGyroscopeIRW); }
488
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("imuGyroscopeIRWRng")) { j.at("imuGyroscopeIRWRng").get_to(_imuGyroscopeIRWRng); }
489 // #########################################################################################################################################
490
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("positionBiasUnit")) { j.at("positionBiasUnit").get_to(_positionBiasUnit); }
491
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("positionBias")) { j.at("positionBias").get_to(_positionBias); }
492
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("velocityBiasUnit")) { j.at("velocityBiasUnit").get_to(_velocityBiasUnit); }
493
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("velocityBias")) { j.at("velocityBias").get_to(_velocityBias); }
494
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("attitudeBiasUnit")) { j.at("attitudeBiasUnit").get_to(_attitudeBiasUnit); }
495
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("attitudeBias")) { j.at("attitudeBias").get_to(_attitudeBias); }
496
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("positionNoiseUnit")) { j.at("positionNoiseUnit").get_to(_positionNoiseUnit); }
497
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("positionNoise")) { j.at("positionNoise").get_to(_positionNoise); }
498
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("positionRng")) { j.at("positionRng").get_to(_positionRng); }
499
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("velocityNoiseUnit")) { j.at("velocityNoiseUnit").get_to(_velocityNoiseUnit); }
500
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("velocityNoise")) { j.at("velocityNoise").get_to(_velocityNoise); }
501
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("velocityRng")) { j.at("velocityRng").get_to(_velocityRng); }
502
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("attitudeNoiseUnit")) { j.at("attitudeNoiseUnit").get_to(_attitudeNoiseUnit); }
503
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("attitudeNoise")) { j.at("attitudeNoise").get_to(_attitudeNoise); }
504
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("attitudeRng")) { j.at("attitudeRng").get_to(_attitudeRng); }
505 // #########################################################################################################################################
506
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("pseudorangeNoiseUnit")) { j.at("pseudorangeNoiseUnit").get_to(_gui_pseudorangeNoiseUnit); }
507
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("pseudorangeNoise")) { j.at("pseudorangeNoise").get_to(_gui_pseudorangeNoise); }
508
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("pseudorangeRng")) { j.at("pseudorangeRng").get_to(_pseudorangeRng); }
509
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("carrierPhaseNoiseUnit")) { j.at("carrierPhaseNoiseUnit").get_to(_gui_carrierPhaseNoiseUnit); }
510
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("carrierPhaseNoise")) { j.at("carrierPhaseNoise").get_to(_gui_carrierPhaseNoise); }
511
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("carrierPhaseRng")) { j.at("carrierPhaseRng").get_to(_carrierPhaseRng); }
512
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("dopplerNoiseUnit")) { j.at("dopplerNoiseUnit").get_to(_gui_dopplerNoiseUnit); }
513
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("dopplerNoise")) { j.at("dopplerNoise").get_to(_gui_dopplerNoise); }
514
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("dopplerRng")) { j.at("dopplerRng").get_to(_dopplerRng); }
515
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("ambiguityLimits")) { j.at("ambiguityLimits").get_to(_gui_ambiguityLimits); }
516
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("ambiguityRng")) { j.at("ambiguityRng").get_to(_ambiguityRng); }
517
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("cycleSlipFrequencyUnit")) { j.at("cycleSlipFrequencyUnit").get_to(_gui_cycleSlipFrequencyUnit); }
518
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("cycleSlipFrequency")) { j.at("cycleSlipFrequency").get_to(_gui_cycleSlipFrequency); }
519
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("cycleSlipRange")) { j.at("cycleSlipRange").get_to(_gui_cycleSlipRange); }
520
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("cycleSlipDetectionProbabilityUnit")) { j.at("cycleSlipDetectionProbabilityUnit").get_to(_gui_cycleSlipDetectionProbabilityUnit); }
521
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("cycleSlipDetectionProbability")) { j.at("cycleSlipDetectionProbability").get_to(_gui_cycleSlipDetectionProbability); }
522
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("cycleSlipRng")) { j.at("cycleSlipRng").get_to(_cycleSlipRng); }
523
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("filterFreq"))
524 {
525 8 uint64_t value = 0;
526
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 j.at("filterFreq").get_to(value);
527 8 _filterFreq = Frequency_(value);
528 }
529
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (j.contains("filterCode")) { j.at("filterCode").get_to(_filterCode); }
530 8 }
531
532 16 bool NAV::ErrorModel::resetNode()
533 {
534 LOG_TRACE("{}: called", nameId());
535
536 16 _lastObservationTime.reset();
537 16 _dt = 0.0;
538
539
3/4
✓ Branch 0 taken 16 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
✓ Branch 3 taken 4 times.
 16 if (_inputType == InputType::ImuObs || _inputType == InputType::ImuObsWDelta)
540 {
541 12 _imuAccelerometerRng.resetSeed(size_t(id));
542 12 _imuGyroscopeRng.resetSeed(size_t(id));
543
544 12 _imuAccelerometerRWRng.resetSeed(size_t(id));
545 12 _imuGyroscopeRWRng.resetSeed(size_t(id));
546 12 _imuAccelerometerIRWRng.resetSeed(size_t(id));
547 12 _imuGyroscopeIRWRng.resetSeed(size_t(id));
548
549 12 _randomWalkAccelerometer.setZero();
550 12 _randomWalkGyroscope.setZero();
551
552 12 _integratedRandomWalkAccelerometer.setZero();
553 12 _integratedRandomWalkGyroscope.setZero();
554 12 _integratedRandomWalkAccelerometer_velocity.setZero();
555 12 _integratedRandomWalkGyroscope_velocity.setZero();
556 }
557
1/2
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
 4 else if (_inputType == InputType::PosVelAtt)
558 {
559 4 _positionRng.resetSeed(size_t(id));
560 4 _velocityRng.resetSeed(size_t(id));
561 4 _attitudeRng.resetSeed(size_t(id));
562 }
563 ✗ else if (_inputType == InputType::GnssObs)
564 {
565 ✗ _pseudorangeRng.resetSeed(size_t(id));
566 ✗ _carrierPhaseRng.resetSeed(size_t(id));
567 ✗ _dopplerRng.resetSeed(size_t(id));
568 ✗ _ambiguityRng.resetSeed(size_t(id));
569 ✗ _ambiguities.clear();
570 ✗ _cycleSlips.clear();
571 ✗ _cycleSlipRng.resetSeed(size_t(id));
572 ✗ _cycleSlipWindowStartTime.reset();
573 }
574
575 16 return true;
576 }
577
578 19 void NAV::ErrorModel::afterCreateLink(OutputPin& startPin, InputPin& endPin)
579 {
580 LOG_TRACE("{}: called for {} ==> {}", nameId(), size_t(startPin.id), size_t(endPin.id));
581
582
3/4
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 11 times.
✓ Branch 4 taken 8 times.
 19 if (endPin.parentNode->id != id)
583 {
584 11 return; // Link on Output Port
585 }
586
587 // Store previous output pin identifier
588
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 auto previousOutputPinDataIdentifier = outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier;
589 // Overwrite output pin identifier with input pin identifier
590
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier = startPin.dataIdentifier;
591
592
7/12
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✓ Branch 11 taken 8 times.
✓ Branch 13 taken 6 times.
✓ Branch 14 taken 2 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 24 if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier, { ImuObsWDelta::type() }))
593 {
594 6 _inputType = InputType::ImuObsWDelta;
595 }
596
6/12
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✓ Branch 11 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 6 else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier, { ImuObs::type() }))
597 {
598 ✗ _inputType = InputType::ImuObs;
599 }
600
6/12
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✓ Branch 11 taken 2 times.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 6 else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier, { PosVelAtt::type() }))
601 {
602 2 _inputType = InputType::PosVelAtt;
603 }
604 ✗ else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier, { GnssObs::type() }))
605 {
606 ✗ _inputType = InputType::GnssObs;
607 }
608
609
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
 8 if (previousOutputPinDataIdentifier != outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier) // If the identifier changed
610 {
611 // Check if connected links on output port are still valid
612
3/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 8 taken 1 times.
✓ Branch 9 taken 8 times.
 9 for (auto& link : outputPins.at(OUTPUT_PORT_INDEX_FLOW).links)
613 {
614
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 if (auto* endPin = link.getConnectedPin())
615 {
616
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
 1 if (!outputPins.at(OUTPUT_PORT_INDEX_FLOW).canCreateLink(*endPin))
617 {
618 // If the link is not valid anymore, delete it
619 ✗ outputPins.at(OUTPUT_PORT_INDEX_FLOW).deleteLink(*endPin);
620 }
621 }
622 }
623
624 // Refresh all links connected to the output pin if the type changed
625
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
 8 if (outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier != previousOutputPinDataIdentifier)
626 {
627
3/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 8 taken 1 times.
✓ Branch 9 taken 8 times.
 9 for (auto& link : outputPins.at(OUTPUT_PORT_INDEX_FLOW).links)
628 {
629
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 if (auto* connectedPin = link.getConnectedPin())
630 {
631
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 outputPins.at(OUTPUT_PORT_INDEX_FLOW).recreateLink(*connectedPin);
632 }
633 }
634 }
635 }
636 20 }
637
638 19 void NAV::ErrorModel::afterDeleteLink(OutputPin& startPin, InputPin& endPin)
639 {
640 LOG_TRACE("{}: called for {} ==> {}", nameId(), size_t(startPin.id), size_t(endPin.id));
641
642 19 if ((endPin.parentNode->id != id // Link on Output port is removed
643
2/2
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 7 times.
 11 && !inputPins.at(INPUT_PORT_INDEX_FLOW).isPinLinked()) // and the Input port is not linked
644
6/6
✓ Branch 0 taken 11 times.
✓ Branch 1 taken 8 times.
✓ Branch 3 taken 8 times.
✓ Branch 4 taken 4 times.
✓ Branch 5 taken 9 times.
✓ Branch 6 taken 10 times.
 38 || (startPin.parentNode->id != id // Link on Input port is removed
645
2/2
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 6 times.
 8 && !outputPins.at(OUTPUT_PORT_INDEX_FLOW).isPinLinked())) // and the Output port is not linked
646 {
647 9 outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier = supportedDataIdentifier;
648 }
649 19 }
650
651 60020 void NAV::ErrorModel::receiveObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
652 {
653
1/2
✓ Branch 1 taken 60009 times.
✗ Branch 2 not taken.
 60020 auto obs = queue.extract_front();
654
3/4
✓ Branch 1 taken 60002 times.
✓ Branch 2 taken 8 times.
✓ Branch 5 taken 60008 times.
✗ Branch 6 not taken.
 60009 if (!_lastObservationTime.empty()) { _dt = static_cast<double>((obs->insTime - _lastObservationTime).count()); }
655
656 // Accelerometer Bias in platform frame coordinates [m/s^2]
657
1/2
✓ Branch 1 taken 60003 times.
✗ Branch 2 not taken.
 60015 Eigen::Vector3d accelerometerBias_p = convertUnit(_imuAccelerometerBias_p, _imuAccelerometerBiasUnit);
658 LOG_DATA("{}: accelerometerBias_p = {} [m/s^2]", nameId(), accelerometerBias_p.transpose());
659
660 // Gyroscope Bias in platform frame coordinates [rad/s]
661
1/2
✓ Branch 1 taken 60008 times.
✗ Branch 2 not taken.
 60003 Eigen::Vector3d gyroscopeBias_p = convertUnit(_imuGyroscopeBias_p, _imuGyroscopeBiasUnit);
662 LOG_DATA("{}: gyroscopeBias_p = {} [rad/s]", nameId(), gyroscopeBias_p.transpose());
663
664 // #########################################################################################################################################
665
666 // Accelerometer Noise standard deviation in platform frame coordinates [m/s^2/sqrt(s)]
667
1/2
✓ Branch 1 taken 60013 times.
✗ Branch 2 not taken.
 60008 Eigen::Vector3d accelerometerNoiseStd = convertUnit(_imuAccelerometerNoise, _imuAccelerometerNoiseUnit);
668 LOG_DATA("{}: accelerometerNoiseStd = {} [m/s^2/sqrt(s)]", nameId(), accelerometerNoiseStd.transpose());
669
670 // Gyroscope Noise standard deviation in platform frame coordinates [rad/s/sqrt(s)]
671
1/2
✓ Branch 1 taken 60012 times.
✗ Branch 2 not taken.
 60013 Eigen::Vector3d gyroscopeNoiseStd = convertUnit(_imuGyroscopeNoise, _imuGyroscopeNoiseUnit);
672 LOG_DATA("{}: gyroscopeNoiseStd = {} [rad/s/sqrt(s)]", nameId(), gyroscopeNoiseStd.transpose());
673
674 // Select the correct data type and make a copy of the node data to modify
675
7/12
✓ Branch 1 taken 60011 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60003 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60015 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 60007 times.
✓ Branch 11 taken 60009 times.
✓ Branch 13 taken 35001 times.
✓ Branch 14 taken 25008 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 180034 if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier, { ImuObsSimulated::type() }))
676 {
677
1/2
✓ Branch 2 taken 35012 times.
✗ Branch 3 not taken.
 35010 invokeCallbacks(OUTPUT_PORT_INDEX_FLOW,
678
2/4
✓ Branch 3 taken 35006 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 35010 times.
✗ Branch 8 not taken.
 70012 receiveImuObsWDelta(std::make_shared<ImuObsSimulated>(*std::static_pointer_cast<const ImuObsSimulated>(obs)),
679 accelerometerBias_p,
680 gyroscopeBias_p,
681 accelerometerNoiseStd,
682 gyroscopeNoiseStd));
683 }
684
6/12
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25008 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 25008 times.
✓ Branch 11 taken 25008 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 25008 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 75024 else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier, { ImuObsWDelta::type() }))
685 {
686 ✗ invokeCallbacks(OUTPUT_PORT_INDEX_FLOW,
687 ✗ receiveImuObsWDelta(std::make_shared<ImuObsWDelta>(*std::static_pointer_cast<const ImuObsWDelta>(obs)),
688 accelerometerBias_p,
689 gyroscopeBias_p,
690 accelerometerNoiseStd,
691 gyroscopeNoiseStd));
692 }
693
6/12
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25008 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 25008 times.
✓ Branch 11 taken 25008 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 25008 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 75024 else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier, { ImuObs::type() }))
694 {
695 ✗ invokeCallbacks(OUTPUT_PORT_INDEX_FLOW,
696 ✗ receiveImuObs(std::make_shared<ImuObs>(*std::static_pointer_cast<const ImuObs>(obs)),
697 accelerometerBias_p,
698 gyroscopeBias_p,
699 accelerometerNoiseStd,
700 gyroscopeNoiseStd));
701 }
702
6/12
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25008 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 25008 times.
✓ Branch 11 taken 25008 times.
✓ Branch 13 taken 25008 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 75024 else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier, { PosVelAtt::type() }))
703 {
704
3/6
✓ Branch 3 taken 25008 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 25008 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 25008 times.
✗ Branch 11 not taken.
 25008 invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, receivePosVelAtt(std::make_shared<PosVelAtt>(*std::static_pointer_cast<const PosVelAtt>(obs))));
705 }
706 ✗ else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier, { GnssObs::type() }))
707 {
708 ✗ invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, receiveGnssObs(std::make_shared<GnssObs>(*std::static_pointer_cast<const GnssObs>(obs))));
709 }
710
711 60015 _lastObservationTime = obs->insTime;
712 195048 }
713
714 35005 std::shared_ptr<NAV::ImuObs> NAV::ErrorModel::receiveImuObs(const std::shared_ptr<ImuObs>& imuObs,
715 const Eigen::Vector3d& accelerometerBias_p,
716 const Eigen::Vector3d& gyroscopeBias_p,
717 const Eigen::Vector3d& accelerometerNoiseStd,
718 const Eigen::Vector3d& gyroscopeNoiseStd)
719 {
720
2/2
✓ Branch 1 taken 34994 times.
✓ Branch 2 taken 6 times.
 35005 if (!_lastObservationTime.empty())
721 {
722 {
723 // Accelerometer RW standard deviation in platform frame coordinates [m/s^2/sqrt(s)]
724
1/2
✓ Branch 1 taken 34992 times.
✗ Branch 2 not taken.
 34994 Eigen::Vector3d accelerometerRWStd = convertUnit(_imuAccelerometerRW, _imuAccelerometerRWUnit);
725 LOG_DATA("{}: accelerometerRWStd = {} [m/s^2/sqrt(s)]", nameId(), accelerometerRWStd.transpose());
726
727 ✗ _randomWalkAccelerometer += Eigen::Vector3d{
728
1/2
✓ Branch 1 taken 35001 times.
✗ Branch 2 not taken.
 34995 _imuAccelerometerRWRng.getRand_normalDist(0.0, accelerometerRWStd(0)),
729
2/4
✓ Branch 1 taken 34999 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35006 times.
✗ Branch 5 not taken.
 35001 _imuAccelerometerRWRng.getRand_normalDist(0.0, accelerometerRWStd(1)),
730
3/6
✓ Branch 1 taken 35003 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35006 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35005 times.
✗ Branch 8 not taken.
 35006 _imuAccelerometerRWRng.getRand_normalDist(0.0, accelerometerRWStd(2))
731
3/6
✓ Branch 1 taken 34995 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34996 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34990 times.
✗ Branch 8 not taken.
 69997 } * sqrt(_dt);
732 }
733 {
734 // Accelerometer IRW standard deviation in platform frame coordinates [m/s^3/sqrt(s)]
735
1/2
✓ Branch 1 taken 34996 times.
✗ Branch 2 not taken.
 34990 Eigen::Vector3d accelerometerIRWStd = convertUnit(_imuAccelerometerIRW, _imuAccelerometerIRWUnit);
736 LOG_DATA("{}: accelerometerIRWStd = {} [m/s^3/sqrt(s)]", nameId(), accelerometerIRWStd.transpose());
737
738 // compute velocity RW first
739 ✗ _integratedRandomWalkAccelerometer_velocity += Eigen::Vector3d{
740
1/2
✓ Branch 1 taken 35005 times.
✗ Branch 2 not taken.
 34999 _imuAccelerometerIRWRng.getRand_normalDist(0.0, accelerometerIRWStd(0)),
741
2/4
✓ Branch 1 taken 35002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35005 times.
✗ Branch 5 not taken.
 35005 _imuAccelerometerIRWRng.getRand_normalDist(0.0, accelerometerIRWStd(1)),
742
3/6
✓ Branch 1 taken 35005 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35006 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35005 times.
✗ Branch 8 not taken.
 35005 _imuAccelerometerIRWRng.getRand_normalDist(0.0, accelerometerIRWStd(2))
743
3/6
✓ Branch 1 taken 34999 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35003 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34998 times.
✗ Branch 8 not taken.
 70001 } * sqrt(_dt);
744
745 // then compute IRW
746
2/4
✓ Branch 1 taken 35002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35000 times.
✗ Branch 5 not taken.
 34998 _integratedRandomWalkAccelerometer += _integratedRandomWalkAccelerometer_velocity * _dt;
747 }
748 {
749 // Gyro RW standard deviation in platform frame coordinates [rad/s/sqrt(s)]
750
1/2
✓ Branch 1 taken 35003 times.
✗ Branch 2 not taken.
 35000 Eigen::Vector3d gyroscopeRWStd = convertUnit(_imuGyroscopeRW, _imuGyroscopeRWUnit);
751 LOG_DATA("{}: gyroscopeRWStd = {} [rad/s/sqrt(s)]", nameId(), gyroscopeRWStd.transpose());
752
753 ✗ _randomWalkGyroscope += Eigen::Vector3d{
754
1/2
✓ Branch 1 taken 35006 times.
✗ Branch 2 not taken.
 35002 _imuGyroscopeRWRng.getRand_normalDist(0.0, gyroscopeRWStd(0)),
755
2/4
✓ Branch 1 taken 35003 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35006 times.
✗ Branch 5 not taken.
 35006 _imuGyroscopeRWRng.getRand_normalDist(0.0, gyroscopeRWStd(1)),
756
3/6
✓ Branch 1 taken 35004 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35006 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35004 times.
✗ Branch 8 not taken.
 35006 _imuGyroscopeRWRng.getRand_normalDist(0.0, gyroscopeRWStd(2))
757
3/6
✓ Branch 1 taken 35002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35002 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35002 times.
✗ Branch 8 not taken.
 70007 } * sqrt(_dt);
758 }
759 {
760 // Gyro RW standard deviation in platform frame coordinates [rad/s^2/sqrt(s)]
761
1/2
✓ Branch 1 taken 35003 times.
✗ Branch 2 not taken.
 35002 Eigen::Vector3d gyroscopeIRWStd = convertUnit(_imuGyroscopeIRW, _imuGyroscopeIRWUnit);
762 LOG_DATA("{}: gyroscopeIRWStd = {} [rad/s^2/sqrt(s)]", nameId(), gyroscopeIRWStd.transpose());
763
764 // compute velocity RW first
765 ✗ _integratedRandomWalkGyroscope_velocity += Eigen::Vector3d{
766
1/2
✓ Branch 1 taken 35006 times.
✗ Branch 2 not taken.
 35000 _imuGyroscopeIRWRng.getRand_normalDist(0.0, gyroscopeIRWStd(0)),
767
2/4
✓ Branch 1 taken 35003 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35006 times.
✗ Branch 5 not taken.
 35006 _imuGyroscopeIRWRng.getRand_normalDist(0.0, gyroscopeIRWStd(1)),
768
3/6
✓ Branch 1 taken 35004 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35005 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35006 times.
✗ Branch 8 not taken.
 35006 _imuGyroscopeIRWRng.getRand_normalDist(0.0, gyroscopeIRWStd(2))
769
3/6
✓ Branch 1 taken 35000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35002 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35002 times.
✗ Branch 8 not taken.
 70009 } * sqrt(_dt);
770
771 // then compute IRW
772
2/4
✓ Branch 1 taken 35002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35003 times.
✗ Branch 5 not taken.
 35002 _integratedRandomWalkGyroscope += _integratedRandomWalkGyroscope_velocity * _dt;
773 }
774 }
775
776 // #########################################################################################################################################
777
778 35006 imuObs->p_acceleration += accelerometerBias_p
779 ✗ + _randomWalkAccelerometer
780
3/6
✓ Branch 1 taken 35009 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35006 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35004 times.
✗ Branch 8 not taken.
 70017 + _integratedRandomWalkAccelerometer;
781
782 35010 imuObs->p_angularRate += gyroscopeBias_p
783 ✗ + _randomWalkGyroscope
784
3/6
✓ Branch 1 taken 35008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35010 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35005 times.
✗ Branch 8 not taken.
 70014 + _integratedRandomWalkGyroscope;
785
2/2
✓ Branch 0 taken 34999 times.
✓ Branch 1 taken 6 times.
 35005 if (_dt > 1e-9)
786 {
787
1/2
✓ Branch 1 taken 35005 times.
✗ Branch 2 not taken.
 70003 imuObs->p_acceleration += Eigen::Vector3d{ _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(0)),
788
2/4
✓ Branch 1 taken 35006 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35006 times.
✗ Branch 5 not taken.
 35005 _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(1)),
789
3/6
✓ Branch 1 taken 35004 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35005 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35004 times.
✗ Branch 8 not taken.
 35006 _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(2)) }
790
3/6
✓ Branch 1 taken 35000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35003 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34992 times.
✗ Branch 8 not taken.
 105007 / std::sqrt(_dt); // Scale with input frequency
791
1/2
✓ Branch 1 taken 35005 times.
✗ Branch 2 not taken.
 70002 imuObs->p_angularRate += Eigen::Vector3d{ _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(0)),
792
2/4
✓ Branch 1 taken 35002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35006 times.
✗ Branch 5 not taken.
 35005 _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(1)),
793
3/6
✓ Branch 1 taken 35003 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35006 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35005 times.
✗ Branch 8 not taken.
 35006 _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(2)) }
794
3/6
✓ Branch 1 taken 34996 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35006 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35006 times.
✗ Branch 8 not taken.
 105001 / std::sqrt(_dt); // Scale with input frequency
795 }
796
797 35012 return imuObs;
798 }
799
800 35012 std::shared_ptr<NAV::ImuObsWDelta> NAV::ErrorModel::receiveImuObsWDelta(const std::shared_ptr<ImuObsWDelta>& imuObsWDelta,
801 const Eigen::Vector3d& accelerometerBias_p,
802 const Eigen::Vector3d& gyroscopeBias_p,
803 const Eigen::Vector3d& accelerometerNoiseStd,
804 const Eigen::Vector3d& gyroscopeNoiseStd)
805 {
806
1/2
✓ Branch 2 taken 35011 times.
✗ Branch 3 not taken.
 35012 receiveImuObs(imuObsWDelta, accelerometerBias_p, gyroscopeBias_p, accelerometerNoiseStd, gyroscopeNoiseStd);
807
808
2/2
✓ Branch 0 taken 35001 times.
✓ Branch 1 taken 6 times.
 35007 double imuObsWDeltaAverageWindow = _dt != 0.0 ? _dt / imuObsWDelta->dtime : 1.0;
809
810
4/8
✓ Branch 1 taken 35010 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35008 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 35007 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 34998 times.
✗ Branch 13 not taken.
 35002 imuObsWDelta->dvel += imuObsWDelta->dtime * (accelerometerBias_p + _randomWalkAccelerometer + _integratedRandomWalkAccelerometer);
811
4/8
✓ Branch 1 taken 35004 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35007 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 35000 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 35009 times.
✗ Branch 13 not taken.
 34998 imuObsWDelta->dtheta += imuObsWDelta->dtime * (gyroscopeBias_p + _randomWalkGyroscope + _integratedRandomWalkGyroscope);
812
2/2
✓ Branch 0 taken 35003 times.
✓ Branch 1 taken 6 times.
 35009 if (_dt > 1e-9)
813 {
814
1/2
✓ Branch 1 taken 35004 times.
✗ Branch 2 not taken.
 69999 imuObsWDelta->dvel += Eigen::Vector3d{ _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(0) / std::sqrt(imuObsWDeltaAverageWindow)),
815
2/4
✓ Branch 1 taken 35003 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35006 times.
✗ Branch 5 not taken.
 35004 _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(1) / std::sqrt(imuObsWDeltaAverageWindow)),
816
3/6
✓ Branch 1 taken 35002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35006 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35005 times.
✗ Branch 8 not taken.
 35006 _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(2) / std::sqrt(imuObsWDeltaAverageWindow)) }
817
4/8
✓ Branch 2 taken 34997 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 35002 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 35002 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 34987 times.
✗ Branch 12 not taken.
 105008 * imuObsWDelta->dtime / std::sqrt(_dt); // Scale with input frequency
818
1/2
✓ Branch 1 taken 35003 times.
✗ Branch 2 not taken.
 69991 imuObsWDelta->dtheta += Eigen::Vector3d{ _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(0) / std::sqrt(imuObsWDeltaAverageWindow)),
819
2/4
✓ Branch 1 taken 35002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35003 times.
✗ Branch 5 not taken.
 35003 _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(1) / std::sqrt(imuObsWDeltaAverageWindow)),
820
3/6
✓ Branch 1 taken 35001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35006 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35003 times.
✗ Branch 8 not taken.
 35003 _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(2) / std::sqrt(imuObsWDeltaAverageWindow)) }
821
4/8
✓ Branch 2 taken 34991 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 34998 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 35000 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 35000 times.
✗ Branch 12 not taken.
 104990 * imuObsWDelta->dtime / std::sqrt(_dt); // Scale with input frequency
822 }
823
824 35006 return imuObsWDelta;
825 }
826
827 25008 std::shared_ptr<NAV::PosVelAtt> NAV::ErrorModel::receivePosVelAtt(const std::shared_ptr<PosVelAtt>& posVelAtt)
828 {
829 // Position Bias in latLonAlt in [rad, rad, m]
830
2/4
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
 25008 Eigen::Vector3d lla_positionBias = Eigen::Vector3d::Zero();
831
1/2
✓ Branch 0 taken 25008 times.
✗ Branch 1 not taken.
 25008 switch (_positionBiasUnit)
832 {
833 25008 case PositionBiasUnits::meter:
834 {
835
4/8
✓ Branch 2 taken 25008 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 25008 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 25008 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 25008 times.
✗ Branch 13 not taken.
 25008 Eigen::Vector3d e_positionBias = trafo::e_Quat_n(posVelAtt->latitude(), posVelAtt->longitude()) * _positionBias;
836
2/4
✓ Branch 2 taken 25008 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 25008 times.
 25008 if (!e_positionBias.isZero())
837 {
838 ✗ lla_positionBias = trafo::ecef2lla_WGS84(posVelAtt->e_position() + e_positionBias) - posVelAtt->lla_position();
839 }
840 25008 break;
841 }
842 }
843 LOG_DATA("{}: lla_positionBias = {} [rad, rad, m]", nameId(), lla_positionBias.transpose());
844
845 // Velocity bias in local-navigation coordinates in [m/s]
846
2/4
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
 25008 Eigen::Vector3d n_velocityBias = Eigen::Vector3d::Zero();
847
1/2
✓ Branch 0 taken 25008 times.
✗ Branch 1 not taken.
 25008 switch (_velocityBiasUnit)
848 {
849 25008 case VelocityBiasUnits::m_s:
850
1/2
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
 25008 n_velocityBias = _velocityBias;
851 25008 break;
852 }
853 LOG_DATA("{}: n_velocityBias = {} [m/s]", nameId(), n_velocityBias.transpose());
854
855 // Roll, pitch, yaw bias in [rad]
856
2/4
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
 25008 Eigen::Vector3d attitudeBias = Eigen::Vector3d::Zero();
857
1/3
✗ Branch 0 not taken.
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
 25008 switch (_attitudeBiasUnit)
858 {
859 ✗ case AttitudeBiasUnits::rad:
860 ✗ attitudeBias = _attitudeBias;
861 ✗ break;
862 25008 case AttitudeBiasUnits::deg:
863
1/2
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
 25008 attitudeBias = deg2rad(_attitudeBias);
864 25008 break;
865 }
866 LOG_DATA("{}: attitudeBias = {} [rad]", nameId(), attitudeBias.transpose());
867
868 // #########################################################################################################################################
869
870 // Position Noise standard deviation in latitude, longitude and altitude [rad, rad, m]
871
2/4
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
 25008 Eigen::Vector3d lla_positionNoiseStd = Eigen::Vector3d::Zero();
872
2/4
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
 25008 Eigen::Vector3d NED_pos_variance = Eigen::Vector3d::Zero();
873
2/4
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
 25008 Eigen::Vector3d NED_velocity_variance = Eigen::Vector3d::Zero();
874
1/3
✓ Branch 0 taken 25008 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 25008 switch (_positionNoiseUnit)
875 {
876 25008 case PositionNoiseUnits::meter:
877 {
878
4/8
✓ Branch 2 taken 25008 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 25008 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 25008 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 25008 times.
✗ Branch 13 not taken.
 25008 Eigen::Vector3d e_positionNoiseStd = trafo::e_Quat_n(posVelAtt->latitude(), posVelAtt->longitude()) * _positionNoise;
879
3/4
✓ Branch 2 taken 25008 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 24222 times.
✓ Branch 5 taken 786 times.
 25008 if (!e_positionNoiseStd.isZero())
880 {
881
4/8
✓ Branch 5 taken 24222 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24222 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 24222 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 24222 times.
✗ Branch 15 not taken.
 24222 lla_positionNoiseStd = trafo::ecef2lla_WGS84(posVelAtt->e_position() + e_positionNoiseStd) - posVelAtt->lla_position();
882 }
883
2/4
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
 25008 NED_pos_variance = _positionNoise.cwiseAbs2();
884 25008 break;
885 }
886 ✗ case PositionNoiseUnits::meter2:
887 {
888 ✗ Eigen::Vector3d e_positionNoiseStd = trafo::e_Quat_n(posVelAtt->latitude(), posVelAtt->longitude()) * _positionNoise.cwiseSqrt();
889 ✗ if (!e_positionNoiseStd.isZero())
890 {
891 ✗ lla_positionNoiseStd = trafo::ecef2lla_WGS84(posVelAtt->e_position() + e_positionNoiseStd) - posVelAtt->lla_position();
892 }
893 ✗ NED_pos_variance = _positionNoise;
894 ✗ break;
895 }
896 }
897 LOG_DATA("{}: lla_positionNoiseStd = {} [rad, rad, m]", nameId(), lla_positionNoiseStd.transpose());
898
899 // Velocity Noise standard deviation in local-navigation coordinates in [m/s]
900
2/4
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
 25008 Eigen::Vector3d n_velocityNoiseStd = Eigen::Vector3d::Zero();
901
1/3
✓ Branch 0 taken 25008 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 25008 switch (_velocityNoiseUnit)
902 {
903 25008 case VelocityNoiseUnits::m_s:
904
1/2
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
 25008 n_velocityNoiseStd = _velocityNoise;
905
2/4
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
 25008 NED_velocity_variance = _velocityNoise.cwiseAbs2();
906 25008 break;
907 ✗ case VelocityNoiseUnits::m2_s2:
908 ✗ n_velocityNoiseStd = _velocityNoise.cwiseSqrt();
909 ✗ NED_velocity_variance = _velocityNoise;
910 ✗ break;
911 }
912 LOG_DATA("{}: n_velocityNoiseStd = {} [m/s]", nameId(), n_velocityNoiseStd.transpose());
913
914 // Attitude Noise standard deviation in [rad]
915
2/4
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
 25008 Eigen::Vector3d attitudeNoiseStd = Eigen::Vector3d::Zero();
916
1/5
✗ Branch 0 not taken.
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 25008 switch (_attitudeNoiseUnit)
917 {
918 ✗ case AttitudeNoiseUnits::rad:
919 ✗ attitudeNoiseStd = _attitudeNoise;
920 ✗ break;
921 25008 case AttitudeNoiseUnits::deg:
922
1/2
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
 25008 attitudeNoiseStd = deg2rad(_attitudeNoise);
923 25008 break;
924 ✗ case AttitudeNoiseUnits::rad2:
925 ✗ attitudeNoiseStd = _attitudeNoise.cwiseSqrt();
926 ✗ break;
927 ✗ case AttitudeNoiseUnits::deg2:
928 ✗ attitudeNoiseStd = deg2rad(_attitudeNoise.cwiseSqrt());
929 ✗ break;
930 }
931 LOG_DATA("{}: attitudeNoiseStd = {} [rad]", nameId(), attitudeNoiseStd.transpose());
932
933 // #########################################################################################################################################
934
935
1/2
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
 75024 posVelAtt->setStateAndStdDev_n(posVelAtt->lla_position()
936
1/2
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
 25008 + lla_positionBias
937
3/6
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25008 times.
✗ Branch 8 not taken.
 50016 + Eigen::Vector3d{ _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(0)),
938
2/4
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
 25008 _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(1)),
939
3/6
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25008 times.
✗ Branch 8 not taken.
 25008 _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(2)) },
940
941
2/4
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
 25008 NED_pos_variance.asDiagonal().toDenseMatrix(),
942 25008 posVelAtt->n_velocity()
943
1/2
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
 25008 + n_velocityBias
944
3/6
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25008 times.
✗ Branch 8 not taken.
 50016 + Eigen::Vector3d{ _velocityRng.getRand_normalDist(0.0, n_velocityNoiseStd(0)),
945
2/4
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
 25008 _velocityRng.getRand_normalDist(0.0, n_velocityNoiseStd(1)),
946
3/6
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25008 times.
✗ Branch 8 not taken.
 25008 _velocityRng.getRand_normalDist(0.0, n_velocityNoiseStd(2)) },
947
2/4
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
 25008 NED_velocity_variance.asDiagonal().toDenseMatrix(),
948
2/4
✓ Branch 2 taken 25008 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25008 times.
✗ Branch 6 not taken.
 50016 trafo::n_Quat_b(posVelAtt->rollPitchYaw()
949
1/2
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
 25008 + attitudeBias
950
3/6
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25008 times.
✗ Branch 8 not taken.
 50016 + Eigen::Vector3d{ _attitudeRng.getRand_normalDist(0.0, attitudeNoiseStd(0)),
951
2/4
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
 25008 _attitudeRng.getRand_normalDist(0.0, attitudeNoiseStd(1)),
952
3/6
✓ Branch 1 taken 25008 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25008 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25008 times.
✗ Branch 8 not taken.
 25008 _attitudeRng.getRand_normalDist(0.0, attitudeNoiseStd(2)) }));
953
954 50016 return posVelAtt;
955 }
956
957 ✗ std::shared_ptr<NAV::GnssObs> NAV::ErrorModel::receiveGnssObs(const std::shared_ptr<GnssObs>& gnssObs)
958 {
959 LOG_DATA("{}: [{}] Simulating error on GnssObs", nameId(), gnssObs->insTime.toYMDHMS(GPST));
960 ✗ double pseudorangeNoise{}; // [m]
961 ✗ switch (_gui_pseudorangeNoiseUnit)
962 {
963 ✗ case PseudorangeNoiseUnits::meter:
964 ✗ pseudorangeNoise = _gui_pseudorangeNoise;
965 ✗ break;
966 }
967 ✗ double carrierPhaseNoise{}; // [m]
968 ✗ switch (_gui_carrierPhaseNoiseUnit)
969 {
970 ✗ case CarrierPhaseNoiseUnits::meter:
971 ✗ carrierPhaseNoise = _gui_carrierPhaseNoise;
972 ✗ break;
973 }
974 ✗ double dopplerNoise{}; // [m/s]
975 ✗ switch (_gui_dopplerNoiseUnit)
976 {
977 ✗ case DopplerNoiseUnits::m_s:
978 ✗ dopplerNoise = _gui_dopplerNoise;
979 ✗ break;
980 }
981
982 ✗ double dtCycleSlipSeconds{}; // [s]
983 ✗ switch (_gui_cycleSlipFrequencyUnit)
984 {
985 ✗ case CycleSlipFrequencyUnits::per_day:
986 ✗ dtCycleSlipSeconds = InsTimeUtil::SECONDS_PER_DAY / _gui_cycleSlipFrequency;
987 ✗ break;
988 ✗ case CycleSlipFrequencyUnits::per_hour:
989 ✗ dtCycleSlipSeconds = InsTimeUtil::SECONDS_PER_HOUR / _gui_cycleSlipFrequency;
990 ✗ break;
991 ✗ case CycleSlipFrequencyUnits::per_minute:
992 ✗ dtCycleSlipSeconds = InsTimeUtil::SECONDS_PER_MINUTE / _gui_cycleSlipFrequency;
993 ✗ break;
994 }
995 ✗ auto dtCycleSlip = std::chrono::nanoseconds(static_cast<int64_t>(dtCycleSlipSeconds * 1e9));
996
997 ✗ double cycleSlipDetectionProbability{}; // [0, 1]
998 ✗ switch (_gui_cycleSlipDetectionProbabilityUnit)
999 {
1000 ✗ case CycleSlipDetectionProbabilityUnits::percent:
1001 ✗ cycleSlipDetectionProbability = _gui_cycleSlipDetectionProbability / 100.0;
1002 ✗ break;
1003 }
1004
1005 ✗ if (_cycleSlipWindowStartTime.empty()
1006 ✗ || gnssObs->insTime >= _cycleSlipWindowStartTime + dtCycleSlip)
1007 {
1008 ✗ _cycleSlipWindowStartTime = gnssObs->insTime;
1009 LOG_DATA("{}: [{}] Starting new cycle-slip window", nameId(), _cycleSlipWindowStartTime.toYMDHMS(GPST));
1010 }
1011
1012 ✗ size_t nObs = 0;
1013 ✗ for (auto& obs : gnssObs->data)
1014 {
1015 ✗ if (obs.satSigId.freq() & _filterFreq
1016 ✗ && obs.satSigId.code & _filterCode)
1017 {
1018 ✗ nObs++;
1019 }
1020 }
1021
1022 ✗ for (auto& obs : gnssObs->data)
1023 {
1024 ✗ if (obs.pseudorange) { obs.pseudorange.value().value += _pseudorangeRng.getRand_normalDist(0.0, pseudorangeNoise); }
1025 ✗ if (obs.doppler) { obs.doppler.value() += rangeRate2doppler(_dopplerRng.getRand_normalDist(0.0, dopplerNoise), obs.satSigId.freq(), 0); } // TODO: Add frequency number here for GLONASS
1026
1027 ✗ if (obs.carrierPhase)
1028 {
1029 // ------------------------------------------- Noise ---------------------------------------------
1030 ✗ auto lambda = InsConst::C / obs.satSigId.freq().getFrequency(0); // wave-length [m] // TODO: Add frequency number here for GLONASS
1031 ✗ obs.carrierPhase.value().value += _carrierPhaseRng.getRand_normalDist(0.0, carrierPhaseNoise) / lambda;
1032
1033 // ---------------------------------------- Cycle-slip -------------------------------------------
1034
1035 ✗ if (obs.satSigId.freq() & _filterFreq // GUI selected frequencies
1036 ✗ && obs.satSigId.code & _filterCode // GUI selected codes
1037 ✗ && _gui_cycleSlipFrequency != 0.0 // 0 Frequency means disabled
1038 ✗ && !_lastObservationTime.empty() // Do not apply a cycle slip on the first message
1039 ✗ && (_cycleSlips.empty() || _cycleSlips.back().time < _cycleSlipWindowStartTime)) // In the current window, there was no cycle-slip yet
1040 {
1041 ✗ double probabilityCycleSlip = _dt / (dtCycleSlipSeconds * static_cast<double>(nObs)) * 2.0; // Double chance, because often does not happen otherwise
1042 ✗ if (_cycleSlipRng.getRand_uniformRealDist(0.0, 1.0) <= probabilityCycleSlip
1043 ✗ || (gnssObs->insTime >= _cycleSlipWindowStartTime + dtCycleSlip - std::chrono::nanoseconds(static_cast<int64_t>((_dt + 0.001) * 1e9)))) // Last message this window
1044 {
1045 ✗ int newAmbiguity = 0;
1046 ✗ int oldAmbiguity = !_ambiguities[obs.satSigId].empty()
1047 ✗ ? _ambiguities[obs.satSigId].back().second
1048 ✗ : static_cast<int>(_ambiguityRng.getRand_uniformIntDist(_gui_ambiguityLimits[0], _gui_ambiguityLimits[1]));
1049 ✗ auto deltaAmbiguity = static_cast<int>(_cycleSlipRng.getRand_uniformIntDist(1, _gui_cycleSlipRange));
1050 ✗ auto signAmbiguity = _cycleSlipRng.getRand_uniformIntDist(0, 1) == 0.0 ? 1 : -1;
1051
1052 ✗ if (_gui_ambiguityLimits[0] == _gui_ambiguityLimits[1]) // Ambiguities disabled
1053 {
1054 ✗ newAmbiguity = oldAmbiguity + signAmbiguity * deltaAmbiguity;
1055 }
1056 ✗ else if (oldAmbiguity == _gui_ambiguityLimits[0])
1057 {
1058 ✗ newAmbiguity = std::min(oldAmbiguity + deltaAmbiguity, _gui_ambiguityLimits[1]);
1059 }
1060 ✗ else if (oldAmbiguity == _gui_ambiguityLimits[1])
1061 {
1062 ✗ newAmbiguity = std::max(oldAmbiguity - deltaAmbiguity, _gui_ambiguityLimits[0]);
1063 }
1064 else
1065 {
1066 ✗ newAmbiguity = std::clamp(oldAmbiguity + signAmbiguity * deltaAmbiguity, _gui_ambiguityLimits[0], _gui_ambiguityLimits[1]);
1067 }
1068
1069 ✗ _ambiguities[obs.satSigId].emplace_back(gnssObs->insTime, newAmbiguity);
1070
1071 ✗ if (_cycleSlipRng.getRand_uniformRealDist(0.0, 1.0) <= cycleSlipDetectionProbability)
1072 {
1073 ✗ obs.carrierPhase.value().LLI = 1;
1074 }
1075 ✗ _cycleSlips.push_back(CycleSlipInfo{ .time = gnssObs->insTime, .satSigId = obs.satSigId, .LLI = obs.carrierPhase.value().LLI != 0 });
1076 ✗ LOG_DEBUG("{}: [{}] Simulating cycle-slip for satellite [{}] with LLI {}", nameId(), gnssObs->insTime.toYMDHMS(GPST),
1077 obs.satSigId, obs.carrierPhase.value().LLI);
1078 }
1079 }
1080
1081 // ----------------------------------------- Ambiguity -------------------------------------------
1082 ✗ if (!_ambiguities.contains(obs.satSigId) && _gui_ambiguityLimits[0] != _gui_ambiguityLimits[1])
1083 {
1084 ✗ _ambiguities[obs.satSigId].emplace_back(gnssObs->insTime, _ambiguityRng.getRand_uniformIntDist(_gui_ambiguityLimits[0], _gui_ambiguityLimits[1]));
1085 }
1086 ✗ if (_ambiguities.contains(obs.satSigId))
1087 {
1088 ✗ obs.carrierPhase.value().value += _ambiguities.at(obs.satSigId).back().second;
1089 }
1090 }
1091 }
1092
1093 ✗ return gnssObs;
1094 }
1095