| 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 "AllanDeviation.hpp" | ||
| 10 | #include <algorithm> | ||
| 11 | #include <cstddef> | ||
| 12 | #include <cstdint> | ||
| 13 | #include <imgui.h> | ||
| 14 | #include <mutex> | ||
| 15 | |||
| 16 | #include "util/Logger.hpp" | ||
| 17 | |||
| 18 | #include "internal/NodeManager.hpp" | ||
| 19 | #include <Eigen/src/Core/util/Meta.h> | ||
| 20 | #include <fmt/core.h> | ||
| 21 | namespace nm = NAV::NodeManager; | ||
| 22 | #include "internal/FlowManager.hpp" | ||
| 23 | |||
| 24 | #include "internal/gui/widgets/HelpMarker.hpp" | ||
| 25 | #include "internal/gui/widgets/imgui_ex.hpp" | ||
| 26 | #include "implot.h" | ||
| 27 | |||
| 28 | #include "NodeData/IMU/ImuObs.hpp" | ||
| 29 | |||
| 30 | #include <chrono> | ||
| 31 | |||
| 32 | #include <unsupported/Eigen/MatrixFunctions> | ||
| 33 | |||
| 34 | 112 | NAV::AllanDeviation::AllanDeviation() | |
| 35 |
3/6✓ 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.
|
112 | : Node(typeStatic()) |
| 36 | { | ||
| 37 | LOG_TRACE("{}: called", name); | ||
| 38 | |||
| 39 | 112 | _hasConfig = true; | |
| 40 | 112 | _lockConfigDuringRun = false; | |
| 41 | 112 | _guiConfigDefaultWindowSize = { 630, 530 }; | |
| 42 | |||
| 43 |
4/8✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 112 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 112 times.
✓ Branch 9 taken 112 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
|
336 | nm::CreateInputPin(this, "ImuObs", Pin::Type::Flow, { NAV::ImuObs::type() }, &AllanDeviation::receiveImuObs); |
| 44 | 224 | } | |
| 45 | |||
| 46 | 224 | NAV::AllanDeviation::~AllanDeviation() | |
| 47 | { | ||
| 48 | LOG_TRACE("{}: called", nameId()); | ||
| 49 | 224 | } | |
| 50 | |||
| 51 | 224 | std::string NAV::AllanDeviation::typeStatic() | |
| 52 | { | ||
| 53 |
1/2✓ Branch 1 taken 224 times.
✗ Branch 2 not taken.
|
448 | return "AllanDeviation"; |
| 54 | } | ||
| 55 | |||
| 56 | ✗ | std::string NAV::AllanDeviation::type() const | |
| 57 | { | ||
| 58 | ✗ | return typeStatic(); | |
| 59 | } | ||
| 60 | |||
| 61 | 112 | std::string NAV::AllanDeviation::category() | |
| 62 | { | ||
| 63 |
1/2✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
|
224 | return "Data Processor"; |
| 64 | } | ||
| 65 | |||
| 66 | ✗ | void NAV::AllanDeviation::guiConfig() | |
| 67 | { | ||
| 68 | ✗ | if (ImGui::Checkbox("Compute Allan Deviation last", &_updateLast)) { flow::ApplyChanges(); } | |
| 69 | ✗ | if (ImGui::Checkbox("Display Confidence Intervals", &_displayConfidence)) { flow::ApplyChanges(); } | |
| 70 | ✗ | if (_displayConfidence) | |
| 71 | { | ||
| 72 | ✗ | ImGui::SameLine(); | |
| 73 | ✗ | ImGui::SetNextItemWidth(200.0F); | |
| 74 | ✗ | if (ImGui::SliderFloat("Confidence Alpha Channel", &_confidenceFillAlpha, 0.0F, 1.0F, "%.2f")) | |
| 75 | { | ||
| 76 | ✗ | flow::ApplyChanges(); | |
| 77 | } | ||
| 78 | } | ||
| 79 | |||
| 80 | ✗ | const std::array<std::string, 3> legendEntries{ "x", "y", "z" }; | |
| 81 | ✗ | constexpr std::array<double, 5> slopeTicks = { -2, -1, 0, 1, 2 }; | |
| 82 | |||
| 83 | ✗ | if (ImGui::BeginTabBar("AllanDeviationTabBar", ImGuiTabBarFlags_None)) | |
| 84 | { | ||
| 85 | ✗ | for (size_t sensorIter = 0; sensorIter < SensorType_COUNT; sensorIter++) | |
| 86 | { | ||
| 87 | ✗ | auto sensorType = static_cast<SensorType>(sensorIter); | |
| 88 | ✗ | const char* sensorName = to_string(sensorType); | |
| 89 | ✗ | auto& sensor = _sensors.at(sensorIter); | |
| 90 | ✗ | std::scoped_lock<std::mutex> lg(sensor.mutex); | |
| 91 | |||
| 92 | ✗ | if (ImGui::BeginTabItem(fmt::format("{}##TabItem {}", sensorName, size_t(id)).c_str())) | |
| 93 | { | ||
| 94 | ✗ | if (ImPlot::BeginPlot(fmt::format("Allan Deviation of {}##Plot {}", sensorName, size_t(id)).c_str())) | |
| 95 | { | ||
| 96 | ✗ | ImPlot::SetupLegend(ImPlotLocation_SouthWest, ImPlotLegendFlags_None); | |
| 97 | ✗ | ImPlot::SetupAxes("τ [s]", fmt::format("σ(τ) [{}]", unitString(sensorType)).c_str(), | |
| 98 | ImPlotAxisFlags_AutoFit, ImPlotAxisFlags_AutoFit); | ||
| 99 | ✗ | ImPlot::SetupAxisScale(ImAxis_X1, ImPlotScale_Log10); | |
| 100 | ✗ | ImPlot::SetupAxisScale(ImAxis_Y1, ImPlotScale_Log10); | |
| 101 | ✗ | ImPlot::PushStyleVar(ImPlotStyleVar_FillAlpha, _confidenceFillAlpha); | |
| 102 | ✗ | for (size_t d = 0; d < 3; d++) | |
| 103 | { | ||
| 104 | ✗ | if (_displayConfidence & !_averagingTimes.empty()) | |
| 105 | { | ||
| 106 | ✗ | ImPlot::PlotShaded(legendEntries.at(d).data(), | |
| 107 | ✗ | _averagingTimes.data(), | |
| 108 | ✗ | sensor.allanDeviationConfidenceIntervals.at(d).at(0).data(), | |
| 109 | ✗ | sensor.allanDeviationConfidenceIntervals.at(d).at(1).data(), | |
| 110 | ✗ | static_cast<int>(_averagingTimes.size())); | |
| 111 | } | ||
| 112 | ✗ | ImPlot::PlotLine(legendEntries.at(d).data(), | |
| 113 | ✗ | _averagingTimes.data(), | |
| 114 | ✗ | sensor.allanDeviation.at(d).data(), | |
| 115 | ✗ | static_cast<int>(_averagingTimes.size())); | |
| 116 | } | ||
| 117 | ✗ | ImPlot::EndPlot(); | |
| 118 | } | ||
| 119 | ✗ | if (ImGui::TreeNode("Slopes")) | |
| 120 | { | ||
| 121 | ✗ | if (ImPlot::BeginPlot("Slopes of Allan Variance")) | |
| 122 | { | ||
| 123 | ✗ | ImPlot::SetupLegend(ImPlotLocation_SouthWest, ImPlotLegendFlags_None); | |
| 124 | ✗ | ImPlot::SetupAxis(ImAxis_X1, "τ [s]", ImPlotAxisFlags_AutoFit); | |
| 125 | ✗ | ImPlot::SetupAxis(ImAxis_Y1, "µ [ ]", ImPlotAxisFlags_Lock); | |
| 126 | ✗ | ImPlot::SetupAxisScale(ImAxis_X1, ImPlotScale_Log10); | |
| 127 | ✗ | ImPlot::SetupAxisLimits(ImAxis_Y1, -2.5, 2.5); | |
| 128 | ✗ | ImPlot::SetupAxisTicks(ImAxis_Y1, slopeTicks.data(), 5, nullptr, false); | |
| 129 | ✗ | for (size_t d = 0; d < 3; d++) | |
| 130 | { | ||
| 131 | ✗ | ImPlot::PlotLine(legendEntries.at(d).data(), | |
| 132 | ✗ | _averagingTimes.data(), | |
| 133 | ✗ | sensor.slope.at(d).data(), | |
| 134 | ✗ | static_cast<int>(_averagingTimes.size())); | |
| 135 | } | ||
| 136 | ✗ | ImPlot::EndPlot(); | |
| 137 | } | ||
| 138 | |||
| 139 | ✗ | ImGui::TreePop(); | |
| 140 | } | ||
| 141 | ✗ | ImGui::EndTabItem(); | |
| 142 | } | ||
| 143 | ✗ | } | |
| 144 | ✗ | ImGui::EndTabBar(); | |
| 145 | } | ||
| 146 | ✗ | } | |
| 147 | |||
| 148 | ✗ | [[nodiscard]] json NAV::AllanDeviation::save() const | |
| 149 | { | ||
| 150 | LOG_TRACE("{}: called", nameId()); | ||
| 151 | |||
| 152 | return { | ||
| 153 | ✗ | { "displayConfidence", _displayConfidence }, | |
| 154 | ✗ | { "confidenceFillAlpha", _confidenceFillAlpha }, | |
| 155 | ✗ | { "updateLast", _updateLast }, | |
| 156 | ✗ | }; | |
| 157 | ✗ | } | |
| 158 | |||
| 159 | ✗ | void NAV::AllanDeviation::restore(json const& j) | |
| 160 | { | ||
| 161 | LOG_TRACE("{}: called", nameId()); | ||
| 162 | |||
| 163 | ✗ | if (j.contains("displayConfidence")) { j.at("displayConfidence").get_to(_displayConfidence); } | |
| 164 | ✗ | if (j.contains("confidenceFillAlpha")) { j.at("confidenceFillAlpha").get_to(_confidenceFillAlpha); } | |
| 165 | ✗ | if (j.contains("updateLast")) { j.at("updateLast").get_to(_updateLast); } | |
| 166 | ✗ | } | |
| 167 | |||
| 168 | ✗ | bool NAV::AllanDeviation::initialize() | |
| 169 | { | ||
| 170 | LOG_TRACE("{}: called", nameId()); | ||
| 171 | |||
| 172 | ✗ | _startingInsTime.reset(); | |
| 173 | |||
| 174 | ✗ | for (auto& sensor : _sensors) | |
| 175 | { | ||
| 176 | ✗ | std::scoped_lock<std::mutex> lg(sensor.mutex); | |
| 177 | ✗ | sensor.cumSum = { Eigen::Vector3d::Zero() }; | |
| 178 | ✗ | sensor.allanSum = {}; | |
| 179 | ✗ | sensor.allanVariance = {}; | |
| 180 | ✗ | sensor.allanDeviation = {}; | |
| 181 | ✗ | sensor.slope = {}; | |
| 182 | ✗ | sensor.allanDeviationConfidenceIntervals = {}; | |
| 183 | ✗ | } | |
| 184 | |||
| 185 | ✗ | _averagingFactors.clear(); | |
| 186 | ✗ | _averagingTimes.clear(); | |
| 187 | ✗ | _observationCount.clear(); | |
| 188 | |||
| 189 | ✗ | _imuObsCount = 0; | |
| 190 | |||
| 191 | ✗ | _nextAveragingFactorExponent = 1; | |
| 192 | ✗ | _nextAveragingFactor = 1; | |
| 193 | ✗ | _samplingInterval = 0.0; | |
| 194 | |||
| 195 | ✗ | return true; | |
| 196 | } | ||
| 197 | |||
| 198 | ✗ | void NAV::AllanDeviation::deinitialize() | |
| 199 | { | ||
| 200 | LOG_TRACE("{}: called", nameId()); | ||
| 201 | ✗ | } | |
| 202 | |||
| 203 | ✗ | void NAV::AllanDeviation::receiveImuObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */) | |
| 204 | { | ||
| 205 | ✗ | auto obs = std::static_pointer_cast<const ImuObs>(queue.extract_front()); | |
| 206 | |||
| 207 | ✗ | bool lastMessage = false; | |
| 208 | ✗ | if (queue.empty() | |
| 209 | ✗ | && inputPins[INPUT_PORT_INDEX_IMU_OBS].isPinLinked() | |
| 210 | ✗ | && inputPins[INPUT_PORT_INDEX_IMU_OBS].link.getConnectedPin()->noMoreDataAvailable) | |
| 211 | { | ||
| 212 | ✗ | lastMessage = true; | |
| 213 | } | ||
| 214 | |||
| 215 | // save InsTime of first imuObs for sampling interval computation | ||
| 216 | ✗ | if (_startingInsTime.empty()) { _startingInsTime = obs->insTime; } | |
| 217 | |||
| 218 | ✗ | _imuObsCount++; | |
| 219 | { | ||
| 220 | ✗ | std::scoped_lock<std::mutex> lg(_sensors.at(Accel).mutex); | |
| 221 | ✗ | _sensors.at(Accel).cumSum.emplace_back(_sensors.at(Accel).cumSum.back() + obs->p_acceleration); | |
| 222 | ✗ | } | |
| 223 | { | ||
| 224 | ✗ | std::scoped_lock<std::mutex> lg(_sensors.at(Gyro).mutex); | |
| 225 | ✗ | _sensors.at(Gyro).cumSum.emplace_back(_sensors.at(Gyro).cumSum.back() + obs->p_angularRate); | |
| 226 | ✗ | } | |
| 227 | |||
| 228 | // extending _averagingFactors if necessary | ||
| 229 | ✗ | if (_imuObsCount == _nextAveragingFactor * 2) | |
| 230 | { | ||
| 231 | ✗ | _averagingFactors.push_back(_nextAveragingFactor); | |
| 232 | ✗ | _observationCount.push_back(0); | |
| 233 | |||
| 234 | ✗ | for (auto& sensor : _sensors) | |
| 235 | { | ||
| 236 | ✗ | for (size_t d = 0; d < 3; d++) | |
| 237 | { | ||
| 238 | ✗ | sensor.allanSum.at(d).push_back(0); | |
| 239 | } | ||
| 240 | } | ||
| 241 | |||
| 242 | // computation of next averaging factor | ||
| 243 | ✗ | while (static_cast<unsigned int>(round(pow(10., static_cast<double>(_nextAveragingFactorExponent) / _averagingFactorsPerDecade))) == _nextAveragingFactor) | |
| 244 | { | ||
| 245 | ✗ | _nextAveragingFactorExponent++; | |
| 246 | } | ||
| 247 | |||
| 248 | ✗ | _nextAveragingFactor = static_cast<unsigned int>(round(pow(10., static_cast<double>(_nextAveragingFactorExponent) / _averagingFactorsPerDecade))); | |
| 249 | } | ||
| 250 | |||
| 251 | // computation Allan sum | ||
| 252 | ✗ | for (size_t k = 0; k < _averagingFactors.size(); k++) | |
| 253 | { | ||
| 254 | ✗ | for (auto& sensor : _sensors) | |
| 255 | { | ||
| 256 | ✗ | std::scoped_lock<std::mutex> lg(sensor.mutex); | |
| 257 | ✗ | Eigen::Vector3d tempSum = sensor.cumSum.at(_imuObsCount) | |
| 258 | ✗ | - 2 * sensor.cumSum.at(_imuObsCount - static_cast<unsigned int>(_averagingFactors.at(k))) | |
| 259 | ✗ | + sensor.cumSum.at(_imuObsCount - 2 * static_cast<unsigned int>(_averagingFactors.at(k))); | |
| 260 | |||
| 261 | ✗ | for (size_t d = 0; d < 3; d++) | |
| 262 | { | ||
| 263 | ✗ | sensor.allanSum.at(d).at(k) += pow(tempSum(static_cast<Eigen::Index>(d)), 2); | |
| 264 | } | ||
| 265 | ✗ | } | |
| 266 | ✗ | _observationCount.at(k)++; | |
| 267 | } | ||
| 268 | |||
| 269 | // computation of Allan Variance and Deviation | ||
| 270 | ✗ | if (!_updateLast || lastMessage) | |
| 271 | { | ||
| 272 | ✗ | _samplingInterval = static_cast<double>((obs->insTime - _startingInsTime).count()) / _imuObsCount; | |
| 273 | ✗ | _averagingTimes.resize(_averagingFactors.size(), 0.); | |
| 274 | ✗ | _confidenceMultiplicationFactor.resize(_averagingFactors.size(), 0.); | |
| 275 | ✗ | for (size_t k = 0; k < _averagingFactors.size(); k++) | |
| 276 | { | ||
| 277 | ✗ | _averagingTimes.at(k) = _averagingFactors.at(k) * _samplingInterval; | |
| 278 | ✗ | _confidenceMultiplicationFactor.at(k) = sqrt(0.5 / (_imuObsCount / _averagingFactors.at(k) - 1)); | |
| 279 | } | ||
| 280 | |||
| 281 | ✗ | for (auto& sensor : _sensors) | |
| 282 | { | ||
| 283 | ✗ | std::scoped_lock<std::mutex> lg(sensor.mutex); | |
| 284 | ✗ | for (size_t d = 0; d < 3; d++) | |
| 285 | { | ||
| 286 | ✗ | sensor.allanVariance.at(d).resize(_averagingFactors.size(), 0.); | |
| 287 | ✗ | sensor.allanDeviation.at(d).resize(_averagingFactors.size(), 0.); | |
| 288 | |||
| 289 | ✗ | for (size_t j = 0; j < 2; j++) | |
| 290 | { | ||
| 291 | ✗ | sensor.allanDeviationConfidenceIntervals.at(d).at(j).resize(_averagingFactors.size(), 0.); | |
| 292 | } | ||
| 293 | |||
| 294 | ✗ | for (size_t k = 0; k < _averagingFactors.size(); k++) | |
| 295 | { | ||
| 296 | ✗ | sensor.allanVariance.at(d).at(k) = sensor.allanSum.at(d).at(k) / (2 * pow(_averagingFactors.at(k), 2) * _observationCount.at(k)); | |
| 297 | |||
| 298 | ✗ | sensor.allanDeviation.at(d).at(k) = sqrt(sensor.allanVariance.at(d).at(k)); | |
| 299 | |||
| 300 | ✗ | sensor.allanDeviationConfidenceIntervals.at(d).at(0).at(k) = sensor.allanDeviation.at(d).at(k) * (1 - _confidenceMultiplicationFactor.at(k)); | |
| 301 | ✗ | sensor.allanDeviationConfidenceIntervals.at(d).at(1).at(k) = sensor.allanDeviation.at(d).at(k) * (1 + _confidenceMultiplicationFactor.at(k)); | |
| 302 | } | ||
| 303 | } | ||
| 304 | |||
| 305 | // Compute Slopes | ||
| 306 | ✗ | for (size_t d = 0; d < 3; d++) { sensor.slope.at(d).resize(_averagingFactors.size(), 0.); } | |
| 307 | |||
| 308 | ✗ | for (size_t k = 0; k < _averagingFactors.size(); k++) | |
| 309 | { | ||
| 310 | ✗ | size_t lo = (k == 0 ? k : k - 1); | |
| 311 | ✗ | size_t hi = (k == _averagingFactors.size() - 1 ? k : k + 1); | |
| 312 | |||
| 313 | ✗ | double divisor = log(_averagingFactors.at(hi) / _averagingFactors.at(lo)); | |
| 314 | |||
| 315 | ✗ | for (size_t d = 0; d < 3; d++) | |
| 316 | { | ||
| 317 | ✗ | sensor.slope.at(d).at(k) = log(sensor.allanVariance.at(d).at(hi) / sensor.allanVariance.at(d).at(lo)) / divisor; | |
| 318 | } | ||
| 319 | } | ||
| 320 | ✗ | } | |
| 321 | } | ||
| 322 | |||
| 323 | // empty _cumSum for performance's sake | ||
| 324 | ✗ | if (lastMessage) | |
| 325 | { | ||
| 326 | ✗ | for (auto& sensor : _sensors) { sensor.cumSum.clear(); } | |
| 327 | } | ||
| 328 | ✗ | } | |
| 329 | |||
| 330 | ✗ | const char* NAV::AllanDeviation::to_string(SensorType sensorType) | |
| 331 | { | ||
| 332 | ✗ | switch (sensorType) | |
| 333 | { | ||
| 334 | ✗ | case SensorType::Accel: | |
| 335 | ✗ | return "Accelerometer"; | |
| 336 | ✗ | case SensorType::Gyro: | |
| 337 | ✗ | return "Gyroscope"; | |
| 338 | ✗ | case SensorType::SensorType_COUNT: | |
| 339 | ✗ | return "Error: Count"; | |
| 340 | } | ||
| 341 | ✗ | return ""; | |
| 342 | } | ||
| 343 | |||
| 344 | ✗ | const char* NAV::AllanDeviation::unitString(SensorType sensorType) | |
| 345 | { | ||
| 346 | ✗ | switch (sensorType) | |
| 347 | { | ||
| 348 | ✗ | case SensorType::Accel: | |
| 349 | ✗ | return "m/s²"; | |
| 350 | ✗ | case SensorType::Gyro: | |
| 351 | ✗ | return "rad/s"; | |
| 352 | ✗ | case SensorType::SensorType_COUNT: | |
| 353 | ✗ | return "Error: Count"; | |
| 354 | } | ||
| 355 | ✗ | return ""; | |
| 356 | } | ||
| 357 |