INSTINCT Code Coverage Report

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