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