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