| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // This file is part of INSTINCT, the INS Toolkit for Integrated | ||
| 2 | // Navigation Concepts and Training by the Institute of Navigation of | ||
| 3 | // the University of Stuttgart, Germany. | ||
| 4 | // | ||
| 5 | // This Source Code Form is subject to the terms of the Mozilla Public | ||
| 6 | // License, v. 2.0. If a copy of the MPL was not distributed with this | ||
| 7 | // file, You can obtain one at https://mozilla.org/MPL/2.0/. | ||
| 8 | |||
| 9 | #include "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" | ||
| 27 | namespace 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 | |||
| 38 | 112 | NAV::WiFiPositioning::WiFiPositioning() | |
| 39 |
12/24✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 112 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 112 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 112 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 112 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 112 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 112 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 112 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 112 times.
✗ Branch 30 not taken.
✓ Branch 33 taken 112 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 112 times.
✗ Branch 37 not taken.
|
1232 | : Node(typeStatic()) |
| 40 | { | ||
| 41 | LOG_TRACE("{}: called", name); | ||
| 42 | |||
| 43 | 112 | _hasConfig = true; | |
| 44 | 112 | _guiConfigDefaultWindowSize = { 600, 500 }; | |
| 45 | |||
| 46 |
1/2✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
|
112 | updateNumberOfInputPins(); |
| 47 | |||
| 48 |
5/10✓ Branch 2 taken 112 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 112 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 112 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 112 times.
✓ Branch 15 taken 112 times.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
448 | nm::CreateOutputPin(this, NAV::WiFiPositioningSolution::type().c_str(), Pin::Type::Flow, { NAV::WiFiPositioningSolution::type() }); |
| 49 | 224 | } | |
| 50 | |||
| 51 | 224 | NAV::WiFiPositioning::~WiFiPositioning() | |
| 52 | { | ||
| 53 | LOG_TRACE("{}: called", nameId()); | ||
| 54 | 224 | } | |
| 55 | |||
| 56 | 224 | std::string NAV::WiFiPositioning::typeStatic() | |
| 57 | { | ||
| 58 |
1/2✓ Branch 1 taken 224 times.
✗ Branch 2 not taken.
|
448 | return "WiFiPositioning"; |
| 59 | } | ||
| 60 | |||
| 61 | ✗ | std::string NAV::WiFiPositioning::type() const | |
| 62 | { | ||
| 63 | ✗ | return typeStatic(); | |
| 64 | } | ||
| 65 | |||
| 66 | 112 | std::string NAV::WiFiPositioning::category() | |
| 67 | { | ||
| 68 |
1/2✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
|
224 | return "Data Processor"; |
| 69 | } | ||
| 70 | |||
| 71 | ✗ | void 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 | |||
| 575 | ✗ | void 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 | |||
| 698 | ✗ | bool 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 | |||
| 774 | ✗ | void NAV::WiFiPositioning::deinitialize() | |
| 775 | { | ||
| 776 | LOG_TRACE("{}: called", nameId()); | ||
| 777 | ✗ | } | |
| 778 | |||
| 779 | 112 | void NAV::WiFiPositioning::updateNumberOfInputPins() | |
| 780 | { | ||
| 781 |
2/2✓ Branch 1 taken 112 times.
✓ Branch 2 taken 112 times.
|
224 | while (inputPins.size() < _nWifiInputPins) |
| 782 | { | ||
| 783 |
5/10✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 112 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 112 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 112 times.
✓ Branch 14 taken 112 times.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
|
336 | nm::CreateInputPin(this, NAV::WiFiObs::type().c_str(), Pin::Type::Flow, { NAV::WiFiObs::type() }, &WiFiPositioning::recvWiFiObs); |
| 784 | } | ||
| 785 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 112 times.
|
112 | while (inputPins.size() > _nWifiInputPins) |
| 786 | { | ||
| 787 | ✗ | nm::DeleteInputPin(inputPins.back()); | |
| 788 | } | ||
| 789 | 224 | } | |
| 790 | |||
| 791 | ✗ | void 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 | |||
| 895 | ✗ | NAV::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 | |||
| 1011 | ✗ | void 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 | ✗ | } | |
| 1084 |