0.4.1
Loading...
Searching...
No Matches
WiFiPositioning.cpp
Go to the documentation of this file.
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 "WiFiPositioning.hpp"
10
11#include <algorithm>
12#include <ranges>
13#include <regex>
14#include <fmt/ranges.h>
15
16#include "util/Logger.hpp"
17#include "util/Container/Vector.hpp"
18
19#include "Navigation/Constants.hpp"
20
21#include "internal/gui/NodeEditorApplication.hpp"
22#include "internal/gui/widgets/HelpMarker.hpp"
23#include "internal/gui/widgets/imgui_ex.hpp"
24#include "internal/gui/widgets/InputWithUnit.hpp"
25
26#include "internal/NodeManager.hpp"
27namespace nm = NAV::NodeManager;
28#include "internal/FlowManager.hpp"
29
30#include "NodeData/WiFi/WiFiObs.hpp"
31#include "NodeData/WiFi/WiFiPositioningSolution.hpp"
32#include "Navigation/GNSS/Functions.hpp"
33#include "Navigation/Transformations/CoordinateFrames.hpp"
34#include "Navigation/Transformations/Units.hpp"
35
36#include "Navigation/Math/LeastSquares.hpp"
37
50
55
56std::string NAV::WiFiPositioning::typeStatic()
57{
58 return "WiFiPositioning";
59}
60
61std::string NAV::WiFiPositioning::type() const
62{
63 return typeStatic();
64}
65
66std::string NAV::WiFiPositioning::category()
67{
68 return "Data Processor";
69}
70
71void NAV::WiFiPositioning::guiConfig()
72{
73 float columnWidth = 140.0F * gui::NodeEditorApplication::windowFontRatio();
74 float configWidth = 380.0F * gui::NodeEditorApplication::windowFontRatio();
75 float unitWidth = 150.0F * gui::NodeEditorApplication::windowFontRatio();
76
77 if (ImGui::Button(fmt::format("Add Input Pin##{}", size_t(id)).c_str()))
78 {
79 _nWifiInputPins++;
80 LOG_DEBUG("{}: # Input Pins changed to {}", nameId(), _nWifiInputPins);
81 flow::ApplyChanges();
82 updateNumberOfInputPins();
83 }
84 ImGui::SameLine();
85 if (ImGui::Button(fmt::format("Delete Input Pin##{}", size_t(id)).c_str()))
86 {
87 _nWifiInputPins--;
88 LOG_DEBUG("{}: # Input Pins changed to {}", nameId(), _nWifiInputPins);
89 flow::ApplyChanges();
90 updateNumberOfInputPins();
91 }
92
93 ImGui::SetNextItemWidth(250 * gui::NodeEditorApplication::windowFontRatio());
94
95 // ###########################################################################################################
96 // Frames
97 // ###########################################################################################################
98
99 if (auto frame = static_cast<int>(_frame);
100 ImGui::Combo(fmt::format("Frame##{}", size_t(id)).c_str(), &frame, "ECEF\0LLA\0\0"))
101 {
102 _frame = static_cast<decltype(_frame)>(frame);
103 switch (_frame)
104 {
105 case Frame::ECEF:
106 LOG_DEBUG("{}: Frame changed to ECEF", nameId());
107 for (auto& devPos : _devicePositions)
108 {
109 devPos = trafo::lla2ecef_WGS84(Eigen::Vector3d(deg2rad(devPos.x()), deg2rad(devPos.y()), devPos.z()));
110 }
111 break;
112 case Frame::LLA:
113 LOG_DEBUG("{}: Frame changed to LLA", nameId());
114 for (auto& devPos : _devicePositions)
115 {
116 devPos = trafo::ecef2lla_WGS84(devPos);
117 devPos.x() = rad2deg(devPos.x());
118 devPos.y() = rad2deg(devPos.y());
119 }
120 break;
121 }
122 flow::ApplyChanges();
123 }
124
125 if (ImGui::BeginTable("AccessPointInput", 6, ImGuiTableFlags_Borders | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F)))
126 {
127 // Column headers
128 ImGui::TableSetupColumn("MAC address", ImGuiTableColumnFlags_WidthFixed, columnWidth);
129 if (_frame == Frame::ECEF)
130 {
131 ImGui::TableSetupColumn("X", ImGuiTableColumnFlags_WidthFixed, columnWidth);
132 ImGui::TableSetupColumn("Y", ImGuiTableColumnFlags_WidthFixed, columnWidth);
133 ImGui::TableSetupColumn("Z", ImGuiTableColumnFlags_WidthFixed, columnWidth);
134 }
135 else if (_frame == Frame::LLA)
136 {
137 ImGui::TableSetupColumn("Latitude", ImGuiTableColumnFlags_WidthFixed, columnWidth);
138 ImGui::TableSetupColumn("Longitude", ImGuiTableColumnFlags_WidthFixed, columnWidth);
139 ImGui::TableSetupColumn("Altitude", ImGuiTableColumnFlags_WidthFixed, columnWidth);
140 }
141 ImGui::TableSetupColumn("Bias", ImGuiTableColumnFlags_WidthFixed, columnWidth);
142 ImGui::TableSetupColumn("Scale", ImGuiTableColumnFlags_WidthFixed, columnWidth);
143
144 // Automatic header row
145 ImGui::TableHeadersRow();
146
147 for (size_t rowIndex = 0; rowIndex < _numOfDevices; rowIndex++)
148 {
149 ImGui::TableNextRow();
150 ImGui::TableNextColumn();
151 auto* devPosRow = _devicePositions.at(rowIndex).data();
152
153 // MAC address validation
154 std::regex macRegex("^([0-9A-Fa-f]{2}[:-]){5}([0-9A-Fa-f]{2})$");
155
156 ImGui::SetNextItemWidth(columnWidth);
157 if (ImGui::InputText(fmt::format("##Mac{}", rowIndex).c_str(), &_deviceMacAddresses.at(rowIndex), ImGuiInputTextFlags_None))
158 {
159 std::transform(_deviceMacAddresses.at(rowIndex).begin(), _deviceMacAddresses.at(rowIndex).end(), _deviceMacAddresses.at(rowIndex).begin(), ::toupper); // Convert to uppercase
160 if (!std::regex_match(_deviceMacAddresses.at(rowIndex), macRegex))
161 {
162 _deviceMacAddresses.at(rowIndex) = "00:00:00:00:00:00";
163 LOG_DEBUG("{}: Invalid MAC address", nameId());
164 }
165 else
166 {
167 flow::ApplyChanges();
168 }
169 }
170 if (_frame == Frame::ECEF)
171 {
172 ImGui::TableNextColumn();
173 ImGui::SetNextItemWidth(columnWidth);
174 if (ImGui::InputDouble(fmt::format("##InputX{}", rowIndex).c_str(), &devPosRow[0], 0.0, 0.0, "%.4fm"))
175 {
176 flow::ApplyChanges();
177 }
178 ImGui::TableNextColumn();
179 ImGui::SetNextItemWidth(columnWidth);
180 if (ImGui::InputDouble(fmt::format("##InputY{}", rowIndex).c_str(), &devPosRow[1], 0.0, 0.0, "%.4fm"))
181 {
182 flow::ApplyChanges();
183 }
184 ImGui::TableNextColumn();
185 ImGui::SetNextItemWidth(columnWidth);
186 if (ImGui::InputDouble(fmt::format("##InputZ{}", rowIndex).c_str(), &devPosRow[2], 0.0, 0.0, "%.4fm"))
187 {
188 flow::ApplyChanges();
189 }
190 }
191 else if (_frame == Frame::LLA)
192 {
193 ImGui::TableNextColumn();
194 ImGui::SetNextItemWidth(columnWidth);
195 if (ImGui::InputDoubleL(fmt::format("##InputLat{}", rowIndex).c_str(), &devPosRow[0], -180, 180, 0.0, 0.0, "%.8f°"))
196 {
197 flow::ApplyChanges();
198 }
199 ImGui::TableNextColumn();
200 ImGui::SetNextItemWidth(columnWidth);
201 if (ImGui::InputDoubleL(fmt::format("##InputLon{}", rowIndex).c_str(), &devPosRow[1], -180, 180, 0.0, 0.0, "%.8f°"))
202 {
203 flow::ApplyChanges();
204 }
205 ImGui::TableNextColumn();
206 ImGui::SetNextItemWidth(columnWidth);
207 if (ImGui::InputDouble(fmt::format("##InputHeight{}", rowIndex).c_str(), &devPosRow[2], 0.0, 0.0, "%.4fm"))
208 {
209 flow::ApplyChanges();
210 }
211 }
212 ImGui::TableNextColumn();
213 ImGui::SetNextItemWidth(columnWidth);
214 if (ImGui::InputDouble(fmt::format("##InputBias{}", rowIndex).c_str(), &_deviceBias.at(rowIndex), 0.0, 0.0, "%.4fm"))
215 {
216 flow::ApplyChanges();
217 }
218 ImGui::TableNextColumn();
219 ImGui::SetNextItemWidth(columnWidth);
220 if (ImGui::InputDouble(fmt::format("##InputScale{}", rowIndex).c_str(), &_deviceScale.at(rowIndex), 0.0, 0.0, "%.4f"))
221 {
222 flow::ApplyChanges();
223 }
224 }
225 ImGui::EndTable();
226 }
227 if (ImGui::Button(fmt::format("Add Device##{}", size_t(id)).c_str(), ImVec2(columnWidth * 2.1F, 0)))
228 {
229 _numOfDevices++;
230 _deviceMacAddresses.emplace_back("00:00:00:00:00:00");
231 _devicePositions.emplace_back(Eigen::Vector3d::Zero());
232 _deviceBias.emplace_back(0.0);
233 _deviceScale.emplace_back(0.0);
234 flow::ApplyChanges();
235 }
236 ImGui::SameLine();
237 if (ImGui::Button(fmt::format("Delete Device##{}", size_t(id)).c_str(), ImVec2(columnWidth * 2.1F, 0)))
238 {
239 if (_numOfDevices > 0)
240 {
241 _numOfDevices--;
242 _deviceMacAddresses.pop_back();
243 _devicePositions.pop_back();
244 _deviceBias.pop_back();
245 _deviceScale.pop_back();
246 flow::ApplyChanges();
247 }
248 }
249 ImGui::Separator();
250 if (auto solutionMode = static_cast<int>(_solutionMode);
251 ImGui::Combo(fmt::format("Solution##{}", size_t(id)).c_str(), &solutionMode, "Least squares\0Kalman Filter\0\0"))
252 {
253 _solutionMode = static_cast<decltype(_solutionMode)>(solutionMode);
254 switch (_solutionMode)
255 {
256 case SolutionMode::LSQ:
257 LOG_DEBUG("{}: Solution changed to Least squares 3D", nameId());
258 break;
259 case SolutionMode::KF:
260 LOG_DEBUG("{}: Solution changed to Kalman Filter", nameId());
261 break;
262 }
263 flow::ApplyChanges();
264 }
265
266 // ###########################################################################################################
267 // Least Squares
268 // ###########################################################################################################
269 if (_solutionMode == SolutionMode::LSQ)
270 {
271 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
272 if (ImGui::TreeNode(fmt::format("x0 - Initial State##{}", size_t(id)).c_str()))
273 {
274 Eigen::Vector3d llaPos = trafo::ecef2lla_WGS84(_initialState.e_position);
275 llaPos.block<2, 1>(0, 0) = rad2deg(llaPos.block<2, 1>(0, 0));
276
277 ImGui::SetNextItemWidth(configWidth);
278 if (ImGui::InputDouble3(fmt::format("Position ECEF (m)##{}", "m",
279 size_t(id))
280 .c_str(),
281 _initialState.e_position.data(), "%.4f", ImGuiInputTextFlags_CharsScientific))
282 {
283 LOG_DEBUG("{}: e_position changed to {}", nameId(), _initialState.e_position);
284 flow::ApplyChanges();
285 }
286
287 ImGui::SetNextItemWidth(configWidth);
288 if (ImGui::InputDouble3(fmt::format("Position LLA (°,°,m)##{}", "(°,°,m)",
289 size_t(id))
290 .c_str(),
291 llaPos.data(), "%.8f", ImGuiInputTextFlags_CharsScientific))
292 {
293 llaPos.block<2, 1>(0, 0) = deg2rad(llaPos.block<2, 1>(0, 0));
294 _initialState.e_position = trafo::lla2ecef_WGS84(llaPos);
295 LOG_DEBUG("{}: e_position changed to {}", nameId(), _initialState.e_position);
296 flow::ApplyChanges();
297 }
298
299 if (_estimateBias)
300 {
301 ImGui::SetNextItemWidth(configWidth);
302 if (ImGui::InputDouble(fmt::format("Bias (m)##{}", "m",
303 size_t(id))
304 .c_str(),
305 &_initialState.bias, 0, 0, "%.3e", ImGuiInputTextFlags_CharsScientific))
306 {
307 LOG_DEBUG("{}: bias changed to {}", nameId(), _initialState.bias);
308 flow::ApplyChanges();
309 }
310 }
311
312 ImGui::TreePop();
313 }
314 }
315 // ###########################################################################################################
316 // Kalman Filter
317 // ###########################################################################################################
318 if (_solutionMode == SolutionMode::KF)
319 {
320 // ###########################################################################################################
321 // Measurement Uncertainties 𝐑
322 // ###########################################################################################################
323
324 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
325 if (ImGui::TreeNode(fmt::format("R - Measurement Noise ##{}", size_t(id)).c_str()))
326 {
327 if (gui::widgets::InputDoubleWithUnit(fmt::format("({})##{}",
328 _measurementNoiseUnit == MeasurementNoiseUnit::meter2
329 ? "Variance σ²"
330 : "Standard deviation σ",
331 size_t(id))
332 .c_str(),
333 configWidth, unitWidth, &_measurementNoise, _measurementNoiseUnit, "m^2, m^2, m^2\0"
334 "m, m, m\0\0",
335 0, 0, "%.3e", ImGuiInputTextFlags_CharsScientific))
336 {
337 LOG_DEBUG("{}: measurementNoise changed to {}", nameId(), _measurementNoise);
338 LOG_DEBUG("{}: measurementNoiseUnit changed to {}", nameId(), fmt::underlying(_measurementNoiseUnit));
339 flow::ApplyChanges();
340 }
341 ImGui::TreePop();
342 }
343
344 // ###########################################################################################################
345 // Process Noise Covariance 𝐐
346 // ###########################################################################################################
347 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
348 if (ImGui::TreeNode(fmt::format("Q - Process Noise ##{}", size_t(id)).c_str()))
349 {
350 if (gui::widgets::InputDoubleWithUnit(fmt::format("({})##{}",
351 _processNoiseUnit == ProcessNoiseUnit::meter2
352 ? "Variance σ²"
353 : "Standard deviation σ",
354 size_t(id))
355 .c_str(),
356 configWidth, unitWidth, &_processNoise, _processNoiseUnit, "m^2, m^2, m^2\0"
357 "m, m, m\0\0",
358 0, 0, "%.3e", ImGuiInputTextFlags_CharsScientific))
359 {
360 LOG_DEBUG("{}: processNoise changed to {}", nameId(), _processNoise);
361 LOG_DEBUG("{}: processNoiseUnit changed to {}", nameId(), fmt::underlying(_processNoiseUnit));
362 flow::ApplyChanges();
363 }
364 ImGui::TreePop();
365 }
366
367 // ###########################################################################################################
368 // Initial State Estimate 𝐱0
369 // ###########################################################################################################
370 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
371 if (ImGui::TreeNode(fmt::format("x0 - Initial State##{}", size_t(id)).c_str()))
372 {
373 Eigen::Vector3d llaPos = trafo::ecef2lla_WGS84(_initialState.e_position);
374 llaPos.block<2, 1>(0, 0) = rad2deg(llaPos.block<2, 1>(0, 0));
375
376 ImGui::SetNextItemWidth(configWidth);
377 if (ImGui::InputDouble3(fmt::format("Position ECEF (m)##{}", "m",
378 size_t(id))
379 .c_str(),
380 _initialState.e_position.data(), "%.4f", ImGuiInputTextFlags_CharsScientific))
381 {
382 LOG_DEBUG("{}: e_position changed to {}", nameId(), _initialState.e_position);
383 flow::ApplyChanges();
384 }
385
386 ImGui::SetNextItemWidth(configWidth);
387 if (ImGui::InputDouble3(fmt::format("Position LLA ##{}", "m",
388 size_t(id))
389 .c_str(),
390 llaPos.data(), "%.8f°", ImGuiInputTextFlags_CharsScientific))
391 {
392 llaPos.block<2, 1>(0, 0) = deg2rad(llaPos.block<2, 1>(0, 0));
393 _initialState.e_position = trafo::lla2ecef_WGS84(llaPos);
394 LOG_DEBUG("{}: e_position changed to {}", nameId(), _initialState.e_position);
395 flow::ApplyChanges();
396 }
397
398 ImGui::SetNextItemWidth(configWidth);
399 if (ImGui::InputDouble3(fmt::format("Velocity (m/s)##{}", "m",
400 size_t(id))
401 .c_str(),
402 _state.e_velocity.data(), "%.3e", ImGuiInputTextFlags_CharsScientific))
403 {
404 LOG_DEBUG("{}: e_position changed to {}", nameId(), _state.e_velocity);
405 flow::ApplyChanges();
406 }
407
408 if (_estimateBias)
409 {
410 ImGui::SetNextItemWidth(configWidth);
411 if (ImGui::InputDouble(fmt::format("Bias (m)##{}", "m",
412 size_t(id))
413 .c_str(),
414 &_state.bias, 0, 0, "%.3e", ImGuiInputTextFlags_CharsScientific))
415 {
416 LOG_DEBUG("{}: bias changed to {}", nameId(), _state.bias);
417 flow::ApplyChanges();
418 }
419 }
420
421 ImGui::TreePop();
422 }
423
424 // ###########################################################################################################
425 // 𝐏 Error covariance matrix
426 // ###########################################################################################################
427
428 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
429 if (ImGui::TreeNode(fmt::format("P - Error covariance matrix (init)##{}", size_t(id)).c_str()))
430 {
431 if (gui::widgets::InputDouble3WithUnit(fmt::format("Position covariance ({})##{}",
432 _initCovariancePositionUnit == InitCovariancePositionUnit::meter2
433 ? "Variance σ²"
434 : "Standard deviation σ",
435 size_t(id))
436 .c_str(),
437 configWidth, unitWidth, _initCovariancePosition.data(), _initCovariancePositionUnit, "m^2, m^2, m^2\0"
438 "m, m, m\0\0",
439 "%.2e", ImGuiInputTextFlags_CharsScientific))
440 {
441 LOG_DEBUG("{}: initCovariancePosition changed to {}", nameId(), _initCovariancePosition);
442 LOG_DEBUG("{}: initCovariancePositionUnit changed to {}", nameId(), fmt::underlying(_initCovariancePositionUnit));
443 flow::ApplyChanges();
444 }
445
446 if (gui::widgets::InputDouble3WithUnit(fmt::format("Velocity covariance ({})##{}",
447 _initCovarianceVelocityUnit == InitCovarianceVelocityUnit::m2_s2
448 ? "Variance σ²"
449 : "Standard deviation σ",
450 size_t(id))
451 .c_str(),
452 configWidth, unitWidth, _initCovarianceVelocity.data(), _initCovarianceVelocityUnit, "m^2/s^2\0"
453 "m/s\0\0",
454 "%.2e", ImGuiInputTextFlags_CharsScientific))
455 {
456 LOG_DEBUG("{}: initCovarianceVelocity changed to {}", nameId(), _initCovarianceVelocity);
457 LOG_DEBUG("{}: initCovarianceVelocityUnit changed to {}", nameId(), fmt::underlying(_initCovarianceVelocityUnit));
458 flow::ApplyChanges();
459 }
460
461 if (_estimateBias)
462 {
463 if (gui::widgets::InputDoubleWithUnit(fmt::format("Bias covariance ({})##{}",
464 _initCovarianceBiasUnit == InitCovarianceBiasUnit::meter2
465 ? "Variance σ²"
466 : "Standard deviation σ",
467 size_t(id))
468 .c_str(),
469 configWidth, unitWidth, &_initCovarianceBias, _initCovarianceBiasUnit, "m^2\0"
470 "m\0\0",
471 0, 0, "%.2e", ImGuiInputTextFlags_CharsScientific))
472 {
473 LOG_DEBUG("{}: initCovarianceBias changed to {}", nameId(), _initCovarianceBias);
474 LOG_DEBUG("{}: initCovarianceBiasUnit changed to {}", nameId(), fmt::underlying(_initCovarianceBiasUnit));
475 flow::ApplyChanges();
476 }
477 }
478
479 ImGui::TreePop();
480 }
481 }
482 ImGui::Separator();
483 // ###########################################################################################################
484 // Estimate Bias
485 // ###########################################################################################################
486 if (ImGui::Checkbox(fmt::format("Estimate Bias##{}", size_t(id)).c_str(), &_estimateBias))
487 {
488 if (_estimateBias)
489 {
490 LOG_DEBUG("{}: Estimate Bias changed to Yes", nameId());
491 _numStates = 7;
492 }
493 else
494 {
495 LOG_DEBUG("{}: Estimate Bias changed to No", nameId());
496 _numStates = 6;
497 }
498 flow::ApplyChanges();
499 }
500 // ###########################################################################################################
501 // Weighted Solution
502 // ###########################################################################################################
503 if (ImGui::Checkbox(fmt::format("Weighted Solution##{}", size_t(id)).c_str(), &_weightedSolution))
504 {
505 if (_weightedSolution)
506 {
507 LOG_DEBUG("{}: Weighted Solution changed to Yes", nameId());
508 }
509 else
510 {
511 LOG_DEBUG("{}: Weighted Solution changed to No", nameId());
512 }
513 flow::ApplyChanges();
514 }
515
516 // ###########################################################################################################
517 // Use Initial Values
518 // ###########################################################################################################
519 if (_solutionMode == SolutionMode::LSQ)
520 {
521 if (ImGui::Checkbox(fmt::format("Use Initial Values##{}", size_t(id)).c_str(), &_useInitialValues))
522 {
523 if (_useInitialValues)
524 {
525 LOG_DEBUG("{}: Use Initial Values changed to Yes", nameId());
526 }
527 else
528 {
529 LOG_DEBUG("{}: Use Initial Values changed to No", nameId());
530 }
531 flow::ApplyChanges();
532 }
533 }
534}
535
536[[nodiscard]] json NAV::WiFiPositioning::save() const
537{
538 LOG_TRACE("{}: called", nameId());
539
540 json j;
541
542 j["nWifiInputPins"] = _nWifiInputPins;
543 j["numStates"] = _numStates;
544 j["numMeasurements"] = _numMeasurements;
545 j["frame"] = _frame;
546 j["estimateBias"] = _estimateBias;
547 j["weightedSolution"] = _weightedSolution;
548 j["useInitialValues"] = _useInitialValues;
549 j["deviceMacAddresses"] = _deviceMacAddresses;
550 j["devicePositions"] = _devicePositions;
551 j["deviceBias"] = _deviceBias;
552 j["deviceScale"] = _deviceScale;
553 j["numOfDevices"] = _numOfDevices;
554 j["solutionMode"] = _solutionMode;
555 j["e_position"] = _state.e_position;
556 j["e_velocity"] = _state.e_velocity;
557 j["bias"] = _state.bias;
558 j["intialStatePosition"] = _initialState.e_position;
559 j["initialStateVelocity"] = _initialState.e_velocity;
560 j["initialStateBias"] = _initialState.bias;
561 j["initCovariancePosition"] = _initCovariancePosition;
562 j["initCovariancePositionUnit"] = _initCovariancePositionUnit;
563 j["initCovarianceVelocity"] = _initCovarianceVelocity;
564 j["initCovarianceVelocityUnit"] = _initCovarianceVelocityUnit;
565 j["initCovarianceBias"] = _initCovarianceBias;
566 j["initCovarianceBiasUnit"] = _initCovarianceBiasUnit;
567 j["measurementNoise"] = _measurementNoise;
568 j["measurementNoiseUnit"] = _measurementNoiseUnit;
569 j["processNoise"] = _processNoise;
570 j["processNoiseUnit"] = _processNoiseUnit;
571
572 return j;
573}
574
575void NAV::WiFiPositioning::restore(json const& j)
576{
577 LOG_TRACE("{}: called", nameId());
578
579 if (j.contains("nWifiInputPins"))
580 {
581 j.at("nWifiInputPins").get_to(_nWifiInputPins);
582 updateNumberOfInputPins();
583 }
584 if (j.contains("numStates"))
585 {
586 j.at("numStates").get_to(_numStates);
587 }
588 if (j.contains("numMeasurements"))
589 {
590 j.at("numMeasurements").get_to(_numMeasurements);
591 }
592 if (j.contains("frame"))
593 {
594 j.at("frame").get_to(_frame);
595 }
596 if (j.contains("estimateBias"))
597 {
598 j.at("estimateBias").get_to(_estimateBias);
599 }
600 if (j.contains("weightedSolution"))
601 {
602 j.at("weightedSolution").get_to(_weightedSolution);
603 }
604 if (j.contains("useInitialValues"))
605 {
606 j.at("useInitialValues").get_to(_useInitialValues);
607 }
608 if (j.contains("deviceMacAddresses"))
609 {
610 j.at("deviceMacAddresses").get_to(_deviceMacAddresses);
611 }
612 if (j.contains("devicePositions"))
613 {
614 j.at("devicePositions").get_to(_devicePositions);
615 }
616 if (j.contains("deviceBias"))
617 {
618 j.at("deviceBias").get_to(_deviceBias);
619 }
620 if (j.contains("deviceScale"))
621 {
622 j.at("deviceScale").get_to(_deviceScale);
623 }
624 if (j.contains("numOfDevices"))
625 {
626 j.at("numOfDevices").get_to(_numOfDevices);
627 }
628 if (j.contains("solutionMode"))
629 {
630 j.at("solutionMode").get_to(_solutionMode);
631 }
632 if (j.contains("e_position"))
633 {
634 j.at("e_position").get_to(_state.e_position);
635 }
636 if (j.contains("e_velocity"))
637 {
638 j.at("e_velocity").get_to(_state.e_velocity);
639 }
640 if (j.contains("bias"))
641 {
642 j.at("bias").get_to(_state.bias);
643 }
644 if (j.contains("intialStatePosition"))
645 {
646 j.at("intialStatePosition").get_to(_initialState.e_position);
647 }
648 if (j.contains("initialStateVelocity"))
649 {
650 j.at("initialStateVelocity").get_to(_initialState.e_velocity);
651 }
652 if (j.contains("initialStateBias"))
653 {
654 j.at("initialStateBias").get_to(_initialState.bias);
655 }
656 if (j.contains("initCovariancePosition"))
657 {
658 j.at("initCovariancePosition").get_to(_initCovariancePosition);
659 }
660 if (j.contains("initCovariancePositionUnit"))
661 {
662 j.at("initCovariancePositionUnit").get_to(_initCovariancePositionUnit);
663 }
664 if (j.contains("initCovarianceVelocity"))
665 {
666 j.at("initCovarianceVelocity").get_to(_initCovarianceVelocity);
667 }
668 if (j.contains("initCovarianceVelocityUnit"))
669 {
670 j.at("initCovarianceVelocityUnit").get_to(_initCovarianceVelocityUnit);
671 }
672 if (j.contains("initCovarianceBias"))
673 {
674 j.at("initCovarianceBias").get_to(_initCovarianceBias);
675 }
676 if (j.contains("initCovarianceBiasUnit"))
677 {
678 j.at("initCovarianceBiasUnit").get_to(_initCovarianceBiasUnit);
679 }
680 if (j.contains("measurementNoise"))
681 {
682 j.at("measurementNoise").get_to(_measurementNoise);
683 }
684 if (j.contains("measurementNoiseUnit"))
685 {
686 j.at("measurementNoiseUnit").get_to(_measurementNoiseUnit);
687 }
688 if (j.contains("processNoise"))
689 {
690 j.at("processNoise").get_to(_processNoise);
691 }
692 if (j.contains("processNoiseUnit"))
693 {
694 j.at("processNoiseUnit").get_to(_processNoiseUnit);
695 }
696}
697
698bool NAV::WiFiPositioning::initialize()
699{
700 LOG_TRACE("{}: called", nameId());
701
702 _kalmanFilter = KalmanFilter{ _numStates, _numMeasurements };
703
704 // Initial state
705 _state.e_position = _initialState.e_position;
706 _state.e_velocity = _initialState.e_velocity;
707 if (_estimateBias)
708 {
709 _state.bias = _initialState.bias;
710 }
711
712 // Initial Covariance of the velocity in [m²/s²]
713 Eigen::Vector3d variance_vel = Eigen::Vector3d::Zero();
714 if (_initCovarianceVelocityUnit == InitCovarianceVelocityUnit::m2_s2)
715 {
716 variance_vel = _initCovarianceVelocity;
717 }
718 else if (_initCovarianceVelocityUnit == InitCovarianceVelocityUnit::m_s)
719 {
720 variance_vel = _initCovarianceVelocity.array().pow(2);
721 }
722
723 // Initial Covariance of the position in [m²]
724 Eigen::Vector3d variance_pos = Eigen::Vector3d::Zero();
725 if (_initCovariancePositionUnit == InitCovariancePositionUnit::meter2)
726 {
727 variance_pos = _initCovariancePosition;
728 }
729 else if (_initCovariancePositionUnit == InitCovariancePositionUnit::meter)
730 {
731 variance_pos = _initCovariancePosition.array().pow(2);
732 }
733
734 // Initial Covariance of the bias in [m²]
735 double variance_bias = 0.0;
736 if (_initCovarianceBiasUnit == InitCovarianceBiasUnit::meter2)
737 {
738 variance_bias = _initCovarianceBias;
739 }
740 else if (_initCovarianceBiasUnit == InitCovarianceBiasUnit::meter)
741 {
742 variance_bias = std::pow(_initCovarianceBias, 2);
743 }
744 if (_estimateBias)
745 {
746 _kalmanFilter.P.diagonal() << variance_pos, variance_vel, variance_bias;
747 }
748 else
749 {
750 _kalmanFilter.P.diagonal() << variance_pos, variance_vel;
751 }
752 if (_estimateBias)
753 {
754 _kalmanFilter.x << _state.e_position, _state.e_velocity, _state.bias;
755 }
756 else
757 {
758 _kalmanFilter.x << _state.e_position, _state.e_velocity;
759 }
760 if (_measurementNoiseUnit == MeasurementNoiseUnit::meter2)
761 {
762 _kalmanFilter.R << _measurementNoise;
763 }
764 else if (_measurementNoiseUnit == MeasurementNoiseUnit::meter)
765 {
766 _kalmanFilter.R << std::pow(_measurementNoise, 2);
767 }
768
769 LOG_DEBUG("WiFiPositioning initialized");
770
771 return true;
772}
773
774void NAV::WiFiPositioning::deinitialize()
775{
776 LOG_TRACE("{}: called", nameId());
777}
778
790
791void NAV::WiFiPositioning::recvWiFiObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
792{
793 auto obs = std::static_pointer_cast<const WiFiObs>(queue.extract_front());
794 auto it = std::ranges::find(_deviceMacAddresses, obs->macAddress);
795 if (it != _deviceMacAddresses.end()) // Check if the MAC address is in the list
796 {
797 // Get the index of the found element
798 auto index = static_cast<size_t>(std::distance(_deviceMacAddresses.begin(), it));
799
800 // Check if a device with the same position already exists and update the distance
801 bool deviceExists = false;
802 for (auto& device : _devices)
803 {
804 if (_frame == Frame::ECEF)
805 {
806 if (device.position == _devicePositions.at(index))
807 {
808 deviceExists = true;
809 device.distance = obs->distance * _deviceScale.at(index) + _deviceBias.at(index);
810 device.distanceStd = obs->distanceStd * _deviceScale.at(index);
811 device.time = obs->insTime;
812 break;
813 }
814 }
815 else if (_frame == Frame::LLA)
816 {
817 Eigen::Vector3d ecefPos = _devicePositions.at(index);
818 ecefPos.block<2, 1>(0, 0) = deg2rad(ecefPos.block<2, 1>(0, 0));
819 ecefPos = trafo::lla2ecef_WGS84(ecefPos);
820 if (device.position == ecefPos)
821 {
822 deviceExists = true;
823 device.distance = obs->distance * _deviceScale.at(index) + _deviceBias.at(index);
824 device.distanceStd = obs->distanceStd * _deviceScale.at(index);
825 device.time = obs->insTime;
826 break;
827 }
828 }
829 }
830
831 // If the device does not exist, add it to the list
832 if (!deviceExists)
833 {
834 if (_frame == Frame::LLA)
835 {
836 Eigen::Vector3d llaPos = _devicePositions.at(index);
837 llaPos.block<2, 1>(0, 0) = deg2rad(llaPos.block<2, 1>(0, 0));
838 _devices.push_back({ trafo::lla2ecef_WGS84(llaPos), obs->insTime, obs->distance * _deviceScale.at(index) + _deviceBias.at(index), obs->distanceStd * _deviceScale.at(index) });
839 }
840 else if (_frame == Frame::ECEF)
841 {
842 _devices.push_back({ _devicePositions.at(index), obs->insTime, obs->distance * _deviceScale.at(index) + _deviceBias.at(index), obs->distanceStd * _deviceScale.at(index) });
843 }
844 }
845
846 // Calculate the solution
847 auto wifiPositioningSolution = std::make_shared<NAV::WiFiPositioningSolution>();
848 wifiPositioningSolution->insTime = obs->insTime;
849 // Least Squares
850 if (_solutionMode == SolutionMode::LSQ)
851 {
852 if (_devices.size() == _numOfDevices)
853 {
854 LeastSquaresResult<Eigen::VectorXd, Eigen::MatrixXd> lsqSolution = WiFiPositioning::lsqSolution();
855 wifiPositioningSolution->setPositionAndCov_e(lsqSolution.solution.block<3, 1>(0, 0),
856 lsqSolution.variance.block<3, 3>(0, 0));
857 if (wifiPositioningSolution->lla_position().hasNaN())
858 {
859 LOG_WARN("{}: WiFi LSQ calculation failed, invalid position", nameId());
860 return;
861 }
862 if (_estimateBias)
863 {
864 wifiPositioningSolution->bias = _state.bias;
865 wifiPositioningSolution->biasStdev = lsqSolution.variance(3, 3);
866 }
867 invokeCallbacks(OUTPUT_PORT_INDEX_WIFISOL, wifiPositioningSolution);
868 }
869 }
870 // Kalman Filter
871 else if (_solutionMode == SolutionMode::KF)
872 {
873 WiFiPositioning::kfSolution();
874 wifiPositioningSolution->setPosVelAndCov_e(_kalmanFilter.x.block<3, 1>(0, 0),
875 _kalmanFilter.x.block<3, 1>(3, 0),
876 _kalmanFilter.P.block<6, 6>(0, 0));
877 if (wifiPositioningSolution->lla_position().hasNaN())
878 {
879 LOG_WARN("{}: WiFi KF calculation failed, invalid position", nameId());
880 return;
881 }
882 if (_estimateBias)
883 {
884 wifiPositioningSolution->bias = _kalmanFilter.x(6);
885 wifiPositioningSolution->biasStdev = _kalmanFilter.P(6, 6);
886 }
887 invokeCallbacks(OUTPUT_PORT_INDEX_WIFISOL, wifiPositioningSolution);
888 }
889
890 LOG_DATA("{}: [{}] Received distance to device {} at position {} with distance {}", nameId(), obs->insTime.toYMDHMS(GPST),
891 obs->macAddress, _devicePositions.at(index).transpose(), obs->distance);
892 }
893}
894
895NAV::LeastSquaresResult<Eigen::VectorXd, Eigen::MatrixXd> NAV::WiFiPositioning::lsqSolution()
896{
897 LeastSquaresResult<Eigen::VectorXd, Eigen::MatrixXd> lsq;
898 int n = (_estimateBias) ? 4 : 3; // Number of unknowns
899
900 // Check if the number of devices is sufficient to compute the position
901 if ((_estimateBias && _devices.size() < 5) || (!_estimateBias && _devices.size() < 4))
902 {
903 LOG_WARN("{}: Received less than {} observations. Can't compute position", nameId(), (_estimateBias ? 5 : 4));
904 return lsq;
905 }
906
907 LOG_DATA("{}: Received {} observations", nameId(), _devices.size());
908
909 Eigen::MatrixXd e_H = Eigen::MatrixXd::Zero(static_cast<int>(_devices.size()), n);
910 Eigen::MatrixXd W = Eigen::MatrixXd::Identity(static_cast<int>(_devices.size()), static_cast<int>(_devices.size()));
911 Eigen::VectorXd ddist = Eigen::VectorXd::Zero(static_cast<int>(_devices.size()));
912 size_t numMeasurements = _devices.size();
913
914 // Check if the initial position is NaN
915 if (std::isnan(_state.e_position(0)) || std::isnan(_state.e_position(1)) || std::isnan(_state.e_position(2)) || _useInitialValues)
916 {
917 _state.e_position << _initialState.e_position;
918 if (_estimateBias)
919 {
920 _state.bias = _initialState.bias;
921 }
922 }
923
924 // Iteratively solve the linear least squares problem
925 for (size_t o = 0; o < 15; o++)
926 {
927 LOG_DATA("{}: Iteration {}", nameId(), o);
928 for (size_t i = 0; i < numMeasurements; i++)
929 {
930 // Calculate the distance between the device and the estimated position
931 double distEst = (_devices.at(i).position - _state.e_position).norm();
932 if (_estimateBias)
933 {
934 distEst += _state.bias;
935 }
936
937 // Calculate the residual vector
938 ddist(static_cast<int>(i)) = _devices.at(i).distance - distEst;
939
940 Eigen::Vector3d e_lineOfSightUnitVector = Eigen::Vector3d::Zero();
941 if ((_state.e_position - _devices.at(i).position).norm() != 0) // Check if it is the same position
942 {
943 e_lineOfSightUnitVector = e_calcLineOfSightUnitVector(_state.e_position, _devices.at(i).position);
944 }
945
946 // Calculate the design matrix
947 e_H.block<1, 3>(static_cast<int>(i), 0) = -e_lineOfSightUnitVector;
948 if (_estimateBias)
949 {
950 e_H(static_cast<int>(i), 3) = 1;
951 }
952
953 // Calculate the weight matrix
954 if (_weightedSolution)
955 {
956 W(static_cast<int>(i), static_cast<int>(i)) = 1 / std::pow(_devices.at(i).distanceStd, 2);
957 }
958 }
959 // Solve the linear least squares problem
960 lsq = solveWeightedLinearLeastSquaresUncertainties(e_H, W, ddist);
961
962 if (_estimateBias)
963 {
964 LOG_DATA("{}: [{}] dx (lsq) {}, {}, {}, {}", nameId(), o, lsq.solution(0), lsq.solution(1), lsq.solution(2), lsq.solution(3));
965 LOG_DATA("{}: [{}] stdev_dx (lsq)\n{}", nameId(), o, lsq.variance.cwiseSqrt());
966 }
967 else
968 {
969 LOG_DATA("{}: [{}] dx (lsq) {}, {}, {}", nameId(), o, lsq.solution(0), lsq.solution(1), lsq.solution(2));
970 LOG_DATA("{}: [{}] stdev_dx (lsq)\n{}", nameId(), o, lsq.variance.cwiseSqrt());
971 }
972
973 // Update the estimated position
974 _state.e_position += lsq.solution.block<3, 1>(0, 0);
975
976 // Update the estimated bias
977 if (_estimateBias)
978 {
979 _state.bias += lsq.solution(3);
980 }
981
982 bool solInaccurate = pow(lsq.solution.norm(), 2) > 1e-3;
983 if (!solInaccurate) // Solution is accurate enough
984 {
985 lsq.solution.block<3, 1>(0, 0) = _state.e_position;
986 break;
987 }
988 if (o == 14)
989 {
990 LOG_DATA("{}: Solution did not converge", nameId());
991 lsq.solution.setConstant(std::numeric_limits<double>::quiet_NaN());
992 lsq.variance.setConstant(std::numeric_limits<double>::quiet_NaN());
993 if (_estimateBias)
994 {
995 _state.bias = std::numeric_limits<double>::quiet_NaN();
996 }
997 _state.e_position = _initialState.e_position;
998 if (_estimateBias)
999 {
1000 _state.bias = _initialState.bias;
1001 }
1002 }
1003 }
1004
1005 _devices.clear();
1006 LOG_DATA("{}: Position: {}", nameId(), _state.e_position.transpose());
1007
1008 return lsq;
1009}
1010
1011void NAV::WiFiPositioning::kfSolution()
1012{
1013 double tau_i = !_lastPredictTime.empty()
1014 ? static_cast<double>((_devices.at(0).time - _lastPredictTime).count())
1015 : 0.0;
1016
1017 // ###########################################################################################################
1018 // Prediction
1019 // ###########################################################################################################
1020
1021 _lastPredictTime = _devices.at(0).time;
1022 if (tau_i > 0)
1023 {
1024 // Transition matrix
1025 Eigen::MatrixXd F = Eigen::MatrixXd::Zero(_numStates, _numStates);
1026 F(0, 3) = 1;
1027 F(1, 4) = 1;
1028 F(2, 5) = 1;
1029 _kalmanFilter.Phi = transitionMatrix_Phi_Taylor(F, tau_i, 1);
1030
1031 // Process noise covariance matrix
1032 _kalmanFilter.Q.block(0, 0, 3, 3) = std::pow(tau_i, 3) / 3.0 * Eigen::Matrix3d::Identity();
1033 _kalmanFilter.Q.block(3, 0, 3, 3) = std::pow(tau_i, 2) / 2.0 * Eigen::Matrix3d::Identity();
1034 _kalmanFilter.Q.block(0, 3, 3, 3) = std::pow(tau_i, 2) / 2.0 * Eigen::Matrix3d::Identity();
1035 _kalmanFilter.Q.block(3, 3, 3, 3) = tau_i * Eigen::Matrix3d::Identity();
1036 if (_estimateBias)
1037 {
1038 _kalmanFilter.Q(6, 6) = tau_i;
1039 }
1040 if (_processNoiseUnit == ProcessNoiseUnit::meter2)
1041 {
1042 _kalmanFilter.Q *= _processNoise;
1043 }
1044 else if (_processNoiseUnit == ProcessNoiseUnit::meter)
1045 {
1046 _kalmanFilter.Q *= std::pow(_processNoise, 2);
1047 }
1048 // Predict
1049 _kalmanFilter.predict();
1050 }
1051
1052 // ###########################################################################################################
1053 // Update
1054 // ###########################################################################################################
1055
1056 // Measurement
1057 double estimatedDistance = (_devices.at(0).position - _kalmanFilter.x.block<3, 1>(0, 0)).norm();
1058 if (_estimateBias)
1059 {
1060 estimatedDistance += _kalmanFilter.x(6);
1061 }
1062 _kalmanFilter.z << _devices.at(0).distance - estimatedDistance;
1063 if (_weightedSolution)
1064 {
1065 _kalmanFilter.R << std::pow(_devices.at(0).distanceStd, 2);
1066 }
1067
1068 // Design matrix
1069 Eigen::MatrixXd H = Eigen::MatrixXd::Zero(1, _numStates);
1070 H.block<1, 3>(0, 0) = -e_calcLineOfSightUnitVector(_kalmanFilter.x.block<3, 1>(0, 0), _devices.at(0).position);
1071 if (_estimateBias)
1072 {
1073 H(0, 6) = 1;
1074 }
1075 _kalmanFilter.H << H;
1076
1077 // Update
1078 _kalmanFilter.correctWithMeasurementInnovation();
1079
1080 _devices.clear();
1081 LOG_DATA("{}: Position: {}", nameId(), _kalmanFilter.x.block<3, 1>(0, 0).transpose());
1082 LOG_DATA("{}: Velocity: {}", nameId(), _kalmanFilter.x.block<3, 1>(3, 0).transpose());
1083}
Constants.hpp
Holds all Constants.
CoordinateFrames.hpp
Transformation collection.
FlowManager.hpp
Save/Load the Nodes.
json
nlohmann::json json
json namespace
Definition FlowManager.hpp:21
Functions.hpp
GNSS helper functions.
HelpMarker.hpp
Text Help Marker (?) with Tooltip.
InputWithUnit.hpp
Defines Widgets which allow the input of values and the selection of the unit.
LeastSquares.hpp
Least Squares Algorithm.
Logger.hpp
Utility class for logging to console and file.
LOG_DEBUG
#define LOG_DEBUG
Debug information. Should not be called on functions which receive observations (spamming)
Definition Logger.hpp:67
LOG_DATA
#define LOG_DATA
All output which occurs repeatedly every time observations are received.
Definition Logger.hpp:29
LOG_WARN
#define LOG_WARN
Error occurred, but a fallback option exists and program continues to work normally.
Definition Logger.hpp:71
LOG_TRACE
#define LOG_TRACE
Detailled info to trace the execution of the program. Should not be called on functions which receive...
Definition Logger.hpp:65
NodeManager.hpp
Manages all Nodes.
Vector.hpp
Vector Utility functions.
WiFiObs.hpp
Espressif Observation Class.
WiFiPositioningSolution.hpp
WiFi Positioning Algorithm output.
WiFiPositioning.hpp
WiFi Positioning.
NAV::InputPin::NodeDataQueue
TsDeque< std::shared_ptr< const NAV::NodeData > > NodeDataQueue
Node data queue type.
Definition Pin.hpp:707
NAV::KalmanFilter
Generalized Kalman Filter class.
Definition KalmanFilter.hpp:25
NAV::Node::_guiConfigDefaultWindowSize
ImVec2 _guiConfigDefaultWindowSize
Definition Node.hpp:410
NAV::Node::Node
Node(std::string name)
Constructor.
Definition Node.cpp:30
NAV::Node::inputPins
std::vector< InputPin > inputPins
List of input pins.
Definition Node.hpp:397
NAV::Node::nameId
std::string nameId() const
Node name and id.
Definition Node.cpp:253
NAV::Node::name
std::string name
Name of the Node.
Definition Node.hpp:395
NAV::Node::invokeCallbacks
void invokeCallbacks(size_t portIndex, const std::shared_ptr< const NodeData > &data)
Calls all registered callbacks on the specified output port.
Definition Node.cpp:180
NAV::Node::_hasConfig
bool _hasConfig
Flag if the config window should be shown.
Definition Node.hpp:413
NAV::TsDeque::extract_front
auto extract_front()
Returns a copy of the first element in the container and removes it from the container.
Definition TsDeque.hpp:494
NAV::WiFiObs::type
static std::string type()
Returns the type of the data class.
Definition WiFiObs.hpp:27
NAV::WiFiPositioningSolution::type
static std::string type()
Returns the type of the data class.
Definition WiFiPositioningSolution.hpp:29
NAV::WiFiPositioning::_deviceBias
std::vector< double > _deviceBias
Input of biases.
Definition WiFiPositioning.hpp:148
NAV::WiFiPositioning::_numStates
uint8_t _numStates
Number of states.
Definition WiFiPositioning.hpp:73
NAV::WiFiPositioning::_measurementNoise
double _measurementNoise
GUI selection of the process noise (standard deviation σ or Variance σ²)
Definition WiFiPositioning.hpp:194
NAV::WiFiPositioning::_kalmanFilter
KalmanFilter _kalmanFilter
Kalman Filter representation - States: 3xVel, 3xPos, (1xBias) - Measurements: 1xDist.
Definition WiFiPositioning.hpp:79
NAV::WiFiPositioning::_devices
std::vector< Device > _devices
Devices which are used for the positioning.
Definition WiFiPositioning.hpp:165
NAV::WiFiPositioning::_frame
Frame _frame
Frame to calculate the position in.
Definition WiFiPositioning.hpp:104
NAV::WiFiPositioning::_nWifiInputPins
size_t _nWifiInputPins
Amount of wifi input pins.
Definition WiFiPositioning.hpp:90
NAV::WiFiPositioning::_initCovarianceVelocity
Eigen::Vector3d _initCovarianceVelocity
GUI selection of the initial covariance diagonal values for velocity (standard deviation σ or Varianc...
Definition WiFiPositioning.hpp:236
NAV::WiFiPositioning::type
std::string type() const override
String representation of the Class Type.
Definition WiFiPositioning.cpp:61
NAV::WiFiPositioning::restore
void restore(const json &j) override
Restores the node from a json object.
Definition WiFiPositioning.cpp:575
NAV::WiFiPositioning::_numMeasurements
uint8_t _numMeasurements
Number of measurements.
Definition WiFiPositioning.hpp:76
NAV::WiFiPositioning::save
json save() const override
Saves the node into a json object.
Definition WiFiPositioning.cpp:536
NAV::WiFiPositioning::Frame::LLA
@ LLA
Latitude-Longitude-Altitude frame.
Definition WiFiPositioning.hpp:101
NAV::WiFiPositioning::Frame::ECEF
@ ECEF
Earth-Centered Earth-Fixed frame.
Definition WiFiPositioning.hpp:100
NAV::WiFiPositioning::_initCovariancePosition
Eigen::Vector3d _initCovariancePosition
GUI selection of the initial covariance diagonal values for position (standard deviation σ or Varianc...
Definition WiFiPositioning.hpp:222
NAV::WiFiPositioning::WiFiPositioning
WiFiPositioning()
Default constructor.
Definition WiFiPositioning.cpp:38
NAV::WiFiPositioning::guiConfig
void guiConfig() override
ImGui config window which is shown on double click.
Definition WiFiPositioning.cpp:71
NAV::WiFiPositioning::MeasurementNoiseUnit::meter
@ meter
Standard deviation NED [m, m, m].
Definition WiFiPositioning.hpp:188
NAV::WiFiPositioning::_deviceScale
std::vector< double > _deviceScale
Input of scales.
Definition WiFiPositioning.hpp:151
NAV::WiFiPositioning::lsqSolution
LeastSquaresResult< Eigen::VectorXd, Eigen::MatrixXd > lsqSolution()
Calculate the position using the least squares method.
Definition WiFiPositioning.cpp:895
NAV::WiFiPositioning::_deviceMacAddresses
std::vector< std::string > _deviceMacAddresses
Input of mac addresses.
Definition WiFiPositioning.hpp:142
NAV::WiFiPositioning::_initCovarianceVelocityUnit
InitCovarianceVelocityUnit _initCovarianceVelocityUnit
Gui selection for the Unit of the initial covariance for the velocity.
Definition WiFiPositioning.hpp:233
NAV::WiFiPositioning::_estimateBias
bool _estimateBias
Selection of whether the bias will be additionally estimated.
Definition WiFiPositioning.hpp:116
NAV::WiFiPositioning::deinitialize
void deinitialize() override
Deinitialize the node.
Definition WiFiPositioning.cpp:774
NAV::WiFiPositioning::_processNoise
double _processNoise
GUI selection of the process noise (standard deviation σ or Variance σ²)
Definition WiFiPositioning.hpp:208
NAV::WiFiPositioning::updateNumberOfInputPins
void updateNumberOfInputPins()
Adds/Deletes Input Pins depending on the variable _nNavInfoPins.
Definition WiFiPositioning.cpp:779
NAV::WiFiPositioning::initialize
bool initialize() override
Initialize the node.
Definition WiFiPositioning.cpp:698
NAV::WiFiPositioning::_solutionMode
SolutionMode _solutionMode
Solution Mode.
Definition WiFiPositioning.hpp:113
NAV::WiFiPositioning::_lastPredictTime
InsTime _lastPredictTime
Time when the last prediction was triggered.
Definition WiFiPositioning.hpp:168
NAV::WiFiPositioning::~WiFiPositioning
~WiFiPositioning() override
Destructor.
Definition WiFiPositioning.cpp:51
NAV::WiFiPositioning::_useInitialValues
bool _useInitialValues
Selection of whether the initial values should always be used or those of the last position.
Definition WiFiPositioning.hpp:122
NAV::WiFiPositioning::_initCovarianceBias
double _initCovarianceBias
GUI selection of the initial covariance diagonal values for bias (standard deviation σ or Variance σ²...
Definition WiFiPositioning.hpp:250
NAV::WiFiPositioning::_initCovariancePositionUnit
InitCovariancePositionUnit _initCovariancePositionUnit
Gui selection for the Unit of the initial covariance for the position.
Definition WiFiPositioning.hpp:219
NAV::WiFiPositioning::category
static std::string category()
String representation of the Class Category.
Definition WiFiPositioning.cpp:66
NAV::WiFiPositioning::_initialState
State _initialState
Initial state.
Definition WiFiPositioning.hpp:139
NAV::WiFiPositioning::typeStatic
static std::string typeStatic()
String representation of the Class Type.
Definition WiFiPositioning.cpp:56
NAV::WiFiPositioning::_weightedSolution
bool _weightedSolution
Selection of whether the solution will be weighted.
Definition WiFiPositioning.hpp:119
NAV::WiFiPositioning::ProcessNoiseUnit::meter2
@ meter2
Variance NED [m^2, m^2, m^2].
Definition WiFiPositioning.hpp:201
NAV::WiFiPositioning::ProcessNoiseUnit::meter
@ meter
Standard deviation NED [m, m, m].
Definition WiFiPositioning.hpp:202
NAV::WiFiPositioning::kfSolution
void kfSolution()
Calculate the position.
Definition WiFiPositioning.cpp:1011
NAV::WiFiPositioning::_processNoiseUnit
ProcessNoiseUnit _processNoiseUnit
Gui selection for the Unit of the initial covariance for the position.
Definition WiFiPositioning.hpp:205
NAV::WiFiPositioning::_initCovarianceBiasUnit
InitCovarianceBiasUnit _initCovarianceBiasUnit
Gui selection for the Unit of the initial covariance for the bias.
Definition WiFiPositioning.hpp:247
NAV::WiFiPositioning::_devicePositions
std::vector< Eigen::Vector3d > _devicePositions
Input of positions.
Definition WiFiPositioning.hpp:145
NAV::WiFiPositioning::recvWiFiObs
void recvWiFiObs(InputPin::NodeDataQueue &queue, size_t pinIdx)
Receive Function for the WiFi Observations.
Definition WiFiPositioning.cpp:791
NAV::WiFiPositioning::_state
State _state
State estimated by the algorithm.
Definition WiFiPositioning.hpp:136
NAV::WiFiPositioning::OUTPUT_PORT_INDEX_WIFISOL
static constexpr size_t OUTPUT_PORT_INDEX_WIFISOL
WiFiPositioningSolution.
Definition WiFiPositioning.hpp:70
NAV::WiFiPositioning::_measurementNoiseUnit
MeasurementNoiseUnit _measurementNoiseUnit
Gui selection for the Unit of the initial covariance for the position.
Definition WiFiPositioning.hpp:191
NAV::WiFiPositioning::_numOfDevices
size_t _numOfDevices
Number of devices.
Definition WiFiPositioning.hpp:154
NAV::gui::NodeEditorApplication::windowFontRatio
static float windowFontRatio()
Ratio to multiply for GUI window elements.
Definition NodeEditorApplication.cpp:1916
imgui_ex.hpp
ImGui extensions.
ImGui::InputDouble3
bool InputDouble3(const char *label, double v[3], const char *format, ImGuiInputTextFlags flags)
Shows an InputText GUI element for an array of 'double[3]'.
Definition imgui_ex.cpp:178
ImGui::InputDoubleL
bool InputDoubleL(const char *label, double *v, double v_min, double v_max, double step, double step_fast, const char *format, ImGuiInputTextFlags flags)
Shows a value limited InputText GUI element for 'double'.
Definition imgui_ex.cpp:294
NAV::NodeManager::CreateOutputPin
OutputPin * CreateOutputPin(Node *node, const char *name, Pin::Type pinType, const std::vector< std::string > &dataIdentifier, OutputPin::PinData data=static_cast< void * >(nullptr), int idx=-1)
Create an Output Pin object.
Definition NodeManager.cpp:202
NAV::NodeManager::DeleteInputPin
bool DeleteInputPin(InputPin &pin)
Deletes the input pin. Invalidates the pin reference given.
Definition NodeManager.cpp:234
NAV::NodeManager::CreateInputPin
InputPin * CreateInputPin(Node *node, const char *name, Pin::Type pinType, const std::vector< std::string > &dataIdentifier={}, InputPin::Callback callback=static_cast< InputPin::FlowFirableCallbackFunc >(nullptr), InputPin::FlowFirableCheckFunc firable=nullptr, int priority=0, int idx=-1)
Create an Input Pin object.
Definition NodeManager.cpp:176
NAV::flow::ApplyChanges
void ApplyChanges()
Signals that there have been changes to the flow.
Definition FlowManager.cpp:442
NAV::gui::widgets::InputDoubleWithUnit
InputWithUnitChange InputDoubleWithUnit(const char *label, float itemWidth, float unitWidth, double *v, U &combo_current_item, const char *combo_items_separated_by_zeros, double step=0.0, double step_fast=0.0, const char *format="%.6f", ImGuiInputTextFlags flags=0, int combo_popup_max_height_in_items=-1)
Shows an InputText GUI element to modify the provided value and also set its unit.
Definition InputWithUnit.hpp:285
NAV::gui::widgets::InputDouble3WithUnit
InputWithUnitChange InputDouble3WithUnit(const char *label, float itemWidth, float unitWidth, double v[3], U &combo_current_item, const char *combo_items_separated_by_zeros, const char *format="%.6f", ImGuiInputTextFlags flags=0, int combo_popup_max_height_in_items=-1)
Shows an InputText GUI element to modify the provided value and also set its unit.
Definition InputWithUnit.hpp:325
NAV::trafo::ecef2lla_WGS84
Eigen::Vector3< typename Derived::Scalar > ecef2lla_WGS84(const Eigen::MatrixBase< Derived > &e_position)
Converts Earth-centered-Earth-fixed coordinates into latitude, longitude and altitude using WGS84.
Definition CoordinateFrames.hpp:420
NAV::trafo::lla2ecef_WGS84
Eigen::Vector3< typename Derived::Scalar > lla2ecef_WGS84(const Eigen::MatrixBase< Derived > &lla_position)
Converts latitude, longitude and altitude into Earth-centered-Earth-fixed coordinates using WGS84.
Definition CoordinateFrames.hpp:402
NAV::GPST
@ GPST
GPS Time.
Definition TimeSystem.hpp:30
NAV::transitionMatrix_Phi_Taylor
Derived::PlainObject transitionMatrix_Phi_Taylor(const Eigen::MatrixBase< Derived > &F, double tau_s, size_t order)
Calculates the state transition matrix 𝚽 limited to specified order in 𝐅𝜏ₛ
Definition KalmanFilter.hpp:195
NAV::solveWeightedLinearLeastSquaresUncertainties
KeyedLeastSquaresResult< Scalar, StateKeyType > solveWeightedLinearLeastSquaresUncertainties(const KeyedMatrixX< Scalar, MeasKeyType, StateKeyType > &H, const KeyedMatrixX< Scalar, MeasKeyType, MeasKeyType > &W, const KeyedVectorX< Scalar, MeasKeyType > &dz)
Finds the "weighted least squares" solution.
Definition KeyedLeastSquares.hpp:104
NAV::deg2rad
constexpr auto deg2rad(const T &deg)
Convert Degree to Radians.
Definition Units.hpp:21
NAV::rad2deg
constexpr auto rad2deg(const T &rad)
Convert Radians to Degree.
Definition Units.hpp:39
NAV::e_calcLineOfSightUnitVector
Eigen::Vector3< typename DerivedA::Scalar > e_calcLineOfSightUnitVector(const Eigen::MatrixBase< DerivedA > &e_posAnt, const Eigen::MatrixBase< DerivedB > &e_posSat)
Calculates the line-of-sight unit vector from the user antenna to the satellite.
Definition Functions.hpp:31
NAV::LeastSquaresResult
Least Squares Uncertainties return value.
Definition LeastSquares.hpp:28
NAV::LeastSquaresResult::variance
VarianceMatrix variance
Least squares variance.
Definition LeastSquares.hpp:30
NAV::LeastSquaresResult::solution
SolutionVector solution
Least squares solution.
Definition LeastSquares.hpp:29
NAV::Pin::Type::Flow
@ Flow
NodeData Trigger.
Definition Pin.hpp:52