| 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 "ImuFusion.hpp" | ||
| 10 | |||
| 11 | #include "util/Logger.hpp" | ||
| 12 | |||
| 13 | #include <numeric> | ||
| 14 | #include "Navigation/Math/Math.hpp" | ||
| 15 | #include "Navigation/Transformations/CoordinateFrames.hpp" | ||
| 16 | #include "Navigation/Transformations/Units.hpp" | ||
| 17 | |||
| 18 | #include "Navigation/INS/SensorCombiner/IRWKF/IRWKF.hpp" | ||
| 19 | #include "Navigation/INS/SensorCombiner/BsplineKF/BsplineKF.hpp" | ||
| 20 | #include "Navigation/INS/SensorCombiner/BsplineKF/QuadraticBsplines.hpp" | ||
| 21 | #include "NodeData/State/InsGnssLCKFSolution.hpp" | ||
| 22 | |||
| 23 | #include <imgui_internal.h> | ||
| 24 | #include "internal/gui/widgets/imgui_ex.hpp" | ||
| 25 | #include "internal/gui/widgets/InputWithUnit.hpp" | ||
| 26 | #include "internal/gui/widgets/EnumCombo.hpp" | ||
| 27 | #include "internal/gui/widgets/HelpMarker.hpp" | ||
| 28 | #include "internal/gui/NodeEditorApplication.hpp" | ||
| 29 | #include "util/Json.hpp" | ||
| 30 | |||
| 31 | #include "internal/NodeManager.hpp" | ||
| 32 | namespace nm = NAV::NodeManager; | ||
| 33 | #include "internal/FlowManager.hpp" | ||
| 34 | |||
| 35 | namespace NAV | ||
| 36 | { | ||
| 37 | /// @brief Write info to a json object | ||
| 38 | /// @param[out] j Json output | ||
| 39 | /// @param[in] data Object to read info from | ||
| 40 | ✗ | static void to_json(json& j, const PinData& data) // NOLINT(misc-use-anonymous-namespace) | |
| 41 | { | ||
| 42 | ✗ | j = json{ | |
| 43 | // ---------------------------------------- Initialization ------------------------------------------- | ||
| 44 | ✗ | { "initAngularRateBias", data.initAngularRateBias }, | |
| 45 | ✗ | { "initAngularRateBiasUnit", data.initAngularRateBiasUnit }, | |
| 46 | ✗ | { "initAccelerationBias", data.initAccelerationBias }, | |
| 47 | ✗ | { "initAccelerationBiasUnit", data.initAccelerationBiasUnit }, | |
| 48 | ✗ | { "initCovarianceAngularRate", data.initCovarianceAngularRate }, | |
| 49 | ✗ | { "initCovarianceAngularRateUnit", data.initCovarianceAngularRateUnit }, | |
| 50 | ✗ | { "initCovarianceAcceleration", data.initCovarianceAcceleration }, | |
| 51 | ✗ | { "initCovarianceAccelerationUnit", data.initCovarianceAccelerationUnit }, | |
| 52 | ✗ | { "initCovarianceBiasAngRate", data.initCovarianceBiasAngRate }, | |
| 53 | ✗ | { "initCovarianceBiasAngRateUnit", data.initCovarianceBiasAngRateUnit }, | |
| 54 | ✗ | { "initCovarianceBiasAcc", data.initCovarianceBiasAcc }, | |
| 55 | ✗ | { "initCovarianceBiasAccUnit", data.initCovarianceBiasAccUnit }, | |
| 56 | // ----------------------------------------- Process Noise ------------------------------------------- | ||
| 57 | ✗ | { "varBiasAccelerationNoise", data.varBiasAccelerationNoise }, | |
| 58 | ✗ | { "varBiasAccelerationNoiseUnit", data.varBiasAccelerationNoiseUnit }, | |
| 59 | ✗ | { "varBiasAngRateNoise", data.varBiasAngRateNoise }, | |
| 60 | ✗ | { "varBiasAngRateNoiseUnit", data.varBiasAngRateNoiseUnit }, | |
| 61 | // --------------------------------------- Measurement Noise ----------------------------------------- | ||
| 62 | ✗ | { "measurementUncertaintyAngularRateUnit", data.measurementUncertaintyAngularRateUnit }, | |
| 63 | ✗ | { "measurementUncertaintyAngularRate", data.measurementUncertaintyAngularRate }, | |
| 64 | ✗ | { "measurementUncertaintyAccelerationUnit", data.measurementUncertaintyAccelerationUnit }, | |
| 65 | ✗ | { "measurementUncertaintyAcceleration", data.measurementUncertaintyAcceleration }, | |
| 66 | ✗ | }; | |
| 67 | ✗ | } | |
| 68 | /// @brief Read info from a json object | ||
| 69 | /// @param[in] j Json variable to read info from | ||
| 70 | /// @param[out] data Output object | ||
| 71 | 9 | static void from_json(const json& j, PinData& data) // NOLINT(misc-use-anonymous-namespace) | |
| 72 | { | ||
| 73 | // ------------------------------------------ Initialization --------------------------------------------- | ||
| 74 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("initAngularRateBias")) |
| 75 | { | ||
| 76 | 9 | j.at("initAngularRateBias").get_to(data.initAngularRateBias); | |
| 77 | } | ||
| 78 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("initAngularRateBiasUnit")) |
| 79 | { | ||
| 80 | 9 | j.at("initAngularRateBiasUnit").get_to(data.initAngularRateBiasUnit); | |
| 81 | } | ||
| 82 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("initAccelerationBias")) |
| 83 | { | ||
| 84 | 9 | j.at("initAccelerationBias").get_to(data.initAccelerationBias); | |
| 85 | } | ||
| 86 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("initAccelerationBiasUnit")) |
| 87 | { | ||
| 88 | 9 | j.at("initAccelerationBiasUnit").get_to(data.initAccelerationBiasUnit); | |
| 89 | } | ||
| 90 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("initCovarianceAngularRate")) |
| 91 | { | ||
| 92 | 9 | j.at("initCovarianceAngularRate").get_to(data.initCovarianceAngularRate); | |
| 93 | } | ||
| 94 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("initCovarianceAngularRateUnit")) |
| 95 | { | ||
| 96 | 9 | j.at("initCovarianceAngularRateUnit").get_to(data.initCovarianceAngularRateUnit); | |
| 97 | } | ||
| 98 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("initCovarianceAcceleration")) |
| 99 | { | ||
| 100 | 9 | j.at("initCovarianceAcceleration").get_to(data.initCovarianceAcceleration); | |
| 101 | } | ||
| 102 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("initCovarianceAccelerationUnit")) |
| 103 | { | ||
| 104 | 9 | j.at("initCovarianceAccelerationUnit").get_to(data.initCovarianceAccelerationUnit); | |
| 105 | } | ||
| 106 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("initCovarianceBiasAngRate")) |
| 107 | { | ||
| 108 | 9 | j.at("initCovarianceBiasAngRate").get_to(data.initCovarianceBiasAngRate); | |
| 109 | } | ||
| 110 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("initCovarianceBiasAngRateUnit")) |
| 111 | { | ||
| 112 | 9 | j.at("initCovarianceBiasAngRateUnit").get_to(data.initCovarianceBiasAngRateUnit); | |
| 113 | } | ||
| 114 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("initCovarianceBiasAcc")) |
| 115 | { | ||
| 116 | 9 | j.at("initCovarianceBiasAcc").get_to(data.initCovarianceBiasAcc); | |
| 117 | } | ||
| 118 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("initCovarianceBiasAccUnit")) |
| 119 | { | ||
| 120 | 9 | j.at("initCovarianceBiasAccUnit").get_to(data.initCovarianceBiasAccUnit); | |
| 121 | } | ||
| 122 | // ------------------------------------------- Process Noise --------------------------------------------- | ||
| 123 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("varBiasAccelerationNoise")) |
| 124 | { | ||
| 125 | 9 | j.at("varBiasAccelerationNoise").get_to(data.varBiasAccelerationNoise); | |
| 126 | } | ||
| 127 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("varBiasAccelerationNoiseUnit")) |
| 128 | { | ||
| 129 | 9 | j.at("varBiasAccelerationNoiseUnit").get_to(data.varBiasAccelerationNoiseUnit); | |
| 130 | } | ||
| 131 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("varBiasAngRateNoise")) |
| 132 | { | ||
| 133 | 9 | j.at("varBiasAngRateNoise").get_to(data.varBiasAngRateNoise); | |
| 134 | } | ||
| 135 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("varBiasAngRateNoiseUnit")) |
| 136 | { | ||
| 137 | 9 | j.at("varBiasAngRateNoiseUnit").get_to(data.varBiasAngRateNoiseUnit); | |
| 138 | } | ||
| 139 | // ----------------------------------------- Measurement Noise ------------------------------------------- | ||
| 140 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("measurementUncertaintyAngularRate")) |
| 141 | { | ||
| 142 | 9 | j.at("measurementUncertaintyAngularRate").get_to(data.measurementUncertaintyAngularRate); | |
| 143 | } | ||
| 144 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("measurementUncertaintyAngularRateUnit")) |
| 145 | { | ||
| 146 | 9 | j.at("measurementUncertaintyAngularRateUnit").get_to(data.measurementUncertaintyAngularRateUnit); | |
| 147 | } | ||
| 148 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("measurementUncertaintyAcceleration")) |
| 149 | { | ||
| 150 | 9 | j.at("measurementUncertaintyAcceleration").get_to(data.measurementUncertaintyAcceleration); | |
| 151 | } | ||
| 152 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | if (j.contains("measurementUncertaintyAccelerationUnit")) |
| 153 | { | ||
| 154 | 9 | j.at("measurementUncertaintyAccelerationUnit").get_to(data.measurementUncertaintyAccelerationUnit); | |
| 155 | } | ||
| 156 | 9 | } | |
| 157 | |||
| 158 | /// @brief Write info to a json object | ||
| 159 | /// @param[out] j Json output | ||
| 160 | /// @param[in] data Object to read info from | ||
| 161 | ✗ | static void to_json(json& j, const PinDataIRWKF& data) // NOLINT(misc-use-anonymous-namespace) | |
| 162 | { | ||
| 163 | ✗ | j = json{ | |
| 164 | // ---------------------------------------- Initialization ------------------------------------------- | ||
| 165 | ✗ | { "initAngularRate", data.initAngularRate }, | |
| 166 | ✗ | { "initAngularRateUnit", data.initAngularRateUnit }, | |
| 167 | ✗ | { "initAcceleration", data.initAcceleration }, | |
| 168 | ✗ | { "initAccelerationUnit", data.initAccelerationUnit }, | |
| 169 | ✗ | { "initAngularAcc", data.initAngularAcc }, | |
| 170 | ✗ | { "initAngularAccUnit", data.initAngularAccUnit }, | |
| 171 | ✗ | { "initJerk", data.initJerk }, | |
| 172 | ✗ | { "initJerkUnit", data.initJerkUnit }, | |
| 173 | ✗ | { "initCovarianceAngularAcc", data.initCovarianceAngularAcc }, | |
| 174 | ✗ | { "initCovarianceAngularAccUnit", data.initCovarianceAngularAccUnit }, | |
| 175 | ✗ | { "initCovarianceJerk", data.initCovarianceJerk }, | |
| 176 | ✗ | { "initCovarianceJerkUnit", data.initCovarianceJerkUnit }, | |
| 177 | |||
| 178 | // ----------------------------------------- Process Noise ------------------------------------------- | ||
| 179 | ✗ | { "varAngularAccNoise", data.varAngularAccNoise }, | |
| 180 | ✗ | { "varAngularAccNoiseUnit", data.varAngularAccNoiseUnit }, | |
| 181 | ✗ | { "varJerkNoise", data.varJerkNoise }, | |
| 182 | ✗ | { "varJerkNoiseUnit", data.varJerkNoiseUnit }, | |
| 183 | ✗ | }; | |
| 184 | ✗ | } | |
| 185 | /// @brief Read info from a json object | ||
| 186 | /// @param[in] j Json variable to read info from | ||
| 187 | /// @param[out] data Output object | ||
| 188 | 2 | static void from_json(const json& j, PinDataIRWKF& data) // NOLINT(misc-use-anonymous-namespace) | |
| 189 | { | ||
| 190 | // ------------------------------------------ Initialization --------------------------------------------- | ||
| 191 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initAngularRate")) |
| 192 | { | ||
| 193 | 2 | j.at("initAngularRate").get_to(data.initAngularRate); | |
| 194 | } | ||
| 195 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initAngularRateUnit")) |
| 196 | { | ||
| 197 | 2 | j.at("initAngularRateUnit").get_to(data.initAngularRateUnit); | |
| 198 | } | ||
| 199 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initAcceleration")) |
| 200 | { | ||
| 201 | 2 | j.at("initAcceleration").get_to(data.initAcceleration); | |
| 202 | } | ||
| 203 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initAccelerationUnit")) |
| 204 | { | ||
| 205 | 2 | j.at("initAccelerationUnit").get_to(data.initAccelerationUnit); | |
| 206 | } | ||
| 207 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initAngularAcc")) |
| 208 | { | ||
| 209 | 2 | j.at("initAngularAcc").get_to(data.initAngularAcc); | |
| 210 | } | ||
| 211 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initAngularAccUnit")) |
| 212 | { | ||
| 213 | 2 | j.at("initAngularAccUnit").get_to(data.initAngularAccUnit); | |
| 214 | } | ||
| 215 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initJerk")) |
| 216 | { | ||
| 217 | 2 | j.at("initJerk").get_to(data.initJerk); | |
| 218 | } | ||
| 219 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initJerkUnit")) |
| 220 | { | ||
| 221 | 2 | j.at("initJerkUnit").get_to(data.initJerkUnit); | |
| 222 | } | ||
| 223 | |||
| 224 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initCovarianceAngularAcc")) |
| 225 | { | ||
| 226 | 2 | j.at("initCovarianceAngularAcc").get_to(data.initCovarianceAngularAcc); | |
| 227 | } | ||
| 228 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initCovarianceAngularAccUnit")) |
| 229 | { | ||
| 230 | 2 | j.at("initCovarianceAngularAccUnit").get_to(data.initCovarianceAngularAccUnit); | |
| 231 | } | ||
| 232 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initCovarianceJerk")) |
| 233 | { | ||
| 234 | 2 | j.at("initCovarianceJerk").get_to(data.initCovarianceJerk); | |
| 235 | } | ||
| 236 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initCovarianceJerkUnit")) |
| 237 | { | ||
| 238 | 2 | j.at("initCovarianceJerkUnit").get_to(data.initCovarianceJerkUnit); | |
| 239 | } | ||
| 240 | |||
| 241 | // ------------------------------------------- Process Noise --------------------------------------------- | ||
| 242 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("varAngularAccNoise")) |
| 243 | { | ||
| 244 | 2 | j.at("varAngularAccNoise").get_to(data.varAngularAccNoise); | |
| 245 | } | ||
| 246 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("varAngularAccNoiseUnit")) |
| 247 | { | ||
| 248 | 2 | j.at("varAngularAccNoiseUnit").get_to(data.varAngularAccNoiseUnit); | |
| 249 | } | ||
| 250 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("varJerkNoise")) |
| 251 | { | ||
| 252 | 2 | j.at("varJerkNoise").get_to(data.varJerkNoise); | |
| 253 | } | ||
| 254 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("varJerkNoiseUnit")) |
| 255 | { | ||
| 256 | 2 | j.at("varJerkNoiseUnit").get_to(data.varJerkNoiseUnit); | |
| 257 | } | ||
| 258 | 2 | } | |
| 259 | |||
| 260 | /// @brief Write info to a json object | ||
| 261 | /// @param[out] j Json output | ||
| 262 | /// @param[in] data Object to read info from | ||
| 263 | ✗ | static void to_json(json& j, const PinDataBsplineKF& data) // NOLINT(misc-use-anonymous-namespace) | |
| 264 | { | ||
| 265 | ✗ | j = json{ | |
| 266 | // ---------------------------------------- Initialization ------------------------------------------- | ||
| 267 | ✗ | { "initAngularRate", data.initCoeffsAngRate }, | |
| 268 | ✗ | { "initCoeffsAngularRateUnit", data.initCoeffsAngularRateUnit }, | |
| 269 | ✗ | { "initCoeffsAccel", data.initCoeffsAccel }, | |
| 270 | ✗ | { "initCoeffsAccelUnit", data.initCoeffsAccelUnit }, | |
| 271 | ✗ | { "initCovarianceCoeffsAngRate", data.initCovarianceCoeffsAngRate }, | |
| 272 | ✗ | { "initCovarianceCoeffsAngRateUnit", data.initCovarianceCoeffsAngRateUnit }, | |
| 273 | ✗ | { "initCovarianceCoeffsAccel", data.initCovarianceCoeffsAccel }, | |
| 274 | ✗ | { "initCovarianceCoeffsAccelUnit", data.initCovarianceCoeffsAccelUnit }, | |
| 275 | |||
| 276 | // ----------------------------------------- Process Noise ------------------------------------------- | ||
| 277 | ✗ | { "varCoeffsAngRateNoise", data.varCoeffsAngRateNoise }, | |
| 278 | ✗ | { "varCoeffsAngRateUnit", data.varCoeffsAngRateUnit }, | |
| 279 | ✗ | { "varCoeffsAccelNoise", data.varCoeffsAccelNoise }, | |
| 280 | ✗ | { "varCoeffsAccelUnit", data.varCoeffsAccelUnit }, | |
| 281 | ✗ | }; | |
| 282 | ✗ | } | |
| 283 | /// @brief Read info from a json object | ||
| 284 | /// @param[in] j Json variable to read info from | ||
| 285 | /// @param[out] data Output object | ||
| 286 | 2 | static void from_json(const json& j, PinDataBsplineKF& data) // NOLINT(misc-use-anonymous-namespace) | |
| 287 | { | ||
| 288 | // ------------------------------------------ Initialization --------------------------------------------- | ||
| 289 | |||
| 290 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
|
2 | if (j.contains("initCoeffsAngRate")) |
| 291 | { | ||
| 292 | ✗ | j.at("initCoeffsAngRate").get_to(data.initCoeffsAngRate); | |
| 293 | } | ||
| 294 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initCoeffsAngularRateUnit")) |
| 295 | { | ||
| 296 | 2 | j.at("initCoeffsAngularRateUnit").get_to(data.initCoeffsAngularRateUnit); | |
| 297 | } | ||
| 298 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initCoeffsAccel")) |
| 299 | { | ||
| 300 | 2 | j.at("initCoeffsAccel").get_to(data.initCoeffsAccel); | |
| 301 | } | ||
| 302 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initCoeffsAccelUnit")) |
| 303 | { | ||
| 304 | 2 | j.at("initCoeffsAccelUnit").get_to(data.initCoeffsAccelUnit); | |
| 305 | } | ||
| 306 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initCovarianceCoeffsAngRate")) |
| 307 | { | ||
| 308 | 2 | j.at("initCovarianceCoeffsAngRate").get_to(data.initCovarianceCoeffsAngRate); | |
| 309 | } | ||
| 310 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initCovarianceCoeffsAngRateUnit")) |
| 311 | { | ||
| 312 | 2 | j.at("initCovarianceCoeffsAngRateUnit").get_to(data.initCovarianceCoeffsAngRateUnit); | |
| 313 | } | ||
| 314 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initCovarianceCoeffsAccel")) |
| 315 | { | ||
| 316 | 2 | j.at("initCovarianceCoeffsAccel").get_to(data.initCovarianceCoeffsAccel); | |
| 317 | } | ||
| 318 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initCovarianceCoeffsAccelUnit")) |
| 319 | { | ||
| 320 | 2 | j.at("initCovarianceCoeffsAccelUnit").get_to(data.initCovarianceCoeffsAccelUnit); | |
| 321 | } | ||
| 322 | |||
| 323 | // ------------------------------------------- Process Noise --------------------------------------------- | ||
| 324 | |||
| 325 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("varCoeffsAngRateNoise")) |
| 326 | { | ||
| 327 | 2 | j.at("varCoeffsAngRateNoise").get_to(data.varCoeffsAngRateNoise); | |
| 328 | } | ||
| 329 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("varCoeffsAngRateUnit")) |
| 330 | { | ||
| 331 | 2 | j.at("varCoeffsAngRateUnit").get_to(data.varCoeffsAngRateUnit); | |
| 332 | } | ||
| 333 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("varCoeffsAccelNoise")) |
| 334 | { | ||
| 335 | 2 | j.at("varCoeffsAccelNoise").get_to(data.varCoeffsAccelNoise); | |
| 336 | } | ||
| 337 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("varCoeffsAccelUnit")) |
| 338 | { | ||
| 339 | 2 | j.at("varCoeffsAccelUnit").get_to(data.varCoeffsAccelUnit); | |
| 340 | } | ||
| 341 | 2 | } | |
| 342 | |||
| 343 | } // namespace NAV | ||
| 344 | |||
| 345 | 114 | NAV::ImuFusion::ImuFusion() | |
| 346 |
12/24✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 114 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 114 times.
✗ Branch 13 not taken.
✓ Branch 19 taken 114 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 114 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 114 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 114 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 114 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 114 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 114 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 114 times.
✗ Branch 41 not taken.
|
114 | : Imu(typeStatic()) |
| 347 | { | ||
| 348 | LOG_TRACE("{}: called", name); | ||
| 349 | |||
| 350 | 114 | _hasConfig = true; | |
| 351 | 114 | _guiConfigDefaultWindowSize = { 991, 1059 }; | |
| 352 | |||
| 353 |
4/8✓ Branch 2 taken 114 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 114 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 114 times.
✓ Branch 10 taken 114 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
456 | nm::CreateOutputPin(this, "Combined ImuObs", Pin::Type::Flow, { NAV::ImuObs::type() }); |
| 354 |
1/2✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
|
114 | updateNumberOfInputPins(); |
| 355 | 228 | } | |
| 356 | |||
| 357 | 232 | NAV::ImuFusion::~ImuFusion() | |
| 358 | { | ||
| 359 | LOG_TRACE("{}: called", nameId()); | ||
| 360 | 232 | } | |
| 361 | |||
| 362 | 226 | std::string NAV::ImuFusion::typeStatic() | |
| 363 | { | ||
| 364 |
1/2✓ Branch 1 taken 226 times.
✗ Branch 2 not taken.
|
452 | return "ImuFusion"; |
| 365 | } | ||
| 366 | |||
| 367 | ✗ | std::string NAV::ImuFusion::type() const | |
| 368 | { | ||
| 369 | ✗ | return typeStatic(); | |
| 370 | } | ||
| 371 | |||
| 372 | 112 | std::string NAV::ImuFusion::category() | |
| 373 | { | ||
| 374 |
1/2✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
|
224 | return "Data Processor"; |
| 375 | } | ||
| 376 | |||
| 377 | ✗ | void NAV::ImuFusion::guiConfig() | |
| 378 | { | ||
| 379 | ✗ | constexpr float configWidth = 380.0F; | |
| 380 | ✗ | constexpr float unitWidth = 150.0F; | |
| 381 | |||
| 382 | ✗ | if (ImGui::BeginTable(fmt::format("Pin Settings##{}", size_t(id)).c_str(), inputPins.size() > 1 ? 2 : 1, | |
| 383 | ✗ | ImGuiTableFlags_Borders | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX, ImVec2(0.0F, 0.0F))) | |
| 384 | { | ||
| 385 | ✗ | ImGui::TableSetupColumn("Pin"); | |
| 386 | ✗ | if (inputPins.size() > 2) | |
| 387 | { | ||
| 388 | ✗ | ImGui::TableSetupColumn(""); | |
| 389 | } | ||
| 390 | ✗ | ImGui::TableHeadersRow(); | |
| 391 | |||
| 392 | ✗ | for (size_t pinIndex = 0; pinIndex < _pinData.size(); ++pinIndex) | |
| 393 | { | ||
| 394 | ✗ | ImGui::TableNextRow(); | |
| 395 | ✗ | ImGui::TableNextColumn(); // Pin | |
| 396 | |||
| 397 | ✗ | ImGui::TextUnformatted(fmt::format("{}", inputPins.at(pinIndex).name).c_str()); | |
| 398 | |||
| 399 | ✗ | if (inputPins.size() > 2) // Minimum # of pins for the fusion to make sense is two | |
| 400 | { | ||
| 401 | ✗ | ImGui::TableNextColumn(); // Delete | |
| 402 | ✗ | if (!(pinIndex == 0)) // Don't delete Pin 1, it's the reference for all other sensor (biases) that follow | |
| 403 | { | ||
| 404 | ✗ | if (ImGui::Button(fmt::format("x##{} - {}", size_t(id), pinIndex).c_str())) | |
| 405 | { | ||
| 406 | ✗ | nm::DeleteInputPin(inputPins.at(pinIndex)); | |
| 407 | ✗ | nm::DeleteOutputPin(outputPins.at(pinIndex)); | |
| 408 | ✗ | _pinData.erase(_pinData.begin() + static_cast<int64_t>(pinIndex - 1)); | |
| 409 | ✗ | --_nInputPins; | |
| 410 | ✗ | flow::ApplyChanges(); | |
| 411 | ✗ | updateNumberOfInputPins(); | |
| 412 | } | ||
| 413 | ✗ | if (ImGui::IsItemHovered()) | |
| 414 | { | ||
| 415 | ✗ | ImGui::SetTooltip("Delete the pin"); | |
| 416 | } | ||
| 417 | } | ||
| 418 | } | ||
| 419 | } | ||
| 420 | |||
| 421 | ✗ | ImGui::TableNextRow(); | |
| 422 | ✗ | ImGui::TableNextColumn(); // Pin | |
| 423 | ✗ | if (ImGui::Button(fmt::format("Add Pin##{}", size_t(id)).c_str())) | |
| 424 | { | ||
| 425 | ✗ | ++_nInputPins; | |
| 426 | ✗ | LOG_DEBUG("{}: # Input Pins changed to {}", nameId(), _nInputPins); | |
| 427 | ✗ | flow::ApplyChanges(); | |
| 428 | ✗ | updateNumberOfInputPins(); | |
| 429 | } | ||
| 430 | |||
| 431 | ✗ | ImGui::EndTable(); | |
| 432 | } | ||
| 433 | |||
| 434 | ✗ | float columnWidth = 130.0F * gui::NodeEditorApplication::windowFontRatio(); | |
| 435 | |||
| 436 | ✗ | ImGui::Separator(); | |
| 437 | |||
| 438 | // ####################################################################################################### | ||
| 439 | // KF config | ||
| 440 | // ####################################################################################################### | ||
| 441 | ✗ | ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); | |
| 442 | |||
| 443 | ✗ | ImGui::SetNextItemWidth(columnWidth); | |
| 444 | ✗ | if (gui::widgets::EnumCombo(fmt::format("IMU fusion type##{}", size_t(id)).c_str(), _imuFusionType)) | |
| 445 | { | ||
| 446 | ✗ | LOG_DEBUG("{}: imuFusionType changed to {}", nameId(), fmt::underlying(_imuFusionType)); | |
| 447 | |||
| 448 | ✗ | flow::ApplyChanges(); | |
| 449 | ✗ | doDeinitialize(); | |
| 450 | } | ||
| 451 | ✗ | ImGui::SameLine(); | |
| 452 | ✗ | gui::widgets::HelpMarker("IRWKF: Integrated-Random-Walk Kalman-Filter (estimates angular acceleration and jerk).\nB-spline KF: Kalman-Filter that estimates three equally spaced quadratic B-splines for angular rate and acceleration, respectively."); | |
| 453 | |||
| 454 | ✗ | ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); | |
| 455 | |||
| 456 | ✗ | ImGui::SetNextItemWidth(columnWidth); | |
| 457 | ✗ | if (ImGui::InputDoubleL(fmt::format("Highest IMU sample rate in [Hz]##{}", size_t(id)).c_str(), &_imuFrequency, 1e-3, 1e4, 0.0, 0.0, "%.0f")) | |
| 458 | { | ||
| 459 | ✗ | LOG_DEBUG("{}: imuFrequency changed to {}", nameId(), _imuFrequency); | |
| 460 | ✗ | flow::ApplyChanges(); | |
| 461 | } | ||
| 462 | ✗ | ImGui::SameLine(); | |
| 463 | ✗ | gui::widgets::HelpMarker("The inverse of this rate is used as the initial 'dt' for the Kalman Filter Prediction (Phi and Q)."); | |
| 464 | |||
| 465 | ✗ | if (_imuFusionType == ImuFusionType::Bspline) | |
| 466 | { | ||
| 467 | ✗ | ImGui::SetNextItemWidth(columnWidth); | |
| 468 | ✗ | if (ImGui::InputDoubleL(fmt::format("Spacing between the quadratic B-splines in [s]##{}", size_t(id)).c_str(), &_splineSpacing, 1e-3, 1.0, 0.0, 0.0, "%.3f")) | |
| 469 | { | ||
| 470 | ✗ | LOG_DEBUG("{}: splineSpacing changed to {}", nameId(), _splineSpacing); | |
| 471 | ✗ | flow::ApplyChanges(); | |
| 472 | } | ||
| 473 | ✗ | ImGui::SameLine(); | |
| 474 | ✗ | gui::widgets::HelpMarker("Time difference between each quadratic B-spline, maximum: 1.0 second"); | |
| 475 | } | ||
| 476 | |||
| 477 | ✗ | if (ImGui::Checkbox(fmt::format("Rank check for Kalman filter matrices##{}", size_t(id)).c_str(), &_checkKalmanMatricesRanks)) | |
| 478 | { | ||
| 479 | ✗ | LOG_DEBUG("{}: checkKalmanMatricesRanks {}", nameId(), _checkKalmanMatricesRanks); | |
| 480 | ✗ | flow::ApplyChanges(); | |
| 481 | } | ||
| 482 | ✗ | ImGui::SameLine(); | |
| 483 | ✗ | gui::widgets::HelpMarker("Computationally intensive - only recommended for debugging."); | |
| 484 | |||
| 485 | ✗ | if (_imuFusionType != ImuFusionType::IRWKF) | |
| 486 | { | ||
| 487 | ✗ | ImGui::BeginDisabled(); | |
| 488 | } | ||
| 489 | ✗ | if (ImGui::Checkbox(fmt::format("Auto-initialize Kalman filter##{}", size_t(id)).c_str(), &_autoInitKF)) | |
| 490 | { | ||
| 491 | LOG_DATA("{}: auto-initialize KF: {}", nameId(), _autoInitKF); | ||
| 492 | ✗ | flow::ApplyChanges(); | |
| 493 | } | ||
| 494 | ✗ | if (_imuFusionType != ImuFusionType::IRWKF) | |
| 495 | { | ||
| 496 | ✗ | if (_autoInitKF) | |
| 497 | { | ||
| 498 | ✗ | _autoInitKF = false; | |
| 499 | ✗ | LOG_INFO("{}: Auto-initialization for KF turned off. This is currently only available for the IRWKF.", nameId()); | |
| 500 | } | ||
| 501 | ✗ | ImGui::EndDisabled(); | |
| 502 | } | ||
| 503 | ✗ | ImGui::SameLine(); | |
| 504 | ✗ | gui::widgets::HelpMarker("Initializes the KF by averaging the data over a specified time frame. Currently only available for the IRWKF fusion type."); | |
| 505 | ✗ | if (ImGui::Checkbox(fmt::format("Characteristics of the multiple IMUs are identical##{}", size_t(id)).c_str(), &_imuCharacteristicsIdentical)) | |
| 506 | { | ||
| 507 | LOG_DATA("{}: imuCharacteristicsIdentical: {}", nameId(), _imuCharacteristicsIdentical); | ||
| 508 | ✗ | flow::ApplyChanges(); | |
| 509 | } | ||
| 510 | ✗ | ImGui::SameLine(); | |
| 511 | ✗ | gui::widgets::HelpMarker("GUI input cells can be reduced considerably."); | |
| 512 | ✗ | if (ImGui::Checkbox(fmt::format("Biases of the multiple IMUs are identical##{}", size_t(id)).c_str(), &_imuBiasesIdentical)) | |
| 513 | { | ||
| 514 | LOG_DATA("{}: imuBiasesIdentical: {}", nameId(), _imuBiasesIdentical); | ||
| 515 | ✗ | flow::ApplyChanges(); | |
| 516 | } | ||
| 517 | ✗ | ImGui::SameLine(); | |
| 518 | ✗ | gui::widgets::HelpMarker("GUI input cells can be reduced considerably."); | |
| 519 | |||
| 520 | ✗ | ImGui::Separator(); | |
| 521 | |||
| 522 | // ####################################################################################################### | ||
| 523 | // KF initialization | ||
| 524 | // ####################################################################################################### | ||
| 525 | |||
| 526 | ✗ | if (_autoInitKF) | |
| 527 | { | ||
| 528 | ✗ | ImGui::Text("Kalman Filter initialization (auto-init)"); | |
| 529 | |||
| 530 | ✗ | ImGui::SetNextItemWidth(columnWidth); | |
| 531 | ✗ | if (ImGui::InputDoubleL(fmt::format("Averaging time in [s]##{}", size_t(id)).c_str(), &_averageEndTime, 1e-3, 1e4, 0.0, 0.0, "%.0f")) | |
| 532 | { | ||
| 533 | ✗ | LOG_DEBUG("{}: averageEndTime changed to {}", nameId(), _averageEndTime); | |
| 534 | ✗ | flow::ApplyChanges(); | |
| 535 | } | ||
| 536 | ✗ | ImGui::SameLine(); | |
| 537 | ✗ | gui::widgets::HelpMarker("Determines how long the data is averaged before the KF is auto-initialized"); | |
| 538 | |||
| 539 | ✗ | if (ImGui::Checkbox(fmt::format("Initialize Jerk variance to acceleration variance and angular acceleration variance to angular rate variance##{}", size_t(id)).c_str(), &_initJerkAngAcc)) | |
| 540 | { | ||
| 541 | LOG_DATA("{}: initJerkAngAcc: {}", nameId(), _initJerkAngAcc); | ||
| 542 | ✗ | flow::ApplyChanges(); | |
| 543 | } | ||
| 544 | ✗ | ImGui::SameLine(); | |
| 545 | ✗ | gui::widgets::HelpMarker("Otherwise zero"); | |
| 546 | } | ||
| 547 | else | ||
| 548 | { | ||
| 549 | ✗ | ImGui::Text("Kalman Filter initialization (manual init)"); | |
| 550 | |||
| 551 | // ---------------------------------------- State vector x0 ------------------------------------------- | ||
| 552 | ✗ | ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); | |
| 553 | ✗ | if (ImGui::TreeNode(fmt::format("x - State vector##{}", size_t(id)).c_str())) | |
| 554 | { | ||
| 555 | ✗ | if (_imuFusionType == ImuFusionType::IRWKF) | |
| 556 | { | ||
| 557 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate##{}", size_t(id)).c_str(), | |
| 558 | ✗ | configWidth, unitWidth, _pinDataIRWKF.initAngularRate.data(), _pinDataIRWKF.initAngularRateUnit, "deg/s\0" | |
| 559 | "rad/s\0\0", | ||
| 560 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 561 | { | ||
| 562 | LOG_DATA("{}: initAngularRate changed to {}", nameId(), _pinDataIRWKF.initAngularRate); | ||
| 563 | LOG_DATA("{}: AngularRateUnit changed to {}", nameId(), fmt::underlying(_pinDataIRWKF.initAngularRateUnit)); | ||
| 564 | ✗ | flow::ApplyChanges(); | |
| 565 | } | ||
| 566 | |||
| 567 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration##{}", size_t(id)).c_str(), | |
| 568 | ✗ | configWidth, unitWidth, _pinDataIRWKF.initAcceleration.data(), _pinDataIRWKF.initAccelerationUnit, "m/s²\0\0", "%.2e", ImGuiInputTextFlags_CharsScientific)) | |
| 569 | { | ||
| 570 | LOG_DATA("{}: initAcceleration changed to {}", nameId(), _pinDataIRWKF.initAcceleration); | ||
| 571 | LOG_DATA("{}: initAccelerationUnit changed to {}", nameId(), fmt::underlying(_pinDataIRWKF.initAccelerationUnit)); | ||
| 572 | ✗ | flow::ApplyChanges(); | |
| 573 | } | ||
| 574 | |||
| 575 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular Acceleration##{}", size_t(id)).c_str(), | |
| 576 | ✗ | configWidth, unitWidth, _pinDataIRWKF.initAngularAcc.data(), _pinDataIRWKF.initAngularAccUnit, "deg/s²\0" | |
| 577 | "rad/s^2\0\0", | ||
| 578 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 579 | { | ||
| 580 | LOG_DATA("{}: initAngularAcc changed to {}", nameId(), _pinDataIRWKF.initAngularAcc); | ||
| 581 | LOG_DATA("{}: initAngularAccUnit changed to {}", nameId(), fmt::underlying(_pinDataIRWKF.initAngularAccUnit)); | ||
| 582 | ✗ | flow::ApplyChanges(); | |
| 583 | } | ||
| 584 | |||
| 585 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Jerk##{}", size_t(id)).c_str(), | |
| 586 | ✗ | configWidth, unitWidth, _pinDataIRWKF.initJerk.data(), _pinDataIRWKF.initJerkUnit, "m/s³\0\0", | |
| 587 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 588 | { | ||
| 589 | LOG_DATA("{}: initJerk changed to {}", nameId(), _pinDataIRWKF.initJerk); | ||
| 590 | LOG_DATA("{}: PinData::JerkVarianceUnit changed to {}", nameId(), fmt::underlying(_pinDataIRWKF.initJerkUnit)); | ||
| 591 | ✗ | flow::ApplyChanges(); | |
| 592 | } | ||
| 593 | } | ||
| 594 | else // (_imuFusionType == ImuFusionType::Bspline) | ||
| 595 | { | ||
| 596 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("B-spline coefficients for the angular rate##{}", size_t(id)).c_str(), | |
| 597 | ✗ | configWidth, unitWidth, _initCoeffsAngRateTemp.data(), _pinDataBsplineKF.initCoeffsAngularRateUnit, "deg/s\0" | |
| 598 | "rad/s\0\0", | ||
| 599 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 600 | { | ||
| 601 | LOG_DATA("{}: initCoeffsAngularRateUnit changed to {}", nameId(), fmt::underlying(_pinDataBsplineKF.initCoeffsAngularRateUnit)); | ||
| 602 | ✗ | flow::ApplyChanges(); | |
| 603 | } | ||
| 604 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("B-spline coefficients for the acceleration##{}", size_t(id)).c_str(), | |
| 605 | ✗ | configWidth, unitWidth, _initCoeffsAccelTemp.data(), _pinDataBsplineKF.initCoeffsAccelUnit, "m/s²\0\0", | |
| 606 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 607 | { | ||
| 608 | LOG_DATA("{}: initCoeffsAccelUnit changed to {}", nameId(), fmt::underlying(_pinDataBsplineKF.initCoeffsAccelUnit)); | ||
| 609 | ✗ | flow::ApplyChanges(); | |
| 610 | } | ||
| 611 | ✗ | for (uint8_t i = 0; i < _numBsplines; i += 3) | |
| 612 | { | ||
| 613 | ✗ | _pinDataBsplineKF.initCoeffsAngRate.block<3, 1>(i, 0) = _initCoeffsAngRateTemp; | |
| 614 | ✗ | _pinDataBsplineKF.initCoeffsAccel.block<3, 1>(i, 0) = _initCoeffsAccelTemp; | |
| 615 | } | ||
| 616 | LOG_DATA("{}: initCoeffsAngRate changed to {}", nameId(), _pinDataBsplineKF.initCoeffsAngRate); | ||
| 617 | LOG_DATA("{}: initCoeffsAccel changed to {}", nameId(), _pinDataBsplineKF.initCoeffsAccel); | ||
| 618 | } | ||
| 619 | |||
| 620 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate bias of sensor {}##{}", 2, size_t(id)).c_str(), | |
| 621 | ✗ | configWidth, unitWidth, _pinData[1].initAngularRateBias.data(), _pinData[1].initAngularRateBiasUnit, "deg/s\0rad/s\0\0", | |
| 622 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 623 | { | ||
| 624 | ✗ | flow::ApplyChanges(); | |
| 625 | } | ||
| 626 | |||
| 627 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration bias of sensor {}##{}", 2, size_t(id)).c_str(), | |
| 628 | ✗ | configWidth, unitWidth, _pinData[1].initAccelerationBias.data(), _pinData[1].initAccelerationBiasUnit, "m/s^2\0\0", | |
| 629 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 630 | { | ||
| 631 | ✗ | flow::ApplyChanges(); | |
| 632 | } | ||
| 633 | ✗ | if (!_imuBiasesIdentical) | |
| 634 | { | ||
| 635 | ✗ | for (size_t pinIndex = 2; pinIndex < _nInputPins; ++pinIndex) | |
| 636 | { | ||
| 637 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate bias of sensor {}##{}", pinIndex + 1, size_t(id)).c_str(), | |
| 638 | ✗ | configWidth, unitWidth, _pinData[pinIndex].initAngularRateBias.data(), _pinData[pinIndex].initAngularRateBiasUnit, "deg/s\0rad/s\0\0", | |
| 639 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 640 | { | ||
| 641 | ✗ | flow::ApplyChanges(); | |
| 642 | } | ||
| 643 | |||
| 644 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration bias of sensor {}##{}", pinIndex + 1, size_t(id)).c_str(), | |
| 645 | ✗ | configWidth, unitWidth, _pinData[pinIndex].initAccelerationBias.data(), _pinData[pinIndex].initAccelerationBiasUnit, "m/s^2\0\0", | |
| 646 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 647 | { | ||
| 648 | ✗ | flow::ApplyChanges(); | |
| 649 | } | ||
| 650 | } | ||
| 651 | } | ||
| 652 | |||
| 653 | ✗ | ImGui::TreePop(); | |
| 654 | } | ||
| 655 | |||
| 656 | // ----------------------------------- Error covariance matrix P0 ------------------------------------- | ||
| 657 | ✗ | ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); | |
| 658 | ✗ | if (ImGui::TreeNode(fmt::format("P - Error covariance matrix##{}", size_t(id)).c_str())) | |
| 659 | { | ||
| 660 | ✗ | if (_imuFusionType == ImuFusionType::IRWKF) | |
| 661 | { | ||
| 662 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate covariance ({})##{}", | |
| 663 | ✗ | _pinData[0].initCovarianceAngularRateUnit == PinData::AngRateVarianceUnit::rad2_s2 | |
| 664 | ✗ | || _pinData[0].initCovarianceAngularRateUnit == PinData::AngRateVarianceUnit::deg2_s2 | |
| 665 | ✗ | ? "Variance σ²" | |
| 666 | : "Standard deviation σ", | ||
| 667 | ✗ | size_t(id)) | |
| 668 | .c_str(), | ||
| 669 | ✗ | configWidth, unitWidth, _pinData[0].initCovarianceAngularRate.data(), _pinData[0].initCovarianceAngularRateUnit, "(rad/s)²\0" | |
| 670 | "rad/s\0" | ||
| 671 | "(deg/s)²\0" | ||
| 672 | "deg/s\0\0", | ||
| 673 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 674 | { | ||
| 675 | LOG_DATA("{}: initCovarianceAngularRate changed to {}", nameId(), _pinData[0].initCovarianceAngularRate); | ||
| 676 | LOG_DATA("{}: AngRateVarianceUnit changed to {}", nameId(), fmt::underlying(_pinData[0].initCovarianceAngularRateUnit)); | ||
| 677 | ✗ | flow::ApplyChanges(); | |
| 678 | } | ||
| 679 | |||
| 680 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular acceleration covariance ({})##{}", | |
| 681 | ✗ | _pinDataIRWKF.initCovarianceAngularAccUnit == PinDataIRWKF::AngularAccVarianceUnit::rad2_s4 | |
| 682 | ✗ | || _pinDataIRWKF.initCovarianceAngularAccUnit == PinDataIRWKF::AngularAccVarianceUnit::deg2_s4 | |
| 683 | ✗ | ? "Variance σ²" | |
| 684 | : "Standard deviation σ", | ||
| 685 | ✗ | size_t(id)) | |
| 686 | .c_str(), | ||
| 687 | ✗ | configWidth, unitWidth, _pinDataIRWKF.initCovarianceAngularAcc.data(), _pinDataIRWKF.initCovarianceAngularAccUnit, "(rad^2)/(s^4)\0" | |
| 688 | "rad/s^2\0" | ||
| 689 | "(deg^2)/(s^4)\0" | ||
| 690 | "deg/s^2\0\0", | ||
| 691 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 692 | { | ||
| 693 | LOG_DATA("{}: initCovarianceAngularAcc changed to {}", nameId(), _pinDataIRWKF.initCovarianceAngularAcc); | ||
| 694 | LOG_DATA("{}: PinData::AngularAccVarianceUnit changed to {}", nameId(), fmt::underlying(_pinDataIRWKF.initCovarianceAngularAccUnit)); | ||
| 695 | ✗ | flow::ApplyChanges(); | |
| 696 | } | ||
| 697 | |||
| 698 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration covariance ({})##{}", | |
| 699 | ✗ | _pinData[0].initCovarianceAccelerationUnit == PinData::AccelerationVarianceUnit::m2_s4 | |
| 700 | ✗ | ? "Variance σ²" | |
| 701 | : "Standard deviation σ", | ||
| 702 | ✗ | size_t(id)) | |
| 703 | .c_str(), | ||
| 704 | ✗ | configWidth, unitWidth, _pinData[0].initCovarianceAcceleration.data(), _pinData[0].initCovarianceAccelerationUnit, "(m^2)/(s^4)\0" | |
| 705 | "m/s^2\0\0", | ||
| 706 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 707 | { | ||
| 708 | LOG_DATA("{}: initCovarianceAcceleration changed to {}", nameId(), _pinData[0].initCovarianceAcceleration); | ||
| 709 | LOG_DATA("{}: PinData::AccelerationVarianceUnit changed to {}", nameId(), fmt::underlying(_pinData[0].initCovarianceAccelerationUnit)); | ||
| 710 | ✗ | flow::ApplyChanges(); | |
| 711 | } | ||
| 712 | |||
| 713 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Jerk covariance ({})##{}", | |
| 714 | ✗ | _pinDataIRWKF.initCovarianceJerkUnit == PinDataIRWKF::JerkVarianceUnit::m2_s6 | |
| 715 | ✗ | ? "Variance σ²" | |
| 716 | : "Standard deviation σ", | ||
| 717 | ✗ | size_t(id)) | |
| 718 | .c_str(), | ||
| 719 | ✗ | configWidth, unitWidth, _pinDataIRWKF.initCovarianceJerk.data(), _pinDataIRWKF.initCovarianceJerkUnit, "(m^2)/(s^6)\0" | |
| 720 | "m/s^3\0\0", | ||
| 721 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 722 | { | ||
| 723 | LOG_DATA("{}: initCovarianceJerk changed to {}", nameId(), _pinDataIRWKF.initCovarianceJerk); | ||
| 724 | LOG_DATA("{}: PinData::JerkVarianceUnit changed to {}", nameId(), fmt::underlying(_pinDataIRWKF.initCovarianceJerkUnit)); | ||
| 725 | ✗ | flow::ApplyChanges(); | |
| 726 | } | ||
| 727 | } | ||
| 728 | else // (_imuFusionType == ImuFusionType::Bspline) | ||
| 729 | { | ||
| 730 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Covariance of the B-spline coefficients of the angular rate ({})##{}", | |
| 731 | ✗ | _pinDataBsplineKF.initCovarianceCoeffsAngRateUnit == PinData::AngRateVarianceUnit::rad2_s2 | |
| 732 | ✗ | || _pinDataBsplineKF.initCovarianceCoeffsAngRateUnit == PinData::AngRateVarianceUnit::deg2_s2 | |
| 733 | ✗ | ? "Variance σ²" | |
| 734 | : "Standard deviation σ", | ||
| 735 | ✗ | size_t(id)) | |
| 736 | .c_str(), | ||
| 737 | ✗ | configWidth, unitWidth, _initCovarianceCoeffsAngRateTemp.data(), _pinDataBsplineKF.initCovarianceCoeffsAngRateUnit, "(rad/s)²\0" | |
| 738 | "rad/s\0" | ||
| 739 | "(deg/s)²\0" | ||
| 740 | "deg/s\0\0", | ||
| 741 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 742 | { | ||
| 743 | LOG_DATA("{}: initCovarianceCoeffsAngRateUnit changed to {}", nameId(), fmt::underlying(_pinDataBsplineKF.initCovarianceCoeffsAngRateUnit)); | ||
| 744 | ✗ | flow::ApplyChanges(); | |
| 745 | } | ||
| 746 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Covariance of the B-spline coefficients of the acceleration ({})##{}", | |
| 747 | ✗ | _pinDataBsplineKF.initCovarianceCoeffsAccelUnit == PinData::AccelerationVarianceUnit::m2_s4 | |
| 748 | ✗ | ? "Variance σ²" | |
| 749 | : "Standard deviation σ", | ||
| 750 | ✗ | size_t(id)) | |
| 751 | .c_str(), | ||
| 752 | ✗ | configWidth, unitWidth, _initCovarianceCoeffsAccelTemp.data(), _pinDataBsplineKF.initCovarianceCoeffsAccelUnit, "(m^2)/(s^4)\0" | |
| 753 | "m/s^2\0\0", | ||
| 754 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 755 | { | ||
| 756 | LOG_DATA("{}: initCovarianceCoeffsAccelUnit changed to {}", nameId(), fmt::underlying(_pinDataBsplineKF.initCovarianceCoeffsAccelUnit)); | ||
| 757 | ✗ | flow::ApplyChanges(); | |
| 758 | } | ||
| 759 | ✗ | for (uint8_t i = 0; i < _numBsplines; i += 3) | |
| 760 | { | ||
| 761 | ✗ | _pinDataBsplineKF.initCovarianceCoeffsAngRate.block<3, 1>(i, 0) = _initCovarianceCoeffsAngRateTemp; | |
| 762 | ✗ | _pinDataBsplineKF.initCovarianceCoeffsAccel.block<3, 1>(i, 0) = _initCovarianceCoeffsAccelTemp; | |
| 763 | } | ||
| 764 | LOG_DATA("{}: initCovarianceCoeffsAngRate changed to {}", nameId(), _pinDataBsplineKF.initCovarianceCoeffsAngRate); | ||
| 765 | LOG_DATA("{}: initCovarianceCoeffsAccel changed to {}", nameId(), _pinDataBsplineKF.initCovarianceCoeffsAccel); | ||
| 766 | } | ||
| 767 | |||
| 768 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate bias covariance of sensor {} ({})##{}", 2, | |
| 769 | ✗ | _pinData[1].initCovarianceBiasAngRateUnit == PinData::AngRateVarianceUnit::rad2_s2 | |
| 770 | ✗ | || _pinData[1].initCovarianceBiasAngRateUnit == PinData::AngRateVarianceUnit::deg2_s2 | |
| 771 | ✗ | ? "Variance σ²" | |
| 772 | : "Standard deviation σ", | ||
| 773 | ✗ | size_t(id)) | |
| 774 | .c_str(), | ||
| 775 | ✗ | configWidth, unitWidth, _pinData[1].initCovarianceBiasAngRate.data(), _pinData[1].initCovarianceBiasAngRateUnit, "(rad^2)/(s^2)\0" | |
| 776 | "rad/s\0" | ||
| 777 | "(deg^2)/(s^2)\0" | ||
| 778 | "deg/s\0\0", | ||
| 779 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 780 | { | ||
| 781 | LOG_DATA("{}: initCovarianceBiasAngRate changed to {}", nameId(), _pinData[1].initCovarianceBiasAngRate); | ||
| 782 | LOG_DATA("{}: PinData::AngRateVarianceUnit changed to {}", nameId(), fmt::underlying(_pinData[1].initCovarianceBiasAngRateUnit)); | ||
| 783 | ✗ | flow::ApplyChanges(); | |
| 784 | } | ||
| 785 | |||
| 786 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration bias covariance of sensor {} ({})##{}", 2, | |
| 787 | ✗ | _pinData[1].initCovarianceBiasAccUnit == PinData::AccelerationVarianceUnit::m2_s4 | |
| 788 | ✗ | ? "Variance σ²" | |
| 789 | : "Standard deviation σ", | ||
| 790 | ✗ | size_t(id)) | |
| 791 | .c_str(), | ||
| 792 | ✗ | configWidth, unitWidth, _pinData[1].initCovarianceBiasAcc.data(), _pinData[1].initCovarianceBiasAccUnit, "(m^2)/(s^4)\0" | |
| 793 | "m/s^2\0\0", | ||
| 794 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 795 | { | ||
| 796 | LOG_DATA("{}: initCovarianceBiasAcc changed to {}", nameId(), _pinData[1].initCovarianceBiasAcc); | ||
| 797 | LOG_DATA("{}: PinData::AccelerationVarianceUnit changed to {}", nameId(), fmt::underlying(_pinData[1].initCovarianceBiasAccUnit)); | ||
| 798 | ✗ | flow::ApplyChanges(); | |
| 799 | } | ||
| 800 | ✗ | if (!_imuCharacteristicsIdentical) | |
| 801 | { | ||
| 802 | ✗ | for (size_t pinIndex = 2; pinIndex < _nInputPins; ++pinIndex) | |
| 803 | { | ||
| 804 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate bias covariance of sensor {} ({})##{}", pinIndex + 1, | |
| 805 | ✗ | _pinData[pinIndex].initCovarianceBiasAngRateUnit == PinData::AngRateVarianceUnit::rad2_s2 | |
| 806 | ✗ | || _pinData[pinIndex].initCovarianceBiasAngRateUnit == PinData::AngRateVarianceUnit::deg2_s2 | |
| 807 | ✗ | ? "Variance σ²" | |
| 808 | : "Standard deviation σ", | ||
| 809 | ✗ | size_t(id)) | |
| 810 | .c_str(), | ||
| 811 | ✗ | configWidth, unitWidth, _pinData[pinIndex].initCovarianceBiasAngRate.data(), _pinData[pinIndex].initCovarianceBiasAngRateUnit, "(rad^2)/(s^2)\0" | |
| 812 | "rad/s\0" | ||
| 813 | "(deg^2)/(s^2)\0" | ||
| 814 | "deg/s\0\0", | ||
| 815 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 816 | { | ||
| 817 | LOG_DATA("{}: initCovarianceBiasAngRate changed to {}", nameId(), _pinData[pinIndex].initCovarianceBiasAngRate); | ||
| 818 | LOG_DATA("{}: PinData::AngRateVarianceUnit changed to {}", nameId(), fmt::underlying(_pinData[pinIndex].initCovarianceBiasAngRateUnit)); | ||
| 819 | ✗ | flow::ApplyChanges(); | |
| 820 | } | ||
| 821 | |||
| 822 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration bias covariance of sensor {} ({})##{}", pinIndex + 1, | |
| 823 | ✗ | _pinData[pinIndex].initCovarianceBiasAccUnit == PinData::AccelerationVarianceUnit::m2_s4 | |
| 824 | ✗ | ? "Variance σ²" | |
| 825 | : "Standard deviation σ", | ||
| 826 | ✗ | size_t(id)) | |
| 827 | .c_str(), | ||
| 828 | ✗ | configWidth, unitWidth, _pinData[pinIndex].initCovarianceBiasAcc.data(), _pinData[pinIndex].initCovarianceBiasAccUnit, "(m^2)/(s^4)\0" | |
| 829 | "m/s^2\0\0", | ||
| 830 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 831 | { | ||
| 832 | LOG_DATA("{}: initCovarianceBiasAcc changed to {}", nameId(), _pinData[pinIndex].initCovarianceBiasAcc); | ||
| 833 | LOG_DATA("{}: PinData::AccelerationVarianceUnit changed to {}", nameId(), fmt::underlying(_pinData[pinIndex].initCovarianceBiasAccUnit)); | ||
| 834 | ✗ | flow::ApplyChanges(); | |
| 835 | } | ||
| 836 | } | ||
| 837 | } | ||
| 838 | |||
| 839 | ✗ | ImGui::TreePop(); | |
| 840 | } | ||
| 841 | } | ||
| 842 | |||
| 843 | ✗ | ImGui::Separator(); | |
| 844 | |||
| 845 | // ####################################################################################################### | ||
| 846 | // KF noise setting | ||
| 847 | // ####################################################################################################### | ||
| 848 | ✗ | ImGui::Text("Kalman Filter noise setting"); | |
| 849 | |||
| 850 | // -------------------------------------- Process noise matrix Q ----------------------------------------- | ||
| 851 | ✗ | ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); | |
| 852 | ✗ | if (ImGui::TreeNode(fmt::format("Q - System/Process noise covariance matrix##{}", size_t(id)).c_str())) | |
| 853 | { | ||
| 854 | ✗ | ImGui::SetNextItemWidth(configWidth + ImGui::GetStyle().ItemSpacing.x); | |
| 855 | |||
| 856 | ✗ | if (_imuFusionType == ImuFusionType::IRWKF) | |
| 857 | { | ||
| 858 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular acceleration ({})##{}", | |
| 859 | ✗ | _pinDataIRWKF.varAngularAccNoiseUnit == PinDataIRWKF::AngularAccVarianceUnit::rad2_s4 | |
| 860 | ✗ | || _pinDataIRWKF.varAngularAccNoiseUnit == PinDataIRWKF::AngularAccVarianceUnit::deg2_s4 | |
| 861 | ✗ | ? "Variance σ²" | |
| 862 | : "Standard deviation σ", | ||
| 863 | ✗ | size_t(id)) | |
| 864 | .c_str(), | ||
| 865 | ✗ | configWidth, unitWidth, _pinDataIRWKF.varAngularAccNoise.data(), _pinDataIRWKF.varAngularAccNoiseUnit, "(rad^2)/(s^4)\0" | |
| 866 | "rad/s^2\0" | ||
| 867 | "(deg^2)/(s^4)\0" | ||
| 868 | "deg/s^2\0\0", | ||
| 869 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 870 | { | ||
| 871 | LOG_DATA("{}: varAngularAccNoise changed to {}", nameId(), _pinDataIRWKF.varAngularAccNoise.transpose()); | ||
| 872 | LOG_DATA("{}: varAngularAccNoiseUnit changed to {}", nameId(), fmt::underlying(_pinDataIRWKF.varAngularAccNoiseUnit)); | ||
| 873 | ✗ | flow::ApplyChanges(); | |
| 874 | } | ||
| 875 | |||
| 876 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Jerk ({})##{}", | |
| 877 | ✗ | _pinDataIRWKF.varJerkNoiseUnit == PinDataIRWKF::JerkVarianceUnit::m2_s6 | |
| 878 | ✗ | ? "Variance σ²" | |
| 879 | : "Standard deviation σ", | ||
| 880 | ✗ | size_t(id)) | |
| 881 | .c_str(), | ||
| 882 | ✗ | configWidth, unitWidth, _pinDataIRWKF.varJerkNoise.data(), _pinDataIRWKF.varJerkNoiseUnit, "(m^2)/(s^6)\0" | |
| 883 | "m/s^3\0\0", | ||
| 884 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 885 | { | ||
| 886 | LOG_DATA("{}: varJerkNoise changed to {}", nameId(), _pinDataIRWKF.varJerkNoise.transpose()); | ||
| 887 | LOG_DATA("{}: varJerkNoiseUnit changed to {}", nameId(), fmt::underlying(_pinDataIRWKF.varJerkNoiseUnit)); | ||
| 888 | ✗ | flow::ApplyChanges(); | |
| 889 | } | ||
| 890 | } | ||
| 891 | else // (_imuFusionType == ImuFusionType::Bspline) | ||
| 892 | { | ||
| 893 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate B-spline coefficients ({})##{}", | |
| 894 | ✗ | _pinDataBsplineKF.varCoeffsAngRateUnit == PinData::AngRateVarianceUnit::rad2_s2 | |
| 895 | ✗ | || _pinDataBsplineKF.varCoeffsAngRateUnit == PinData::AngRateVarianceUnit::deg2_s2 | |
| 896 | ✗ | ? "Variance σ²" | |
| 897 | : "Standard deviation σ", | ||
| 898 | ✗ | size_t(id)) | |
| 899 | .c_str(), | ||
| 900 | ✗ | configWidth, unitWidth, _procNoiseCoeffsAngRateTemp.data(), _pinDataBsplineKF.varCoeffsAngRateUnit, "(rad^2)/(s^2)\0" | |
| 901 | "rad/s\0" | ||
| 902 | "(deg^2)/(s^2)\0" | ||
| 903 | "deg/s\0\0", | ||
| 904 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 905 | { | ||
| 906 | LOG_DATA("{}: varCoeffsAngRateUnit changed to {}", nameId(), fmt::underlying(_pinDataBsplineKF.varCoeffsAngRateUnit)); | ||
| 907 | ✗ | flow::ApplyChanges(); | |
| 908 | } | ||
| 909 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration B-spline coefficients ({})##{}", | |
| 910 | ✗ | _pinDataBsplineKF.varCoeffsAccelUnit == PinData::AccelerationVarianceUnit::m2_s4 | |
| 911 | ✗ | ? "Variance σ²" | |
| 912 | : "Standard deviation σ", | ||
| 913 | ✗ | size_t(id)) | |
| 914 | .c_str(), | ||
| 915 | ✗ | configWidth, unitWidth, _procNoiseCoeffsAccelTemp.data(), _pinDataBsplineKF.varCoeffsAccelUnit, "(m^2)/(s^4)\0" | |
| 916 | "m/s^2\0\0", | ||
| 917 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 918 | { | ||
| 919 | LOG_DATA("{}: varCoeffsAccelUnit changed to {}", nameId(), fmt::underlying(_pinDataBsplineKF.varCoeffsAccelUnit)); | ||
| 920 | ✗ | flow::ApplyChanges(); | |
| 921 | } | ||
| 922 | ✗ | for (uint8_t i = 0; i < _numBsplines; i += 3) | |
| 923 | { | ||
| 924 | ✗ | _pinDataBsplineKF.varCoeffsAngRateNoise.block<3, 1>(i, 0) = _procNoiseCoeffsAngRateTemp; | |
| 925 | ✗ | _pinDataBsplineKF.varCoeffsAccelNoise.block<3, 1>(i, 0) = _procNoiseCoeffsAccelTemp; | |
| 926 | } | ||
| 927 | LOG_DATA("{}: varCoeffsAngRateNoise changed to {}", nameId(), _pinDataBsplineKF.varCoeffsAngRateNoise.transpose()); | ||
| 928 | LOG_DATA("{}: varCoeffsAccelNoise changed to {}", nameId(), _pinDataBsplineKF.varCoeffsAccelNoise.transpose()); | ||
| 929 | } | ||
| 930 | |||
| 931 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Bias of the angular rate of sensor {} ({})##{}", 2, | |
| 932 | ✗ | _pinData[1].varBiasAngRateNoiseUnit == PinData::AngRateVarianceUnit::rad2_s2 | |
| 933 | ✗ | || _pinData[1].varBiasAngRateNoiseUnit == PinData::AngRateVarianceUnit::deg2_s2 | |
| 934 | ✗ | ? "Variance σ²" | |
| 935 | : "Standard deviation σ", | ||
| 936 | ✗ | size_t(id)) | |
| 937 | .c_str(), // FIXME: adapt config window number of sensors (if pin 3 is deleted, keep 1,2,4 instead of re-counting to 1,2,3) | ||
| 938 | ✗ | configWidth, unitWidth, _pinData[1].varBiasAngRateNoise.data(), _pinData[1].varBiasAngRateNoiseUnit, "(rad/s)^2\0" | |
| 939 | "rad/s\0" | ||
| 940 | "(deg/s)^2\0" | ||
| 941 | "deg/s\0\0", | ||
| 942 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 943 | { | ||
| 944 | LOG_DATA("{}: varBiasAngRateNoise changed to {}", nameId(), _pinData[1].varBiasAngRateNoise.transpose()); | ||
| 945 | LOG_DATA("{}: varBiasAngRateNoiseUnit changed to {}", nameId(), fmt::underlying(_pinData[1].varBiasAngRateNoiseUnit)); | ||
| 946 | ✗ | flow::ApplyChanges(); | |
| 947 | } | ||
| 948 | |||
| 949 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Bias of the acceleration of sensor {} ({})##{}", 2, | |
| 950 | ✗ | _pinData[1].varBiasAccelerationNoiseUnit == PinData::AccelerationVarianceUnit::m2_s4 | |
| 951 | ✗ | ? "Variance σ²" | |
| 952 | : "Standard deviation σ", | ||
| 953 | ✗ | size_t(id)) | |
| 954 | .c_str(), // FIXME: adapt config window number of sensors (if pin 3 is deleted, keep 1,2,4 instead of re-counting to 1,2,3) | ||
| 955 | ✗ | configWidth, unitWidth, _pinData[1].varBiasAccelerationNoise.data(), _pinData[1].varBiasAccelerationNoiseUnit, "(m^2)/(s^4)\0" | |
| 956 | "m/s^2\0\0", | ||
| 957 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 958 | { | ||
| 959 | LOG_DATA("{}: varBiasAccelerationNoise changed to {}", nameId(), _pinData[1].varBiasAccelerationNoise.transpose()); | ||
| 960 | LOG_DATA("{}: varBiasAccelerationNoiseUnit changed to {}", nameId(), fmt::underlying(_pinData[1].varBiasAccelerationNoiseUnit)); | ||
| 961 | ✗ | flow::ApplyChanges(); | |
| 962 | } | ||
| 963 | ✗ | if (!_imuCharacteristicsIdentical) | |
| 964 | { | ||
| 965 | ✗ | for (size_t pinIndex = 2; pinIndex < _nInputPins; ++pinIndex) | |
| 966 | { | ||
| 967 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Bias of the angular rate of sensor {} ({})##{}", pinIndex + 1, | |
| 968 | ✗ | _pinData[pinIndex].varBiasAngRateNoiseUnit == PinData::AngRateVarianceUnit::rad2_s2 | |
| 969 | ✗ | || _pinData[pinIndex].varBiasAngRateNoiseUnit == PinData::AngRateVarianceUnit::deg2_s2 | |
| 970 | ✗ | ? "Variance σ²" | |
| 971 | : "Standard deviation σ", | ||
| 972 | ✗ | size_t(id)) | |
| 973 | .c_str(), // FIXME: adapt config window number of sensors (if pin 3 is deleted, keep 1,2,4 instead of re-counting to 1,2,3) | ||
| 974 | ✗ | configWidth, unitWidth, _pinData[pinIndex].varBiasAngRateNoise.data(), _pinData[pinIndex].varBiasAngRateNoiseUnit, "(rad/s)^2\0" | |
| 975 | "rad/s\0" | ||
| 976 | "(deg/s)^2\0" | ||
| 977 | "deg/s\0\0", | ||
| 978 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 979 | { | ||
| 980 | LOG_DATA("{}: varBiasAngRateNoise changed to {}", nameId(), _pinData[pinIndex].varBiasAngRateNoise.transpose()); | ||
| 981 | LOG_DATA("{}: varBiasAngRateNoiseUnit changed to {}", nameId(), fmt::underlying(_pinData[pinIndex].varBiasAngRateNoiseUnit)); | ||
| 982 | ✗ | flow::ApplyChanges(); | |
| 983 | } | ||
| 984 | |||
| 985 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Bias of the acceleration of sensor {} ({})##{}", pinIndex + 1, | |
| 986 | ✗ | _pinData[pinIndex].varBiasAccelerationNoiseUnit == PinData::AccelerationVarianceUnit::m2_s4 | |
| 987 | ✗ | ? "Variance σ²" | |
| 988 | : "Standard deviation σ", | ||
| 989 | ✗ | size_t(id)) | |
| 990 | .c_str(), // FIXME: adapt config window number of sensors (if pin 3 is deleted, keep 1,2,4 instead of re-counting to 1,2,3) | ||
| 991 | ✗ | configWidth, unitWidth, _pinData[pinIndex].varBiasAccelerationNoise.data(), _pinData[pinIndex].varBiasAccelerationNoiseUnit, "(m^2)/(s^4)\0" | |
| 992 | "m/s^2\0\0", | ||
| 993 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 994 | { | ||
| 995 | LOG_DATA("{}: varBiasAccelerationNoise changed to {}", nameId(), _pinData[pinIndex].varBiasAccelerationNoise.transpose()); | ||
| 996 | LOG_DATA("{}: varBiasAccelerationNoiseUnit changed to {}", nameId(), fmt::underlying(_pinData[pinIndex].varBiasAccelerationNoiseUnit)); | ||
| 997 | ✗ | flow::ApplyChanges(); | |
| 998 | } | ||
| 999 | } | ||
| 1000 | } | ||
| 1001 | |||
| 1002 | ✗ | ImGui::TreePop(); | |
| 1003 | } | ||
| 1004 | |||
| 1005 | // ------------------------------------ Measurement noise matrix R --------------------------------------- | ||
| 1006 | ✗ | ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); | |
| 1007 | ✗ | if (ImGui::TreeNode(fmt::format("R - Measurement noise covariance matrix##{}", size_t(id)).c_str())) | |
| 1008 | { | ||
| 1009 | ✗ | ImGui::SetNextItemWidth(configWidth + ImGui::GetStyle().ItemSpacing.x); | |
| 1010 | |||
| 1011 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate of sensor {} ({})##{}", 1, | |
| 1012 | ✗ | _pinData[0].measurementUncertaintyAngularRateUnit == PinData::AngRateVarianceUnit::rad2_s2 | |
| 1013 | ✗ | || _pinData[0].measurementUncertaintyAngularRateUnit == PinData::AngRateVarianceUnit::deg2_s2 | |
| 1014 | ✗ | ? "Variance σ²" | |
| 1015 | : "Standard deviation σ", | ||
| 1016 | ✗ | size_t(id)) | |
| 1017 | .c_str(), | ||
| 1018 | ✗ | configWidth, unitWidth, _pinData[0].measurementUncertaintyAngularRate.data(), _pinData[0].measurementUncertaintyAngularRateUnit, "(rad/s)^2\0" | |
| 1019 | "rad/s\0" | ||
| 1020 | "(deg/s)^2\0" | ||
| 1021 | "deg/s\0\0", | ||
| 1022 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 1023 | { | ||
| 1024 | LOG_DATA("{}: stdevAngularAcc changed to {}", nameId(), _pinData[0].measurementUncertaintyAngularRate.transpose()); | ||
| 1025 | LOG_DATA("{}: stdevAngularAccUnit changed to {}", nameId(), fmt::underlying(_pinData[0].measurementUncertaintyAngularRateUnit)); | ||
| 1026 | ✗ | flow::ApplyChanges(); | |
| 1027 | } | ||
| 1028 | |||
| 1029 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration of sensor {} ({})##{}", 1, | |
| 1030 | ✗ | _pinData[0].measurementUncertaintyAccelerationUnit == PinData::AccelerationVarianceUnit::m2_s4 | |
| 1031 | ✗ | ? "Variance σ²" | |
| 1032 | : "Standard deviation σ", | ||
| 1033 | ✗ | size_t(id)) | |
| 1034 | .c_str(), | ||
| 1035 | ✗ | configWidth, unitWidth, _pinData[0].measurementUncertaintyAcceleration.data(), _pinData[0].measurementUncertaintyAccelerationUnit, "(m^2)/(s^4)\0" | |
| 1036 | "m/s^2\0\0", | ||
| 1037 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 1038 | { | ||
| 1039 | LOG_DATA("{}: stdevJerk changed to {}", nameId(), _pinData[0].measurementUncertaintyAcceleration.transpose()); | ||
| 1040 | LOG_DATA("{}: stdevJerkUnit changed to {}", nameId(), fmt::underlying(_pinData[0].measurementUncertaintyAccelerationUnit)); | ||
| 1041 | ✗ | flow::ApplyChanges(); | |
| 1042 | } | ||
| 1043 | ✗ | if (!_imuCharacteristicsIdentical) | |
| 1044 | { | ||
| 1045 | ✗ | for (size_t pinIndex = 1; pinIndex < _nInputPins; ++pinIndex) | |
| 1046 | { | ||
| 1047 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Angular rate of sensor {} ({})##{}", pinIndex + 1, | |
| 1048 | ✗ | _pinData[pinIndex].measurementUncertaintyAngularRateUnit == PinData::AngRateVarianceUnit::rad2_s2 | |
| 1049 | ✗ | || _pinData[pinIndex].measurementUncertaintyAngularRateUnit == PinData::AngRateVarianceUnit::deg2_s2 | |
| 1050 | ✗ | ? "Variance σ²" | |
| 1051 | : "Standard deviation σ", | ||
| 1052 | ✗ | size_t(id)) | |
| 1053 | .c_str(), | ||
| 1054 | ✗ | configWidth, unitWidth, _pinData[pinIndex].measurementUncertaintyAngularRate.data(), _pinData[pinIndex].measurementUncertaintyAngularRateUnit, "(rad/s)^2\0" | |
| 1055 | "rad/s\0" | ||
| 1056 | "(deg/s)^2\0" | ||
| 1057 | "deg/s\0\0", | ||
| 1058 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 1059 | { | ||
| 1060 | LOG_DATA("{}: stdevAngularAcc changed to {}", nameId(), _pinData[pinIndex].measurementUncertaintyAngularRate.transpose()); | ||
| 1061 | LOG_DATA("{}: stdevAngularAccUnit changed to {}", nameId(), fmt::underlying(_pinData[pinIndex].measurementUncertaintyAngularRateUnit)); | ||
| 1062 | ✗ | flow::ApplyChanges(); | |
| 1063 | } | ||
| 1064 | |||
| 1065 | ✗ | if (gui::widgets::InputDouble3WithUnit(fmt::format("Acceleration of sensor {} ({})##{}", pinIndex + 1, | |
| 1066 | ✗ | _pinData[pinIndex].measurementUncertaintyAccelerationUnit == PinData::AccelerationVarianceUnit::m2_s4 | |
| 1067 | ✗ | ? "Variance σ²" | |
| 1068 | : "Standard deviation σ", | ||
| 1069 | ✗ | size_t(id)) | |
| 1070 | .c_str(), | ||
| 1071 | ✗ | configWidth, unitWidth, _pinData[pinIndex].measurementUncertaintyAcceleration.data(), _pinData[pinIndex].measurementUncertaintyAccelerationUnit, "(m^2)/(s^4)\0" | |
| 1072 | "m/s^2\0\0", | ||
| 1073 | "%.2e", ImGuiInputTextFlags_CharsScientific)) | ||
| 1074 | { | ||
| 1075 | LOG_DATA("{}: stdevJerk changed to {}", nameId(), _pinData[pinIndex].measurementUncertaintyAcceleration.transpose()); | ||
| 1076 | LOG_DATA("{}: stdevJerkUnit changed to {}", nameId(), fmt::underlying(_pinData[pinIndex].measurementUncertaintyAccelerationUnit)); | ||
| 1077 | ✗ | flow::ApplyChanges(); | |
| 1078 | } | ||
| 1079 | } | ||
| 1080 | } | ||
| 1081 | |||
| 1082 | ✗ | ImGui::TreePop(); | |
| 1083 | } | ||
| 1084 | ✗ | } | |
| 1085 | |||
| 1086 | ✗ | [[nodiscard]] json NAV::ImuFusion::save() const | |
| 1087 | { | ||
| 1088 | LOG_TRACE("{}: called", nameId()); | ||
| 1089 | |||
| 1090 | ✗ | json j; | |
| 1091 | |||
| 1092 | ✗ | j["imuFusionType"] = _imuFusionType; | |
| 1093 | ✗ | j["checkKalmanMatricesRanks"] = _checkKalmanMatricesRanks; | |
| 1094 | ✗ | j["nInputPins"] = _nInputPins; | |
| 1095 | ✗ | j["imuFrequency"] = _imuFrequency; | |
| 1096 | ✗ | j["numStates"] = _numStates; | |
| 1097 | ✗ | j["pinData"] = _pinData; | |
| 1098 | ✗ | j["pinDataIRWKF"] = _pinDataIRWKF; | |
| 1099 | ✗ | j["pinDataBsplineKF"] = _pinDataBsplineKF; | |
| 1100 | ✗ | j["initCoeffsAngRateTemp"] = _initCoeffsAngRateTemp; | |
| 1101 | ✗ | j["initCoeffsAccelTemp"] = _initCoeffsAccelTemp; | |
| 1102 | ✗ | j["initCovarianceCoeffsAngRateTemp"] = _initCovarianceCoeffsAngRateTemp; | |
| 1103 | ✗ | j["initCovarianceCoeffsAccelTemp"] = _initCovarianceCoeffsAccelTemp; | |
| 1104 | ✗ | j["procNoiseCoeffsAngRateTemp"] = _procNoiseCoeffsAngRateTemp; | |
| 1105 | ✗ | j["procNoiseCoeffsAccelTemp"] = _procNoiseCoeffsAccelTemp; | |
| 1106 | ✗ | j["autoInitKF"] = _autoInitKF; | |
| 1107 | ✗ | j["initJerkAngAcc"] = _initJerkAngAcc; | |
| 1108 | ✗ | j["kfInitialized"] = _kfInitialized; | |
| 1109 | ✗ | j["averageEndTime"] = _averageEndTime; | |
| 1110 | ✗ | j["splineSpacing"] = _splineSpacing; | |
| 1111 | |||
| 1112 | ✗ | return j; | |
| 1113 | ✗ | } | |
| 1114 | |||
| 1115 | 2 | void NAV::ImuFusion::restore(json const& j) | |
| 1116 | { | ||
| 1117 | LOG_TRACE("{}: called", nameId()); | ||
| 1118 | |||
| 1119 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("imuFusionType")) |
| 1120 | { | ||
| 1121 | 2 | j.at("imuFusionType").get_to(_imuFusionType); | |
| 1122 | } | ||
| 1123 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("checkKalmanMatricesRanks")) |
| 1124 | { | ||
| 1125 | 2 | j.at("checkKalmanMatricesRanks").get_to(_checkKalmanMatricesRanks); | |
| 1126 | } | ||
| 1127 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("nInputPins")) |
| 1128 | { | ||
| 1129 | 2 | j.at("nInputPins").get_to(_nInputPins); | |
| 1130 | 2 | updateNumberOfInputPins(); | |
| 1131 | } | ||
| 1132 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("imuFrequency")) |
| 1133 | { | ||
| 1134 | 2 | j.at("imuFrequency").get_to(_imuFrequency); | |
| 1135 | } | ||
| 1136 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("numStates")) |
| 1137 | { | ||
| 1138 | 2 | j.at("numStates").get_to(_numStates); | |
| 1139 | } | ||
| 1140 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("pinData")) |
| 1141 | { | ||
| 1142 | 2 | j.at("pinData").get_to(_pinData); | |
| 1143 | } | ||
| 1144 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("pinDataIRWKF")) |
| 1145 | { | ||
| 1146 | 2 | j.at("pinDataIRWKF").get_to(_pinDataIRWKF); | |
| 1147 | } | ||
| 1148 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("pinDataBsplineKF")) |
| 1149 | { | ||
| 1150 | 2 | j.at("pinDataBsplineKF").get_to(_pinDataBsplineKF); | |
| 1151 | } | ||
| 1152 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initCoeffsAngRateTemp")) |
| 1153 | { | ||
| 1154 | 2 | j.at("initCoeffsAngRateTemp").get_to(_initCoeffsAngRateTemp); | |
| 1155 | } | ||
| 1156 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initCoeffsAccelTemp")) |
| 1157 | { | ||
| 1158 | 2 | j.at("initCoeffsAccelTemp").get_to(_initCoeffsAccelTemp); | |
| 1159 | } | ||
| 1160 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initCovarianceCoeffsAngRateTemp")) |
| 1161 | { | ||
| 1162 | 2 | j.at("initCovarianceCoeffsAngRateTemp").get_to(_initCovarianceCoeffsAngRateTemp); | |
| 1163 | } | ||
| 1164 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initCovarianceCoeffsAccelTemp")) |
| 1165 | { | ||
| 1166 | 2 | j.at("initCovarianceCoeffsAccelTemp").get_to(_initCovarianceCoeffsAccelTemp); | |
| 1167 | } | ||
| 1168 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("procNoiseCoeffsAngRateTemp")) |
| 1169 | { | ||
| 1170 | 2 | j.at("procNoiseCoeffsAngRateTemp").get_to(_procNoiseCoeffsAngRateTemp); | |
| 1171 | } | ||
| 1172 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("procNoiseCoeffsAccelTemp")) |
| 1173 | { | ||
| 1174 | 2 | j.at("procNoiseCoeffsAccelTemp").get_to(_procNoiseCoeffsAccelTemp); | |
| 1175 | } | ||
| 1176 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("autoInitKF")) |
| 1177 | { | ||
| 1178 | 2 | j.at("autoInitKF").get_to(_autoInitKF); | |
| 1179 | } | ||
| 1180 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("initJerkAngAcc")) |
| 1181 | { | ||
| 1182 | 2 | j.at("initJerkAngAcc").get_to(_initJerkAngAcc); | |
| 1183 | } | ||
| 1184 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
|
2 | if (j.contains("_kfInitialized")) |
| 1185 | { | ||
| 1186 | ✗ | j.at("_kfInitialized").get_to(_kfInitialized); | |
| 1187 | } | ||
| 1188 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("averageEndTime")) |
| 1189 | { | ||
| 1190 | 2 | j.at("averageEndTime").get_to(_averageEndTime); | |
| 1191 | } | ||
| 1192 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (j.contains("splineSpacing")) |
| 1193 | { | ||
| 1194 | 2 | j.at("splineSpacing").get_to(_splineSpacing); | |
| 1195 | } | ||
| 1196 | 2 | } | |
| 1197 | |||
| 1198 | 6 | bool NAV::ImuFusion::initialize() | |
| 1199 | { | ||
| 1200 | LOG_TRACE("{}: called", nameId()); | ||
| 1201 | |||
| 1202 | 6 | _imuRotations_accel.clear(); | |
| 1203 | 6 | _imuRotations_gyro.clear(); | |
| 1204 | 6 | _biasCovariances.clear(); | |
| 1205 | 6 | _processNoiseVariances.clear(); | |
| 1206 | 6 | _measurementNoiseVariances.clear(); | |
| 1207 | |||
| 1208 | 6 | _cumulatedImuObs.clear(); | |
| 1209 | 6 | _cumulatedPinIds.clear(); | |
| 1210 | 6 | _lastFiltObs.reset(); | |
| 1211 | 6 | _latestTimestamp = InsTime{}; | |
| 1212 | 6 | _firstTimestamp.reset(); | |
| 1213 | |||
| 1214 | 6 | _imuPosSet = false; | |
| 1215 | 6 | _kfInitialized = false; | |
| 1216 | |||
| 1217 |
2/2✓ Branch 1 taken 27 times.
✓ Branch 2 taken 6 times.
|
33 | for (size_t pinIndex = 0; pinIndex < _pinData.size(); pinIndex++) |
| 1218 | { | ||
| 1219 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 27 times.
|
27 | if (!inputPins.at(pinIndex).isPinLinked()) |
| 1220 | { | ||
| 1221 | ✗ | LOG_INFO("Fewer links than input pins - Consider deleting pins that are not connected to limit KF matrices to the necessary size."); | |
| 1222 | } | ||
| 1223 | } | ||
| 1224 | |||
| 1225 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
|
6 | _numStatesEst = _imuFusionType == ImuFusionType::IRWKF ? _numStatesEstIRWKF : _numStatesEstBsplineKF; |
| 1226 | |||
| 1227 | 6 | _numStates = _numStatesEst + static_cast<uint8_t>((_nInputPins - 1) * _numStatesPerPin); | |
| 1228 | |||
| 1229 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | _kalmanFilter = KalmanFilter{ _numStates, _numMeasurements }; |
| 1230 | 6 | _kalmanFilter.setZero(); | |
| 1231 | |||
| 1232 | 6 | initializeMountingAngles(); | |
| 1233 | |||
| 1234 | // --------------------------------------------------------- KF Initializations ------------------------------------------------------------ | ||
| 1235 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
|
6 | if (!_autoInitKF) // i.e. manual initialization thru inputs from the GUI |
| 1236 | { | ||
| 1237 | 3 | auto dtInit = 1.0 / _imuFrequency; // Initial state transition time in [s] | |
| 1238 | |||
| 1239 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
|
3 | if (_imuFusionType == ImuFusionType::IRWKF) |
| 1240 | { | ||
| 1241 | ✗ | _kalmanFilter = IRWKF::initializeKalmanFilterManually(_nInputPins, _pinData, _pinDataIRWKF, _numStates, dtInit, _processNoiseVariances, _kalmanFilter, _imuCharacteristicsIdentical, _imuBiasesIdentical); | |
| 1242 | } | ||
| 1243 | else // (_imuFusionType == ImuFusionType::BsplineKF) | ||
| 1244 | { | ||
| 1245 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | _kalmanFilter = BsplineKF::initializeKalmanFilterManually(_nInputPins, _pinData, _pinDataBsplineKF, _numStates, dtInit, _processNoiseVariances, _kalmanFilter, _imuCharacteristicsIdentical, _imuBiasesIdentical); |
| 1246 | 3 | _latestKnot = 0.0; | |
| 1247 | } | ||
| 1248 | |||
| 1249 | LOG_DATA("{}: Initial kalmanFilter.x = {}", nameId(), _kalmanFilter.x.transpose()); | ||
| 1250 | LOG_DATA("{}: Initial kalmanFilter.P =\n{}", nameId(), _kalmanFilter.P); | ||
| 1251 | LOG_DATA("{}: Initial kalmanFilter.Phi =\n{}", nameId(), _kalmanFilter.Phi); | ||
| 1252 | LOG_DATA("{}: Initial kalmanFilter.Q =\n{}", nameId(), _kalmanFilter.Q); | ||
| 1253 | } | ||
| 1254 | |||
| 1255 | // -------------------------------------------------- Measurement uncertainty matrix R ----------------------------------------------------- | ||
| 1256 | 6 | _measurementNoiseVariances.resize(2 * _nInputPins); | |
| 1257 | |||
| 1258 | 6 | size_t pinDataIdx = 0; | |
| 1259 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 6 times.
|
33 | for (size_t pinIndex = 0; pinIndex < _nInputPins; ++pinIndex) |
| 1260 | { | ||
| 1261 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 27 times.
|
27 | if (!_imuCharacteristicsIdentical) |
| 1262 | { | ||
| 1263 | ✗ | pinDataIdx = pinIndex; | |
| 1264 | } | ||
| 1265 | |||
| 1266 | // Measurement uncertainty for the angular rate (Variance σ²) in [(rad/s)^2, (rad/s)^2, (rad/s)^2] | ||
| 1267 |
1/5✗ Branch 1 not taken.
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
27 | switch (_pinData[pinDataIdx].measurementUncertaintyAngularRateUnit) |
| 1268 | { | ||
| 1269 | ✗ | case PinData::AngRateVarianceUnit::rad_s: | |
| 1270 | ✗ | _measurementNoiseVariances[2 * pinIndex] = (_pinData[pinDataIdx].measurementUncertaintyAngularRate).array().pow(2); | |
| 1271 | ✗ | break; | |
| 1272 | 27 | case PinData::AngRateVarianceUnit::deg_s: | |
| 1273 |
4/8✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 27 times.
✗ Branch 13 not taken.
|
27 | _measurementNoiseVariances[2 * pinIndex] = (deg2rad(_pinData[pinDataIdx].measurementUncertaintyAngularRate)).array().pow(2); |
| 1274 | 27 | break; | |
| 1275 | ✗ | case PinData::AngRateVarianceUnit::rad2_s2: | |
| 1276 | ✗ | _measurementNoiseVariances[2 * pinIndex] = _pinData[pinDataIdx].measurementUncertaintyAngularRate; | |
| 1277 | ✗ | break; | |
| 1278 | ✗ | case PinData::AngRateVarianceUnit::deg2_s2: | |
| 1279 | ✗ | _measurementNoiseVariances[2 * pinIndex] = deg2rad((_pinData[pinDataIdx].measurementUncertaintyAngularRate).cwiseSqrt()).array().pow(2); | |
| 1280 | ✗ | break; | |
| 1281 | } | ||
| 1282 | |||
| 1283 | // Measurement uncertainty for the acceleration (Variance σ²) in [(m^2)/(s^4), (m^2)/(s^4), (m^2)/(s^4)] | ||
| 1284 |
1/3✗ Branch 1 not taken.
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
27 | switch (_pinData[pinDataIdx].measurementUncertaintyAccelerationUnit) |
| 1285 | { | ||
| 1286 | ✗ | case PinData::AccelerationVarianceUnit::m2_s4: | |
| 1287 | ✗ | _measurementNoiseVariances[1 + 2 * pinIndex] = _pinData[pinDataIdx].measurementUncertaintyAcceleration; | |
| 1288 | ✗ | break; | |
| 1289 | 27 | case PinData::AccelerationVarianceUnit::m_s2: | |
| 1290 |
3/6✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 27 times.
✗ Branch 10 not taken.
|
27 | _measurementNoiseVariances[1 + 2 * pinIndex] = (_pinData[pinDataIdx].measurementUncertaintyAcceleration).array().pow(2); |
| 1291 | 27 | break; | |
| 1292 | } | ||
| 1293 | } | ||
| 1294 | |||
| 1295 |
1/2✓ Branch 0 taken 6 times.
✗ Branch 1 not taken.
|
6 | if (_imuCharacteristicsIdentical) |
| 1296 | { | ||
| 1297 | 6 | measurementNoiseMatrix_R(_kalmanFilter.R); | |
| 1298 | LOG_DATA("{}: imuCharacteristicsIdentical - kalmanFilter.R =\n{}", nameId(), _kalmanFilter.R); | ||
| 1299 | } | ||
| 1300 | |||
| 1301 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
|
6 | if (!_autoInitKF) { _kfInitialized = true; } // Auto-init initializes KF at 'avgEndTime' seconds --> see 'recvSignal' |
| 1302 | |||
| 1303 |
1/2✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | LOG_DEBUG("ImuFusion initialized"); |
| 1304 | |||
| 1305 | 6 | return true; | |
| 1306 | } | ||
| 1307 | |||
| 1308 | 2 | void NAV::ImuFusion::deinitialize() | |
| 1309 | { | ||
| 1310 | LOG_TRACE("{}: called", nameId()); | ||
| 1311 | 2 | } | |
| 1312 | |||
| 1313 | 116 | void NAV::ImuFusion::updateNumberOfInputPins() | |
| 1314 | { | ||
| 1315 |
2/2✓ Branch 1 taken 233 times.
✓ Branch 2 taken 116 times.
|
465 | while (inputPins.size() < _nInputPins) |
| 1316 | { | ||
| 1317 |
4/8✓ Branch 1 taken 233 times.
✗ Branch 2 not taken.
✓ Branch 8 taken 233 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 233 times.
✓ Branch 13 taken 233 times.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
|
932 | nm::CreateInputPin(this, fmt::format("Pin {}", inputPins.size() + 1).c_str(), Pin::Type::Flow, |
| 1318 | { NAV::ImuObs::type() }, &ImuFusion::recvSignal); | ||
| 1319 | 233 | _pinData.emplace_back(); | |
| 1320 |
2/2✓ Branch 1 taken 119 times.
✓ Branch 2 taken 114 times.
|
233 | if (outputPins.size() < _nInputPins) |
| 1321 | { | ||
| 1322 |
4/8✓ Branch 2 taken 119 times.
✗ Branch 3 not taken.
✓ Branch 9 taken 119 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 119 times.
✓ Branch 14 taken 119 times.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
|
595 | nm::CreateOutputPin(this, fmt::format("ImuBiases {}1", outputPins.size() + 1).c_str(), Pin::Type::Flow, { NAV::InsGnssLCKFSolution::type() }); |
| 1323 | } | ||
| 1324 | } | ||
| 1325 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 116 times.
|
116 | while (inputPins.size() > _nInputPins) // TODO: while loop still necessary here? guiConfig also deletes pins |
| 1326 | { | ||
| 1327 | ✗ | nm::DeleteInputPin(inputPins.back()); | |
| 1328 | ✗ | nm::DeleteOutputPin(outputPins.back()); | |
| 1329 | ✗ | _pinData.pop_back(); | |
| 1330 | } | ||
| 1331 | 116 | _pinData.resize(_nInputPins); | |
| 1332 | 116 | initializeMountingAngles(); | |
| 1333 | 468 | } | |
| 1334 | |||
| 1335 | 122 | void NAV::ImuFusion::initializeMountingAngles() | |
| 1336 | { | ||
| 1337 | 122 | _imuRotations_accel.resize(_nInputPins); | |
| 1338 | 122 | _imuRotations_gyro.resize(_nInputPins); | |
| 1339 |
2/2✓ Branch 0 taken 264 times.
✓ Branch 1 taken 122 times.
|
386 | for (size_t i = 0; i < _nInputPins; ++i) |
| 1340 | { | ||
| 1341 |
2/4✓ Branch 1 taken 264 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 264 times.
✗ Branch 6 not taken.
|
264 | _imuRotations_accel[i] = Eigen::Matrix3d::Zero(); |
| 1342 |
2/4✓ Branch 1 taken 264 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 264 times.
✗ Branch 6 not taken.
|
264 | _imuRotations_gyro[i] = Eigen::Matrix3d::Zero(); |
| 1343 | |||
| 1344 | // Assigning nan for an efficient check during runtime, whether mounting angles have been read for sensor i | ||
| 1345 | 264 | _imuRotations_accel[i](0, 0) = std::nan(""); | |
| 1346 | 264 | _imuRotations_gyro[i](0, 0) = std::nan(""); | |
| 1347 | } | ||
| 1348 | 122 | } | |
| 1349 | |||
| 1350 | 17486 | void NAV::ImuFusion::recvSignal(NAV::InputPin::NodeDataQueue& queue, size_t pinIdx) | |
| 1351 | { | ||
| 1352 |
1/2✓ Branch 1 taken 17486 times.
✗ Branch 2 not taken.
|
17486 | auto imuObs = std::static_pointer_cast<const ImuObs>(queue.extract_front()); |
| 1353 | |||
| 1354 |
2/6✗ Branch 2 not taken.
✓ Branch 3 taken 17486 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 17486 times.
|
17486 | if (imuObs->insTime.empty() && !imuObs->timeSinceStartup.has_value()) |
| 1355 | { | ||
| 1356 | ✗ | LOG_ERROR("{}: Can't set new imuObs__t0 because the observation has no time tag (insTime/timeSinceStartup)", nameId()); | |
| 1357 | ✗ | return; | |
| 1358 | } | ||
| 1359 | |||
| 1360 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 17484 times.
|
17486 | if (_latestTimestamp.empty()) |
| 1361 | { | ||
| 1362 | // Initial time step for KF prediction | ||
| 1363 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | InsTime dt_init = InsTime{ 0, 0, 0, 0, 0, 1.0 / _imuFrequency }; |
| 1364 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
2 | _latestTimestamp = InsTime{ 0, 0, 0, 0, 0, (imuObs->insTime - dt_init).count() }; |
| 1365 | 2 | _firstTimestamp = imuObs->insTime; | |
| 1366 | |||
| 1367 | // Time until averaging ends and filtering starts (for auto-init of KF) | ||
| 1368 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
|
2 | _avgEndTime = imuObs->insTime + std::chrono::milliseconds(static_cast<int>(1e3 * _averageEndTime)); |
| 1369 | } | ||
| 1370 | |||
| 1371 |
1/2✓ Branch 2 taken 17486 times.
✗ Branch 3 not taken.
|
17486 | _timeSinceStartup = static_cast<double>((imuObs->insTime - _firstTimestamp).count()); |
| 1372 | LOG_DATA("_timeSinceStartup = {}", _timeSinceStartup); | ||
| 1373 | |||
| 1374 | // Predict states over the time difference between the latest signal and the one before | ||
| 1375 |
1/2✓ Branch 2 taken 17486 times.
✗ Branch 3 not taken.
|
17486 | auto dt = static_cast<double>((imuObs->insTime - _latestTimestamp).count()); |
| 1376 | 17486 | _latestTimestamp = imuObs->insTime; | |
| 1377 | LOG_DATA("{}: dt = {}", nameId(), dt); | ||
| 1378 | |||
| 1379 |
2/2✓ Branch 0 taken 17385 times.
✓ Branch 1 taken 101 times.
|
17486 | if (_kfInitialized) |
| 1380 | { | ||
| 1381 |
2/2✓ Branch 0 taken 9903 times.
✓ Branch 1 taken 7482 times.
|
17385 | if (_imuFusionType == ImuFusionType::IRWKF) |
| 1382 | { | ||
| 1383 |
1/2✓ Branch 1 taken 9903 times.
✗ Branch 2 not taken.
|
9903 | IRWKF::stateTransitionMatrix_Phi(_kalmanFilter.Phi, dt); |
| 1384 |
1/2✓ Branch 1 taken 9903 times.
✗ Branch 2 not taken.
|
9903 | IRWKF::processNoiseMatrix_Q(_kalmanFilter.Q, dt, _processNoiseVariances, _numStates); |
| 1385 | } | ||
| 1386 | else // (_imuFusionType == ImuFusionType::Bspline) | ||
| 1387 | { | ||
| 1388 |
1/2✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
|
7482 | BsplineKF::processNoiseMatrix_Q(_kalmanFilter.Q, dt, _processNoiseVariances, _numStates); |
| 1389 | |||
| 1390 |
2/2✓ Branch 0 taken 61 times.
✓ Branch 1 taken 7421 times.
|
7482 | if (_timeSinceStartup >= _latestKnot) |
| 1391 | { | ||
| 1392 | 61 | _latestKnot += _splineSpacing; | |
| 1393 | |||
| 1394 |
1/2✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
|
61 | BsplineKF::rotateCoeffStates(_kalmanFilter.x); |
| 1395 | LOG_DATA("{}: kalmanFilter.P before B-spline coeff rotation =\n{}", nameId(), _kalmanFilter.P.block<18, 18>(0, 0)); | ||
| 1396 |
1/2✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
|
61 | BsplineKF::rotateErrorCovariances(_kalmanFilter.P, _numStates); |
| 1397 | } | ||
| 1398 | } | ||
| 1399 | |||
| 1400 | LOG_DATA("{}: kalmanFilter.P (B-spline coeffs) =\n{}", nameId(), _kalmanFilter.P.block<18, 18>(0, 0)); | ||
| 1401 | LOG_DATA("{}: kalmanFilter.Phi =\n{}", nameId(), _kalmanFilter.Phi); | ||
| 1402 | LOG_DATA("{}: kalmanFilter.Q =\n{}", nameId(), _kalmanFilter.Q); | ||
| 1403 | |||
| 1404 |
2/2✓ Branch 0 taken 7482 times.
✓ Branch 1 taken 9903 times.
|
17385 | if (_checkKalmanMatricesRanks) |
| 1405 | { | ||
| 1406 |
3/6✓ Branch 2 taken 7482 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7482 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
|
7482 | if (inputPins.at(_pinData.size() - 1).isPinLinked()) |
| 1407 | { | ||
| 1408 |
2/4✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
|
7482 | auto rank = _kalmanFilter.P.fullPivLu().rank(); |
| 1409 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 7482 times.
|
7482 | if (rank != _kalmanFilter.P.rows()) |
| 1410 | { | ||
| 1411 | ✗ | LOG_WARN("{}: P.rank = {}", nameId(), rank); | |
| 1412 | } | ||
| 1413 | } | ||
| 1414 | } | ||
| 1415 | } | ||
| 1416 | |||
| 1417 | // Read sensor rotation info from 'imuObs' | ||
| 1418 |
3/4✓ Branch 2 taken 17486 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✓ Branch 6 taken 17477 times.
|
17486 | if (std::isnan(_imuRotations_accel[pinIdx](0, 0))) |
| 1419 | { | ||
| 1420 | // Rotation matrix of the accelerometer platform to body frame | ||
| 1421 |
1/2✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
|
9 | _imuRotations_accel[pinIdx] = imuObs->imuPos.b_quatAccel_p().toRotationMatrix(); |
| 1422 | } | ||
| 1423 |
3/4✓ Branch 2 taken 17486 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✓ Branch 6 taken 17477 times.
|
17486 | if (std::isnan(_imuRotations_gyro[pinIdx](0, 0))) |
| 1424 | { | ||
| 1425 | // Rotation matrix of the gyro platform to body frame | ||
| 1426 |
1/2✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
|
9 | _imuRotations_gyro[pinIdx] = imuObs->imuPos.b_quatGyro_p().toRotationMatrix(); |
| 1427 | } | ||
| 1428 | |||
| 1429 | // Initialize H with mounting angles (DCM) of the sensor that provided the latest measurement | ||
| 1430 |
2/4✓ Branch 1 taken 17486 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17486 times.
✗ Branch 5 not taken.
|
17486 | auto DCM_accel = _imuRotations_accel.at(pinIdx); |
| 1431 | LOG_DATA("DCM_accel =\n{}", DCM_accel); | ||
| 1432 |
2/4✓ Branch 1 taken 17486 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17486 times.
✗ Branch 5 not taken.
|
17486 | auto DCM_gyro = _imuRotations_gyro.at(pinIdx); |
| 1433 | LOG_DATA("{}: DCM_gyro =\n{}", nameId(), DCM_gyro); | ||
| 1434 | |||
| 1435 |
2/2✓ Branch 0 taken 10004 times.
✓ Branch 1 taken 7482 times.
|
17486 | if (_imuFusionType == ImuFusionType::IRWKF) |
| 1436 | { | ||
| 1437 |
1/2✓ Branch 1 taken 10004 times.
✗ Branch 2 not taken.
|
10004 | _kalmanFilter.H = IRWKF::designMatrix_H(DCM_accel, DCM_gyro, pinIdx, _numMeasurements, _numStates, _numStatesEst, _numStatesPerPin); |
| 1438 | LOG_DATA("{}: Sensor (pinIdx): {}, kalmanFilter.H =\n{}", nameId(), _kalmanFilter.H, pinIdx); | ||
| 1439 | } | ||
| 1440 | else // (_imuFusionType == ImuFusionType::BsplineKF) | ||
| 1441 | { | ||
| 1442 |
1/2✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
|
7482 | _kalmanFilter.H = BsplineKF::designMatrix_H(_timeSinceStartup, _splineSpacing, DCM_accel, DCM_gyro, pinIdx, _numMeasurements, _numStates, _numStatesEst, _numStatesPerPin); |
| 1443 | LOG_DATA("{}: timeSinceStartup: {}, Sensor (pinIdx): {}, kalmanFilter.H =\n{}", nameId(), _timeSinceStartup, pinIdx, _kalmanFilter.H); | ||
| 1444 | } | ||
| 1445 | |||
| 1446 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 17486 times.
|
17486 | if (!_imuCharacteristicsIdentical) |
| 1447 | { | ||
| 1448 | ✗ | measurementNoiseMatrix_R(_kalmanFilter.R, pinIdx); | |
| 1449 | LOG_DATA("{}: kalmanFilter.R =\n{}", nameId(), _kalmanFilter.R); | ||
| 1450 | } | ||
| 1451 | |||
| 1452 |
2/2✓ Branch 0 taken 7482 times.
✓ Branch 1 taken 10004 times.
|
17486 | if (_checkKalmanMatricesRanks) |
| 1453 | { | ||
| 1454 |
6/12✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7482 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7482 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7482 times.
✗ Branch 17 not taken.
|
7482 | auto rank = (_kalmanFilter.H * _kalmanFilter.P * _kalmanFilter.H.transpose() + _kalmanFilter.R).fullPivLu().rank(); |
| 1455 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 7482 times.
|
7482 | if (rank != _kalmanFilter.H.rows()) |
| 1456 | { | ||
| 1457 | ✗ | LOG_WARN("{}: (HPH^T + R).rank = {}", nameId(), rank); | |
| 1458 | } | ||
| 1459 | } | ||
| 1460 | |||
| 1461 |
4/4✓ Branch 0 taken 10004 times.
✓ Branch 1 taken 7482 times.
✓ Branch 2 taken 101 times.
✓ Branch 3 taken 9903 times.
|
17486 | if (_autoInitKF && !_kfInitialized) |
| 1462 | { | ||
| 1463 |
2/2✓ Branch 2 taken 100 times.
✓ Branch 3 taken 1 times.
|
101 | if (imuObs->insTime < _avgEndTime) |
| 1464 | { | ||
| 1465 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | _cumulatedImuObs.push_back(imuObs); |
| 1466 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | _cumulatedPinIds.push_back(pinIdx); |
| 1467 | } | ||
| 1468 | else // if (imuObs->insTime == _avgEndTime) // <-- do auto-init, once _avgEndTime is reached | ||
| 1469 | { | ||
| 1470 | 1 | double dtInit = 1.0 / _imuFrequency; | |
| 1471 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | _kalmanFilter = IRWKF::initializeKalmanFilterAuto(_nInputPins, _pinData, _pinDataIRWKF, _cumulatedPinIds, _cumulatedImuObs, _initJerkAngAcc, dtInit, _numStates, _numMeasurements, _processNoiseVariances, _kalmanFilter); |
| 1472 | 1 | _kfInitialized = true; // Start Kalman Filter | |
| 1473 | } | ||
| 1474 | } | ||
| 1475 |
2/2✓ Branch 0 taken 17386 times.
✓ Branch 1 taken 100 times.
|
17486 | if (_kfInitialized) |
| 1476 | { | ||
| 1477 |
1/2✓ Branch 1 taken 17386 times.
✗ Branch 2 not taken.
|
17386 | combineSignals(imuObs); |
| 1478 | } | ||
| 1479 | 17486 | } | |
| 1480 | |||
| 1481 | 17386 | void NAV::ImuFusion::combineSignals(const std::shared_ptr<const ImuObs>& imuObs) | |
| 1482 | { | ||
| 1483 | LOG_DATA("{}: called", nameId()); | ||
| 1484 | |||
| 1485 |
1/2✓ Branch 1 taken 17386 times.
✗ Branch 2 not taken.
|
17386 | auto imuObsFiltered = std::make_shared<ImuObs>(this->_imuPos); |
| 1486 | |||
| 1487 | LOG_DATA("{}: Estimated state before prediction: x =\n{}", nameId(), _kalmanFilter.x); | ||
| 1488 | |||
| 1489 |
1/2✓ Branch 1 taken 17386 times.
✗ Branch 2 not taken.
|
17386 | _kalmanFilter.predict(); |
| 1490 | |||
| 1491 | LOG_DATA("{}: kalmanFilter.P (B-spline coeffs) =\n{}", nameId(), _kalmanFilter.P.block<18, 18>(0, 0)); | ||
| 1492 | |||
| 1493 | LOG_DATA("{}: kalmanFilter.P (B-spline coeffs) =\n{}", nameId(), _kalmanFilter.P.block<18, 18>(0, 0)); | ||
| 1494 | |||
| 1495 |
2/4✓ Branch 2 taken 17386 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 17386 times.
✗ Branch 6 not taken.
|
17386 | _kalmanFilter.z.block<3, 1>(0, 0) = imuObs->p_angularRate; |
| 1496 |
2/4✓ Branch 2 taken 17386 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 17386 times.
✗ Branch 6 not taken.
|
17386 | _kalmanFilter.z.block<3, 1>(3, 0) = imuObs->p_acceleration; |
| 1497 | |||
| 1498 | LOG_DATA("{}: Measurements z =\n{}", nameId(), _kalmanFilter.z); | ||
| 1499 | LOG_DATA("{}: coeff states x =\n{}", nameId(), _kalmanFilter.x.block<18, 1>(0, 0)); | ||
| 1500 | LOG_DATA("{}: Innovation: z - H * x =\n{}", nameId(), _kalmanFilter.z - _kalmanFilter.H * _kalmanFilter.x); | ||
| 1501 | |||
| 1502 |
1/2✓ Branch 1 taken 17386 times.
✗ Branch 2 not taken.
|
17386 | _kalmanFilter.correct(); |
| 1503 | LOG_DATA("{}: Estimated state after correction: x =\n{}", nameId(), _kalmanFilter.x); | ||
| 1504 | |||
| 1505 |
2/2✓ Branch 0 taken 7482 times.
✓ Branch 1 taken 9904 times.
|
17386 | if (_checkKalmanMatricesRanks) |
| 1506 | { | ||
| 1507 |
6/12✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7482 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7482 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7482 times.
✗ Branch 17 not taken.
|
7482 | auto rankH = (_kalmanFilter.H * _kalmanFilter.P * _kalmanFilter.H.transpose() + _kalmanFilter.R).fullPivLu().rank(); |
| 1508 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 7482 times.
|
7482 | if (rankH != _kalmanFilter.H.rows()) |
| 1509 | { | ||
| 1510 | ✗ | LOG_WARN("{}: (HPH^T + R).rank = {}", nameId(), rankH); | |
| 1511 | } | ||
| 1512 | |||
| 1513 |
3/6✓ Branch 2 taken 7482 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7482 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
|
7482 | if (inputPins.at(_pinData.size() - 1).isPinLinked()) |
| 1514 | { | ||
| 1515 |
2/4✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
|
7482 | auto rankP = _kalmanFilter.P.fullPivLu().rank(); |
| 1516 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 7482 times.
|
7482 | if (rankP != _kalmanFilter.P.rows()) |
| 1517 | { | ||
| 1518 | ✗ | LOG_WARN("{}: P.rank = {}", nameId(), rankP); | |
| 1519 | LOG_DATA("{}: kalmanFilter.P =\n{}", nameId(), _kalmanFilter.P); | ||
| 1520 | } | ||
| 1521 | } | ||
| 1522 | } | ||
| 1523 | |||
| 1524 | // Construct imuObs | ||
| 1525 | 17386 | imuObsFiltered->insTime = imuObs->insTime; | |
| 1526 |
2/2✓ Branch 0 taken 9904 times.
✓ Branch 1 taken 7482 times.
|
17386 | if (_imuFusionType == ImuFusionType::IRWKF) |
| 1527 | { | ||
| 1528 |
4/8✓ Branch 1 taken 9904 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9904 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9904 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9904 times.
✗ Branch 11 not taken.
|
9904 | imuObsFiltered->p_acceleration = { _kalmanFilter.x(6, 0), _kalmanFilter.x(7, 0), _kalmanFilter.x(8, 0) }; |
| 1529 |
4/8✓ Branch 1 taken 9904 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9904 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9904 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9904 times.
✗ Branch 11 not taken.
|
9904 | imuObsFiltered->p_angularRate = { _kalmanFilter.x(0, 0), _kalmanFilter.x(1, 0), _kalmanFilter.x(2, 0) }; |
| 1530 | } | ||
| 1531 | else // (_imuFusionType == ImuFusionType::BsplineKF) | ||
| 1532 | { | ||
| 1533 |
1/2✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
|
7482 | auto qBsplines = NAV::BsplineKF::quadraticBsplines(_timeSinceStartup, _splineSpacing); |
| 1534 | |||
| 1535 | LOG_DATA("{}: timeSinceStartup: {}, qBsplines (stacked B-spline values, cumulatively = 1) = {}, {}, {}", nameId(), _timeSinceStartup, qBsplines.at(0), qBsplines.at(1), qBsplines.at(2)); | ||
| 1536 | LOG_DATA("{}: timeSinceStartup: {}, Angular rate B-spline coefficient estimates:\n{}", nameId(), _timeSinceStartup, _kalmanFilter.x.block<9, 1>(0, 0)); | ||
| 1537 | LOG_DATA("{}: timeSinceStartup: {}, Acceleration B-spline coefficient estimates:\n{}", nameId(), _timeSinceStartup, _kalmanFilter.x.block<9, 1>(9, 0)); | ||
| 1538 | |||
| 1539 | // Estimated angular rate: Cumulative sum of the three estimated B-spline coefficients for the angular rate | ||
| 1540 |
3/6✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
|
7482 | auto angRateEst = _kalmanFilter.x.block<3, 1>(0, 0) * qBsplines.at(0) |
| 1541 |
4/8✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7482 times.
✗ Branch 11 not taken.
|
14964 | + _kalmanFilter.x.block<3, 1>(3, 0) * qBsplines.at(1) |
| 1542 |
4/8✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7482 times.
✗ Branch 11 not taken.
|
14964 | + _kalmanFilter.x.block<3, 1>(6, 0) * qBsplines.at(2); |
| 1543 | |||
| 1544 | // Estimated acceleration: Cumulative sum of the three estimated B-spline coefficients for the acceleration | ||
| 1545 |
3/6✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
|
7482 | auto accelEst = _kalmanFilter.x.block<3, 1>(9, 0) * qBsplines.at(0) |
| 1546 |
4/8✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7482 times.
✗ Branch 11 not taken.
|
14964 | + _kalmanFilter.x.block<3, 1>(12, 0) * qBsplines.at(1) |
| 1547 |
4/8✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7482 times.
✗ Branch 11 not taken.
|
14964 | + _kalmanFilter.x.block<3, 1>(15, 0) * qBsplines.at(2); |
| 1548 | |||
| 1549 | LOG_DATA("{}: imuObs->insTime = {}, timeSinceStartup = {}, angRateEst = {}, accelEst = {}", nameId(), imuObs->insTime.toYMDHMS(), _timeSinceStartup, angRateEst.transpose(), accelEst.transpose()); | ||
| 1550 | |||
| 1551 |
1/2✓ Branch 2 taken 7482 times.
✗ Branch 3 not taken.
|
7482 | imuObsFiltered->p_acceleration = accelEst; |
| 1552 |
1/2✓ Branch 2 taken 7482 times.
✗ Branch 3 not taken.
|
7482 | imuObsFiltered->p_angularRate = angRateEst; |
| 1553 | } | ||
| 1554 | |||
| 1555 | // Detect jumps back in time | ||
| 1556 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 17386 times.
|
17386 | if (imuObsFiltered->insTime < _lastFiltObs) |
| 1557 | { | ||
| 1558 | ✗ | LOG_ERROR("{}: imuObsFiltered->insTime < _lastFiltObs --> {}", nameId(), static_cast<double>((imuObsFiltered->insTime - _lastFiltObs).count())); | |
| 1559 | } | ||
| 1560 | 17386 | _lastFiltObs = imuObsFiltered->insTime; | |
| 1561 | |||
| 1562 |
1/2✓ Branch 2 taken 17386 times.
✗ Branch 3 not taken.
|
17386 | invokeCallbacks(OUTPUT_PORT_INDEX_COMBINED_SIGNAL, imuObsFiltered); |
| 1563 | |||
| 1564 |
2/2✓ Branch 0 taken 59640 times.
✓ Branch 1 taken 17386 times.
|
77026 | for (size_t OUTPUT_PORT_INDEX_BIAS = 1; OUTPUT_PORT_INDEX_BIAS < _nInputPins; ++OUTPUT_PORT_INDEX_BIAS) |
| 1565 | { | ||
| 1566 |
1/2✓ Branch 1 taken 59640 times.
✗ Branch 2 not taken.
|
59640 | auto imuRelativeBiases = std::make_shared<InsGnssLCKFSolution>(); |
| 1567 | 59640 | imuRelativeBiases->insTime = imuObs->insTime; | |
| 1568 | 59640 | auto biasIndex = _numStatesEst + static_cast<uint8_t>((OUTPUT_PORT_INDEX_BIAS - 1) * _numStatesPerPin); | |
| 1569 | LOG_DATA("{}: biasIndex = {}", nameId(), biasIndex); | ||
| 1570 | |||
| 1571 |
4/8✓ Branch 1 taken 59640 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59640 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59640 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 59640 times.
✗ Branch 11 not taken.
|
59640 | imuRelativeBiases->b_biasGyro = { _kalmanFilter.x(biasIndex, 0), _kalmanFilter.x(biasIndex + 1, 0), _kalmanFilter.x(biasIndex + 2, 0) }; |
| 1572 |
4/8✓ Branch 1 taken 59640 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59640 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59640 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 59640 times.
✗ Branch 11 not taken.
|
59640 | imuRelativeBiases->b_biasAccel = { _kalmanFilter.x(biasIndex + 3, 0), _kalmanFilter.x(biasIndex + 4, 0), _kalmanFilter.x(biasIndex + 5, 0) }; |
| 1573 | |||
| 1574 | LOG_DATA("{}: timeSinceStartup = {}, Relative bias {}1 Gyro: {}", nameId(), _timeSinceStartup, OUTPUT_PORT_INDEX_BIAS + 1, imuRelativeBiases->b_biasGyro.transpose()); | ||
| 1575 | LOG_DATA("{}: timeSinceStartup = {}, Relative bias {}1 Accel: {}", nameId(), _timeSinceStartup, OUTPUT_PORT_INDEX_BIAS + 1, imuRelativeBiases->b_biasAccel.transpose()); | ||
| 1576 | |||
| 1577 |
1/2✓ Branch 2 taken 59640 times.
✗ Branch 3 not taken.
|
59640 | invokeCallbacks(OUTPUT_PORT_INDEX_BIAS, imuRelativeBiases); |
| 1578 | 59640 | } | |
| 1579 | 17386 | } | |
| 1580 | |||
| 1581 | // -------------------------------------- Measurement noise matrix R ----------------------------------------- | ||
| 1582 | |||
| 1583 | ✗ | Eigen::MatrixXd NAV::ImuFusion::measurementNoiseMatrix_R_adaptive(double alpha, const Eigen::MatrixXd& R, const Eigen::VectorXd& e, const Eigen::MatrixXd& H, const Eigen::MatrixXd& P) | |
| 1584 | { | ||
| 1585 | ✗ | return alpha * R + (1.0 - alpha) * (e * e.transpose() + H * P * H.transpose()); | |
| 1586 | } | ||
| 1587 | |||
| 1588 | 6 | void NAV::ImuFusion::measurementNoiseMatrix_R(Eigen::MatrixXd& R, size_t pinIndex) const | |
| 1589 | { | ||
| 1590 |
3/6✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
|
6 | R.block<3, 3>(0, 0).diagonal() = _measurementNoiseVariances.at(2 * pinIndex); |
| 1591 |
3/6✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
|
6 | R.block<3, 3>(3, 3).diagonal() = _measurementNoiseVariances.at(2 * pinIndex + 1); |
| 1592 | 6 | } | |
| 1593 |