INSTINCT Code Coverage Report


Directory: src/
File: Nodes/DataProcessor/Filter/LowPassFilter.cpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 13 194 6.7%
Functions: 4 21 19.0%
Branches: 12 373 3.2%

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 "LowPassFilter.hpp"
10
11 #include "NodeRegistry.hpp"
12 #include <algorithm>
13 #include <imgui.h>
14 #include "Navigation/INS/Units.hpp"
15 #include "internal/FlowManager.hpp"
16
17 #include "NodeData/IMU/ImuObsSimulated.hpp"
18 #include "NodeData/IMU/ImuObsWDelta.hpp"
19
20 #include "internal/gui/widgets/HelpMarker.hpp"
21 #include "internal/gui/widgets/imgui_ex.hpp"
22 #include "internal/gui/widgets/InputWithUnit.hpp"
23 #include "internal/gui/NodeEditorApplication.hpp"
24
25 #include "Navigation/Transformations/CoordinateFrames.hpp"
26 #include "Navigation/Transformations/Units.hpp"
27 #include "Navigation/GNSS/Functions.hpp"
28
29 #include "util/Eigen.hpp"
30 #include "util/StringUtil.hpp"
31 #include "util/Logger.hpp"
32
33 #include <imgui_internal.h>
34 #include <limits>
35 #include <set>
36 #include <type_traits>
37
38 114 NAV::LowPassFilter::LowPassFilter()
39
2/4
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
114 : Node(typeStatic())
40 {
41 LOG_TRACE("{}: called", name);
42 114 _hasConfig = true;
43 114 _guiConfigDefaultWindowSize = { 500, 300 };
44
45
4/8
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 114 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 114 times.
✓ Branch 9 taken 114 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
342 CreateInputPin("Original", Pin::Type::Flow, { NAV::NodeData::type() }, &LowPassFilter::receiveObs);
46
47
4/8
✓ Branch 2 taken 114 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 114 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 114 times.
✓ Branch 10 taken 114 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
456 CreateOutputPin("Filtered", Pin::Type::Flow, { NAV::NodeData::type() });
48 342 }
49
50 228 NAV::LowPassFilter::~LowPassFilter()
51 {
52 LOG_TRACE("{}: called", nameId());
53 228 }
54
55 228 std::string NAV::LowPassFilter::typeStatic()
56 {
57
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
456 return "LowPassFilter";
58 }
59
60 ✗ std::string NAV::LowPassFilter::type() const
61 {
62 ✗ return typeStatic();
63 }
64
65 114 std::string NAV::LowPassFilter::category()
66 {
67
1/2
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
228 return "Data Processor";
68 }
69
70 ✗ void NAV::LowPassFilter::guiConfig()
71 {
72 ✗ if (outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier.size() != 1)
73 {
74 ✗ ImGui::TextUnformatted("Please connect the input pin to show the options");
75 ✗ return;
76 }
77
78 ✗ ImGui::SetNextItemWidth(400.0F * gui::NodeEditorApplication::defaultFontRatio());
79
80 ✗ if (_gui_availableItemsSelection > _availableItems.size()) { _gui_availableItemsSelection = _availableItems.size() - (_availableItems.empty() ? 0 : 1); }
81 ✗ bool noMoreItems = _filterItems.size() == _availableItems.size();
82 ✗ if (noMoreItems) { ImGui::BeginDisabled(); }
83 ✗ if (ImGui::BeginCombo(fmt::format("##Available data combo {}", size_t(id)).c_str(), !_availableItems.empty() ? _availableItems.at(_gui_availableItemsSelection).c_str() : ""))
84 {
85 ✗ for (size_t i = 0; i < _availableItems.size(); i++)
86 {
87 ✗ const auto& item = _availableItems.at(i);
88 ✗ if (std::ranges::find_if(_filterItems, [&](const FilterItem& filterItem) {
89 ✗ return filterItem.dataDescription == item;
90 })
91 ✗ != _filterItems.end()) { continue; }
92
93 ✗ const bool is_selected = (_gui_availableItemsSelection == i);
94 ✗ if (ImGui::Selectable(item.c_str(), is_selected))
95 {
96 ✗ _gui_availableItemsSelection = i;
97 }
98 ✗ if (is_selected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
99 {
100 ✗ ImGui::SetItemDefaultFocus();
101 }
102 }
103 ✗ ImGui::EndCombo();
104 }
105 ✗ ImGui::SameLine();
106 ✗ if (ImGui::Button(fmt::format("Add##Filter item {}", size_t(id)).c_str()))
107 {
108 ✗ _filterItems.emplace_back(_availableItems.at(_gui_availableItemsSelection), _gui_availableItemsSelection);
109 ✗ flow::ApplyChanges();
110 ✗ bool selectionChanged = false;
111 ✗ for (size_t i = _gui_availableItemsSelection + 1; i < _availableItems.size(); i++)
112 {
113 ✗ const auto& item = _availableItems.at(i);
114 ✗ if (std::ranges::find_if(_filterItems, [&](const FilterItem& filterItem) {
115 ✗ return filterItem.dataDescription == item;
116 })
117 ✗ != _filterItems.end()) { continue; }
118
119 ✗ _gui_availableItemsSelection = i;
120 ✗ selectionChanged = true;
121 ✗ break;
122 }
123 ✗ if (!selectionChanged && _gui_availableItemsSelection != 0)
124 {
125 ✗ for (int i = static_cast<int>(_gui_availableItemsSelection) - 1; i >= 0; i--)
126 {
127 ✗ const auto& item = _availableItems.at(static_cast<size_t>(i));
128 ✗ if (std::ranges::find_if(_filterItems, [&](const FilterItem& filterItem) {
129 ✗ return filterItem.dataDescription == item;
130 })
131 ✗ != _filterItems.end()) { continue; }
132
133 ✗ _gui_availableItemsSelection = static_cast<size_t>(i);
134 ✗ break;
135 }
136 }
137 }
138 ✗ if (noMoreItems) { ImGui::EndDisabled(); }
139
140 ✗ const float COMBO_WIDTH = 100.0F * gui::NodeEditorApplication::windowFontRatio();
141 ✗ const float ITEM_WIDTH = 140.0F * gui::NodeEditorApplication::windowFontRatio();
142
143 ✗ std::optional<size_t> itemToDelete;
144 ✗ for (size_t i = 0; i < _filterItems.size(); i++)
145 {
146 ✗ auto& item = _filterItems.at(i);
147 ✗ bool keep = true;
148 ✗ ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
149 ✗ if (ImGui::CollapsingHeader(fmt::format("{}##{}", item.dataDescription, size_t(id)).c_str(), &keep))
150 {
151 ✗ ImGui::SetNextItemWidth(COMBO_WIDTH);
152 ✗ if (ImGui::BeginCombo(fmt::format("Filter Type##{}", size_t(id)).c_str(), to_string(item.filterType)))
153 {
154 ✗ for (size_t i = 0; i < static_cast<size_t>(FilterType::COUNT); i++)
155 {
156 ✗ const bool is_selected = (static_cast<size_t>(item.filterType) == i);
157 ✗ if (ImGui::Selectable(to_string(static_cast<FilterType>(i)), is_selected))
158 {
159 ✗ item.filterType = static_cast<FilterType>(i);
160 ✗ LOG_DEBUG("{}: filterType changed to {}", nameId(), fmt::underlying(item.filterType));
161 ✗ flow::ApplyChanges();
162 }
163 ✗ if (is_selected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
164 {
165 ✗ ImGui::SetItemDefaultFocus();
166 }
167 }
168 ✗ ImGui::EndCombo();
169 }
170
171 ✗ ImGui::SameLine();
172 ✗ float size = 7.0F * gui::NodeEditorApplication::windowFontRatio();
173 ✗ ImGui::GetWindowDrawList()->AddCircleFilled(ImVec2(ImGui::GetCursorScreenPos().x + size / 1.2F,
174 ✗ ImGui::GetCursorScreenPos().y + size * 1.8F),
175 size,
176 ✗ item.modified
177 ✗ ? ImColor(0.0F, 255.0F, 0.0F)
178 : ImColor(255.0F, 0.0F, 0.0F));
179 ✗ ImGui::Dummy(ImVec2(2 * size, 3.0F * size));
180 ✗ if (ImGui::IsItemHovered())
181 {
182 ✗ ImGui::SetTooltip(item.modified
183 ? "Indicates wether the filter is working."
184 : "Indicates wether the filter is working.\n"
185 "Reasons why it is not working can be:\n"
186 "- Data rate of the incoming values must be greater then 2 * dt\n"
187 "- The data was never included in the observations (dynamic data)\n"
188 "- The data cannot be modified because it is not implemented yet");
189 }
190
191 ✗ ImGui::Indent();
192 ✗ if (item.filterType == FilterType::Linear)
193 {
194 ✗ ImGui::SetNextItemWidth(ITEM_WIDTH);
195 ✗ if (ImGui::InputDoubleL(fmt::format("Cutoff Frequency##{} {}", size_t(id), item.dataDescription).c_str(), &item.linear_filter_cutoff_frequency, 1e-5, 1e5, 0.0, 0.0, "%.5f Hz"))
196 {
197 ✗ LOG_DEBUG("{}: Cutoff Freq. {} changed to {}", nameId(), item.dataDescription, item.linear_filter_cutoff_frequency);
198 ✗ flow::ApplyChanges();
199 }
200 }
201 ✗ ImGui::Unindent();
202 }
203 ✗ if (!keep) { itemToDelete = i; }
204 }
205 ✗ if (itemToDelete) { _filterItems.erase(std::next(_filterItems.begin(), static_cast<int64_t>(*itemToDelete))); }
206 }
207
208 ✗ json NAV::LowPassFilter::save() const
209 {
210 LOG_TRACE("{}: called", nameId());
211
212 ✗ json j;
213
214 ✗ j["availableItems"] = _availableItems;
215 ✗ j["filterItems"] = _filterItems;
216
217 ✗ return j;
218 ✗ }
219
220 ✗ void NAV::LowPassFilter::restore(json const& j)
221 {
222 LOG_TRACE("{}: called", nameId());
223 ✗ if (j.contains("availableItems")) { j.at("availableItems").get_to(_availableItems); }
224 ✗ if (j.contains("filterItems")) { j.at("filterItems").get_to(_filterItems); }
225 ✗ }
226
227 ✗ bool NAV::LowPassFilter::resetNode()
228 {
229 LOG_TRACE("{}: called", nameId());
230
231 ✗ for (auto& item : _filterItems)
232 {
233 ✗ item.dataToFilter.clear();
234 ✗ item.modified = false;
235 }
236
237 ✗ return true;
238 }
239
240 ✗ void NAV::LowPassFilter::afterCreateLink(OutputPin& startPin, [[maybe_unused]] InputPin& endPin)
241 {
242 LOG_TRACE("{}: called for {} ==> {}", nameId(), size_t(startPin.id), size_t(endPin.id));
243
244 ✗ if (endPin.parentNode->id != id)
245 {
246 ✗ return; // Link on Output Port
247 }
248
249 // Store previous output pin identifier
250 ✗ auto previousOutputPinDataIdentifier = outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier;
251 // Overwrite output pin identifier with input pin identifier
252 ✗ outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier = startPin.dataIdentifier;
253
254 ✗ if (previousOutputPinDataIdentifier != outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier) // If the identifier changed
255 {
256 // Check if connected links on output port are still valid
257 ✗ for (auto& link : outputPins.at(OUTPUT_PORT_INDEX_FLOW).links)
258 {
259 ✗ if (auto* endPin = link.getConnectedPin())
260 {
261 ✗ if (!outputPins.at(OUTPUT_PORT_INDEX_FLOW).canCreateLink(*endPin))
262 {
263 // If the link is not valid anymore, delete it
264 ✗ outputPins.at(OUTPUT_PORT_INDEX_FLOW).deleteLink(*endPin);
265 }
266 }
267 }
268
269 // Refresh all links connected to the output pin if the type changed
270 ✗ if (outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier != previousOutputPinDataIdentifier)
271 {
272 ✗ for (auto& link : outputPins.at(OUTPUT_PORT_INDEX_FLOW).links)
273 {
274 ✗ if (auto* connectedPin = link.getConnectedPin())
275 {
276 ✗ outputPins.at(OUTPUT_PORT_INDEX_FLOW).recreateLink(*connectedPin);
277 }
278 }
279 }
280 }
281
282 ✗ if (auto* pin = inputPins.at(INPUT_PORT_INDEX_FLOW).link.getConnectedPin();
283 ✗ pin && _availableItems.empty())
284 {
285 ✗ _gui_availableItemsSelection = 0;
286 ✗ _availableItems = NAV::NodeRegistry::GetStaticDataDescriptors(pin->dataIdentifier);
287 }
288 ✗ }
289
290 ✗ void NAV::LowPassFilter::afterDeleteLink(OutputPin& startPin, InputPin& endPin)
291 {
292 LOG_TRACE("{}: called for {} ==> {}", nameId(), size_t(startPin.id), size_t(endPin.id));
293
294 ✗ _gui_availableItemsSelection = 0;
295 ✗ _availableItems.clear();
296
297 ✗ if ((endPin.parentNode->id != id // Link on Output port is removed
298 ✗ && !inputPins.at(INPUT_PORT_INDEX_FLOW).isPinLinked()) // and the Input port is not linked
299 ✗ || (startPin.parentNode->id != id // Link on Input port is removed
300 ✗ && !outputPins.at(OUTPUT_PORT_INDEX_FLOW).isPinLinked())) // and the Output port is not linked
301 {
302 ✗ outputPins.at(OUTPUT_PORT_INDEX_FLOW).dataIdentifier = { NodeData::type() };
303 }
304 ✗ }
305
306 ✗ void NAV::LowPassFilter::receiveObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
307 {
308 ✗ auto obs = queue.extract_front();
309
310 ✗ for (const auto& desc : obs->dynamicDataDescriptors())
311 {
312 ✗ if (std::ranges::none_of(_availableItems, [&](const auto& header) { return header == desc; }))
313 {
314 ✗ _availableItems.push_back(desc);
315 ✗ flow::ApplyChanges();
316 }
317 ✗ }
318
319 ✗ auto out = NAV::NodeRegistry::CopyNodeData(obs);
320
321 ✗ for (auto& item : _filterItems)
322 {
323 LOG_DATA("{}: [{}] {}", nameId(), item.dataIndex, item.dataDescription);
324 ✗ if (item.dataIndex < out->staticDescriptorCount())
325 {
326 ✗ if (auto value = out->getValueAt(item.dataIndex))
327 {
328 ✗ if (auto newValue = filterData(item, out->insTime, *value))
329 {
330 ✗ item.modified |= out->setValueAt(item.dataIndex, *newValue);
331 }
332 }
333 }
334 ✗ else if (auto value = out->getDynamicDataAt(item.dataDescription))
335 {
336 ✗ if (auto newValue = filterData(item, out->insTime, *value))
337 {
338 ✗ item.modified |= out->setDynamicDataAt(item.dataDescription, *newValue);
339 }
340 }
341 }
342
343 ✗ invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, out);
344 ✗ }
345
346 ✗ std::optional<double> NAV::LowPassFilter::filterData(FilterItem& item, const InsTime& insTime, double value)
347 {
348 ✗ if (item.filterType == FilterType::Linear)
349 {
350 // first we filter accelerations
351 ✗ item.dataToFilter[insTime] = value;
352 // for testing at the moment
353 ✗ double dt = 1.0 / item.linear_filter_cutoff_frequency;
354 // remove all entries that are outside filter time window
355 ✗ std::erase_if(item.dataToFilter, [&](const auto& pair) { return static_cast<double>((insTime - pair.first).count()) > dt; });
356
357 ✗ if (item.dataToFilter.size() > 2)
358 {
359 // average accelerations first
360 ✗ auto N11 = static_cast<double>(item.dataToFilter.size());
361 ✗ double N12 = 0.0;
362 ✗ double N22 = 0.0;
363 ✗ double n1 = 0.0;
364 ✗ double n2 = 0.0;
365 ✗ for (const auto& key_val : item.dataToFilter)
366 {
367 ✗ auto delta_t = static_cast<double>((key_val.first - insTime).count());
368 ✗ N12 += delta_t;
369 ✗ N22 += delta_t * delta_t;
370 ✗ n1 += key_val.second;
371 ✗ n2 += delta_t * key_val.second;
372 }
373 ✗ double determinant_inverse = 1.0 / (N11 * N22 - N12 * N12);
374 ✗ return determinant_inverse * (N22 * n1 - N12 * n2);
375 }
376 }
377 ✗ return {};
378 }
379
380 ✗ const char* NAV::LowPassFilter::to_string(FilterType value)
381 {
382 ✗ switch (value)
383 {
384 ✗ case FilterType::Linear:
385 ✗ return "Linear fit";
386 // case FilterType::Experimental:
387 // return "Experimental";
388 ✗ case FilterType::COUNT:
389 ✗ return "";
390 }
391 ✗ return "";
392 }
393
394 namespace NAV
395 {
396
397 ✗ void to_json(json& j, const LowPassFilter::FilterItem& data)
398 {
399 ✗ j = json{
400 ✗ { "dataDescription", data.dataDescription },
401 ✗ { "filterType", data.filterType },
402 ✗ { "linear_filter_cutoff_frequency", data.linear_filter_cutoff_frequency },
403 ✗ };
404 ✗ }
405
406 ✗ void from_json(const json& j, LowPassFilter::FilterItem& data)
407 {
408 ✗ if (j.contains("dataDescription")) { j.at("dataDescription").get_to(data.dataDescription); }
409 ✗ if (j.contains("filterType")) { j.at("filterType").get_to(data.filterType); }
410 ✗ if (j.contains("linear_filter_cutoff_frequency")) { j.at("linear_filter_cutoff_frequency").get_to(data.linear_filter_cutoff_frequency); }
411 ✗ }
412
413 } // namespace NAV
414