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