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