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