| 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/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 | |||
| 36 | 114 | NAV::WiFiPositioning::WiFiPositioning() | |
| 37 |
12/24✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 114 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 114 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 114 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 114 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 114 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 114 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 114 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 114 times.
✗ Branch 30 not taken.
✓ Branch 33 taken 114 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 114 times.
✗ Branch 37 not taken.
|
1254 | : Node(typeStatic()) |
| 38 | { | ||
| 39 | LOG_TRACE("{}: called", name); | ||
| 40 | |||
| 41 | 114 | _hasConfig = true; | |
| 42 | 114 | _guiConfigDefaultWindowSize = { 600, 500 }; | |
| 43 | |||
| 44 |
1/2✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
|
114 | updateNumberOfInputPins(); |
| 45 | |||
| 46 |
5/10✓ Branch 2 taken 114 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 114 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 114 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 114 times.
✓ Branch 15 taken 114 times.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
456 | CreateOutputPin(NAV::WiFiPositioningSolution::type().c_str(), Pin::Type::Flow, { NAV::WiFiPositioningSolution::type() }); |
| 47 | 228 | } | |
| 48 | |||
| 49 | 228 | NAV::WiFiPositioning::~WiFiPositioning() | |
| 50 | { | ||
| 51 | LOG_TRACE("{}: called", nameId()); | ||
| 52 | 228 | } | |
| 53 | |||
| 54 | 228 | std::string NAV::WiFiPositioning::typeStatic() | |
| 55 | { | ||
| 56 |
1/2✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
|
456 | return "WiFiPositioning"; |
| 57 | } | ||
| 58 | |||
| 59 | ✗ | std::string NAV::WiFiPositioning::type() const | |
| 60 | { | ||
| 61 | ✗ | return typeStatic(); | |
| 62 | } | ||
| 63 | |||
| 64 | 114 | std::string NAV::WiFiPositioning::category() | |
| 65 | { | ||
| 66 |
1/2✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
|
228 | return "Data Processor"; |
| 67 | } | ||
| 68 | |||
| 69 | ✗ | void 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 | |||
| 573 | ✗ | void 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 | |||
| 696 | ✗ | bool 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 | |||
| 772 | ✗ | void NAV::WiFiPositioning::deinitialize() | |
| 773 | { | ||
| 774 | LOG_TRACE("{}: called", nameId()); | ||
| 775 | ✗ | } | |
| 776 | |||
| 777 | 114 | void NAV::WiFiPositioning::updateNumberOfInputPins() | |
| 778 | { | ||
| 779 |
2/2✓ Branch 1 taken 114 times.
✓ Branch 2 taken 114 times.
|
228 | while (inputPins.size() < _nWifiInputPins) |
| 780 | { | ||
| 781 |
5/10✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 114 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 114 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 114 times.
✓ Branch 14 taken 114 times.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
|
342 | CreateInputPin(NAV::WiFiObs::type().c_str(), Pin::Type::Flow, { NAV::WiFiObs::type() }, &WiFiPositioning::recvWiFiObs); |
| 782 | } | ||
| 783 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 114 times.
|
114 | while (inputPins.size() > _nWifiInputPins) |
| 784 | { | ||
| 785 | ✗ | DeleteInputPin(inputPins.size() - 1); | |
| 786 | } | ||
| 787 | 228 | } | |
| 788 | |||
| 789 | ✗ | void 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 | |||
| 893 | ✗ | NAV::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 | |||
| 1009 | ✗ | void 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 | ✗ | } | |
| 1082 |