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