| 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 | /// @file InertialIntegrator.hpp | ||
| 10 | /// @brief Inertial Measurement Integrator | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2023-12-09 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <functional> | ||
| 17 | #include <optional> | ||
| 18 | #include <memory> | ||
| 19 | |||
| 20 | #include "NodeData/IMU/ImuPos.hpp" | ||
| 21 | #include "NodeData/State/PosVelAtt.hpp" | ||
| 22 | |||
| 23 | #include "Navigation/Gravity/Gravity.hpp" | ||
| 24 | #include "Navigation/Math/NumericalIntegration.hpp" | ||
| 25 | #include "Navigation/Time/InsTime.hpp" | ||
| 26 | #include "Navigation/INS/Mechanization.hpp" | ||
| 27 | #include "Navigation/INS/LocalNavFrame/Mechanization.hpp" | ||
| 28 | #include "Navigation/INS/EcefFrame/Mechanization.hpp" | ||
| 29 | |||
| 30 | #include "util/Container/ScrollingBuffer.hpp" | ||
| 31 | #include "util/Eigen.hpp" | ||
| 32 | #include "util/Json.hpp" | ||
| 33 | #include "util/Logger.hpp" | ||
| 34 | |||
| 35 | namespace NAV | ||
| 36 | { | ||
| 37 | |||
| 38 | /// @brief Inertial Measurement Integrator | ||
| 39 | class InertialIntegrator | ||
| 40 | { | ||
| 41 | public: | ||
| 42 | /// Available Integration Algorithms | ||
| 43 | enum class IntegrationAlgorithm : uint8_t | ||
| 44 | { | ||
| 45 | SingleStepRungeKutta1, ///< Runge-Kutta 1st order (explicit) / (Forward) Euler method | ||
| 46 | SingleStepRungeKutta2, ///< Runge-Kutta 2nd order (explicit) / Explicit midpoint method | ||
| 47 | SingleStepHeun2, ///< Heun's method (2nd order) (explicit) | ||
| 48 | SingleStepRungeKutta3, ///< Runge-Kutta 3rd order (explicit) / Simpson's rule | ||
| 49 | SingleStepHeun3, ///< Heun's method (3nd order) (explicit) | ||
| 50 | SingleStepRungeKutta4, ///< Runge-Kutta 4th order (explicit) | ||
| 51 | MultiStepRK3, ///< Multistep Runge-Kutta 3rd order (explicit) / Simpson's rule (taking 2 old epochs into account) | ||
| 52 | MultiStepRK4, ///< Multistep Runge-Kutta 4th order (explicit) (taking 2 old epochs into account) | ||
| 53 | COUNT, ///< Amount of available integration algorithms | ||
| 54 | }; | ||
| 55 | |||
| 56 | /// Available Integration Frames | ||
| 57 | enum class IntegrationFrame : uint8_t | ||
| 58 | { | ||
| 59 | ECEF, ///< Earth-Centered Earth-Fixed frame | ||
| 60 | NED, ///< Local North-East-Down frame | ||
| 61 | }; | ||
| 62 | |||
| 63 | /// Inertial Measurement | ||
| 64 | struct Measurement | ||
| 65 | { | ||
| 66 | bool averagedMeasurement = false; ///< Wether the acceleration is averaged over the last epoch | ||
| 67 | double dt = 0.0; ///< Time since previous observation in [s] | ||
| 68 | Eigen::Vector3d p_acceleration; ///< Acceleration in platform frame coordinates in [m/s^2] | ||
| 69 | Eigen::Vector3d p_angularRate; ///< Angular rate in platform frame coordinates in [rad/s] | ||
| 70 | }; | ||
| 71 | |||
| 72 | /// @brief Inertial state and measurements | ||
| 73 | template<typename T> | ||
| 74 | struct GenericState | ||
| 75 | { | ||
| 76 | InsTime epoch; ///< Epoch of this state | ||
| 77 | |||
| 78 | Eigen::Vector3<T> position; ///< IMU position (e_pos / lla_pos) | ||
| 79 | Eigen::Vector3<T> velocity; ///< IMU velocity (e_vel / n_vel) | ||
| 80 | Eigen::Quaternion<T> attitude; ///< IMU attitude (e_Quat_b / n_Quat_b) | ||
| 81 | |||
| 82 | Measurement m; ///< Inertial measurement | ||
| 83 | |||
| 84 | Eigen::Vector3<T> p_biasAcceleration = Eigen::Vector3<T>::Zero(); ///< Acceleration bias in platform frame coordinates in [m/s^2] | ||
| 85 | Eigen::Vector3<T> p_biasAngularRate = Eigen::Vector3<T>::Zero(); ///< Angular rate bias in platform frame coordinates in [rad/s] | ||
| 86 | |||
| 87 | Eigen::Vector3<T> scaleFactorAccel = Eigen::Vector3<T>::Ones(); ///< Scale factor of the accelerometer [-] | ||
| 88 | Eigen::Vector3<T> scaleFactorGyro = Eigen::Vector3<T>::Ones(); ///< Scale factor of the gyroscope [-] | ||
| 89 | |||
| 90 | Eigen::Quaternion<T> misalignmentAccel = Eigen::Quaternion<T>::Identity(); ///< Misalignment of the accelerometer sensor axes | ||
| 91 | Eigen::Quaternion<T> misalignmentGyro = Eigen::Quaternion<T>::Identity(); ///< Misalignment of the gyroscope sensor axes | ||
| 92 | }; | ||
| 93 | |||
| 94 | /// Inertial state and measurements | ||
| 95 | using State = GenericState<double>; | ||
| 96 | |||
| 97 | /// @brief Default Constructor | ||
| 98 |
4/8✓ Branch 2 taken 242 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 242 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 242 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 242 times.
✗ Branch 12 not taken.
|
242 | InertialIntegrator() = default; |
| 99 | |||
| 100 | /// @brief Constructor | ||
| 101 | /// @param integrationFrame Integration frame to lock to | ||
| 102 | explicit InertialIntegrator(IntegrationFrame integrationFrame); | ||
| 103 | |||
| 104 | /// @brief Clears all internal data | ||
| 105 | void reset(); | ||
| 106 | |||
| 107 | /// @brief Checks if an initial position is set | ||
| 108 | [[nodiscard]] bool hasInitialPosition() const; | ||
| 109 | |||
| 110 | /// @brief Sets the initial state | ||
| 111 | /// @param[in] state State to set | ||
| 112 | /// @param[in] nameId NameId of the calling node for logging | ||
| 113 | void setInitialState(const PosVelAtt& state, const char* nameId); | ||
| 114 | |||
| 115 | /// @brief Pushes the state to the list of states | ||
| 116 | /// @param[in] state State to set | ||
| 117 | /// @param[in] nameId NameId of the calling node for logging | ||
| 118 | void addState(const PosVelAtt& state, const char* nameId); | ||
| 119 | |||
| 120 | /// @brief Sets the sensor biases total values | ||
| 121 | /// @param[in] p_biasAcceleration Acceleration bias in platform frame coordinates in [m/s^2] | ||
| 122 | /// @param[in] p_biasAngularRate Angular rate bias in platform frame coordinates in [rad/s] | ||
| 123 | void setTotalSensorBiases(const Eigen::Vector3d& p_biasAcceleration, const Eigen::Vector3d& p_biasAngularRate); | ||
| 124 | |||
| 125 | /// @brief Sets the sensor biases increment | ||
| 126 | /// @param[in] p_deltaBiasAcceleration Acceleration bias increment in platform frame coordinates in [m/s^2] | ||
| 127 | /// @param[in] p_deltaBiasAngularRate Angular rate bias increment in platform frame coordinates in [rad/s] | ||
| 128 | void applySensorBiasesIncrements(const Eigen::Vector3d& p_deltaBiasAcceleration, const Eigen::Vector3d& p_deltaBiasAngularRate); | ||
| 129 | |||
| 130 | /// @brief Apply the errors to the latest state | ||
| 131 | /// @param[in] lla_positionError δ𝐩_n = [δ𝜙 δλ δ𝘩] The position error (latitude, longitude, altitude) in [rad, rad, m] | ||
| 132 | /// @param[in] n_velocityError δ𝐯_n The velocity error in n frame coordinates in [m/s] | ||
| 133 | /// @param[in] n_attitudeError_b δ𝛙_nb_n The attitude error in n frame coordinates in [rad] | ||
| 134 | void applyStateErrors_n(const Eigen::Vector3d& lla_positionError, const Eigen::Vector3d& n_velocityError, const Eigen::Vector3d& n_attitudeError_b); | ||
| 135 | |||
| 136 | /// @brief Apply the errors to the latest state | ||
| 137 | /// @param[in] e_positionError δr_e The position error in e frame coordinates in [m] | ||
| 138 | /// @param[in] e_velocityError δ𝐯_e The velocity error in e frame coordinates in [m/s] | ||
| 139 | /// @param[in] e_attitudeError_b δ𝛙_eb_e The attitude error in e frame coordinates in [rad] | ||
| 140 | void applyStateErrors_e(const Eigen::Vector3d& e_positionError, const Eigen::Vector3d& e_velocityError, const Eigen::Vector3d& e_attitudeError_b); | ||
| 141 | |||
| 142 | /// Get the latest state if it exists | ||
| 143 | [[nodiscard]] std::optional<std::reference_wrapper<const State>> getLatestState() const; | ||
| 144 | |||
| 145 | /// @brief Return the last acceleration bias in platform frame coordinates in [m/s^2] | ||
| 146 | [[nodiscard]] const Eigen::Vector3d& p_getLastAccelerationBias() const; | ||
| 147 | |||
| 148 | /// @brief Return the last angular rate bias in platform frame coordinates in [rad/s] | ||
| 149 | [[nodiscard]] const Eigen::Vector3d& p_getLastAngularRateBias() const; | ||
| 150 | |||
| 151 | /// @brief Returns the selected integration frame | ||
| 152 | [[nodiscard]] IntegrationFrame getIntegrationFrame() const; | ||
| 153 | |||
| 154 | /// @brief Returns the selected compensation models | ||
| 155 | [[nodiscard]] const PosVelAttDerivativeConstants& getConstants() const; | ||
| 156 | |||
| 157 | /// Wether the measurements are accumulated values over the last epoch. (always true when using delta measurements, so GUI has no effect) | ||
| 158 | [[nodiscard]] bool areAccelerationsAveragedMeasurements() const; | ||
| 159 | |||
| 160 | /// Calculate the current acceleration, if measurements area available | ||
| 161 | [[nodiscard]] std::optional<Eigen::Vector3d> p_calcCurrentAcceleration() const; | ||
| 162 | |||
| 163 | /// Calculate the current angular rate, if measurements area available | ||
| 164 | [[nodiscard]] std::optional<Eigen::Vector3d> p_calcCurrentAngularRate() const; | ||
| 165 | |||
| 166 | /// @brief Calculates the inertial navigation solution | ||
| 167 | /// @param[in] obsTime Time of the observation | ||
| 168 | /// @param[in] p_acceleration Acceleration in platform frame coordinates in [m/s^2] | ||
| 169 | /// @param[in] p_angularRate Angular rate in platform frame coordinates in [rad/s] | ||
| 170 | /// @param[in] imuPos IMU platform frame position with respect to body frame | ||
| 171 | /// @param[in] nameId NameId of the calling node for logging | ||
| 172 | /// @return The new state at the observation time | ||
| 173 | std::shared_ptr<PosVelAtt> calcInertialSolution(const InsTime& obsTime, | ||
| 174 | const Eigen::Vector3d& p_acceleration, const Eigen::Vector3d& p_angularRate, | ||
| 175 | const ImuPos& imuPos, const char* nameId); | ||
| 176 | |||
| 177 | /// @brief Calculates the inertial navigation solution | ||
| 178 | /// @param[in] obsTime Time of the observation | ||
| 179 | /// @param[in] deltaTime Delta time over which the deltaVelocity and deltaTheta were measured in [s] | ||
| 180 | /// @param[in] p_deltaVelocity Integrated acceleration in platform frame coordinates in [m/s] | ||
| 181 | /// @param[in] p_deltaTheta Integrated angular rate in platform frame coordinates in [rad] | ||
| 182 | /// @param[in] imuPos IMU platform frame position with respect to body frame | ||
| 183 | /// @param[in] nameId NameId of the calling node for logging | ||
| 184 | /// @return The new state at the observation time | ||
| 185 | std::shared_ptr<PosVelAtt> calcInertialSolutionDelta(const InsTime& obsTime, double deltaTime, | ||
| 186 | const Eigen::Vector3d& p_deltaVelocity, const Eigen::Vector3d& p_deltaTheta, | ||
| 187 | const ImuPos& imuPos, const char* nameId); | ||
| 188 | |||
| 189 | // Forward declaring external function | ||
| 190 | friend const char* to_string(InertialIntegrator::IntegrationAlgorithm algorithm); | ||
| 191 | friend const char* to_string(InertialIntegrator::IntegrationFrame frame); | ||
| 192 | |||
| 193 | /// @brief Calculates the inertial solution going from state__t1 to state__t0 given that measurements are available for both states | ||
| 194 | /// @param imuPos IMU mounting position connecting the platform to the body frame | ||
| 195 | /// @param state__t0 State at the epoch to calculate (measurements only) | ||
| 196 | /// @param state__t1 State at the previous epoch (state + measurements) | ||
| 197 | /// @param nameId NameId of the calling node for logging | ||
| 198 | /// @return Position, velocity and attitude from the integration step | ||
| 199 | template<typename T> | ||
| 200 | 49997 | [[nodiscard]] Eigen::Vector<T, 10> calcInertialSolution(const ImuPos& imuPos, | |
| 201 | const GenericState<T>& state__t0, | ||
| 202 | const GenericState<T>& state__t1, | ||
| 203 | [[maybe_unused]] const char* nameId) const | ||
| 204 | { | ||
| 205 | // #if LOG_LEVEL <= LOG_LEVEL_DATA | ||
| 206 | // auto printState = [](const GenericState<T>& state, const char* t, const char* nameId) { | ||
| 207 | // LOG_DATA("{} [{}]:\n" | ||
| 208 | // " - {}:\n" | ||
| 209 | // " Position [{}, {}, {}], Velocity [{}, {}, {}], Attitude [{}x, {}y, {}z, {}w]\n" | ||
| 210 | // " dt = {:.5f}, p_accel [{}, {}, {}], p_angRate [{}, {}, {}]\n" | ||
| 211 | // " p_biasAccel [{}, {}, {}], p_biasAngRate [{}, {}, {}]\n" | ||
| 212 | // " p_scaleFacAccel [{}, {}, {}], p_scaleFacAngRate [{}, {}, {}]\n" | ||
| 213 | // " p_misAlignAccel [{}x, {}y, {}z, {}w], p_misAlignAngRate [{}x, {}y, {}z, {}w]", | ||
| 214 | // nameId, t, state.epoch.toYMDHMS(GPST), | ||
| 215 | // state.position(0), state.position(1), state.position(2), | ||
| 216 | // state.velocity(0), state.velocity(1), state.velocity(2), | ||
| 217 | // state.attitude.x(), state.attitude.y(), state.attitude.z(), state.attitude.w(), | ||
| 218 | // state.m.dt, state.m.p_acceleration(0), state.m.p_acceleration(1), state.m.p_acceleration(2), | ||
| 219 | // state.m.p_angularRate(0), state.m.p_angularRate(1), state.m.p_angularRate(2), | ||
| 220 | // state.p_biasAcceleration(0), state.p_biasAcceleration(1), state.p_biasAcceleration(2), | ||
| 221 | // state.p_biasAngularRate(0), state.p_biasAngularRate(1), state.p_biasAngularRate(2), | ||
| 222 | // state.scaleFactorAccel(0), state.scaleFactorAccel(1), state.scaleFactorAccel(2), | ||
| 223 | // state.scaleFactorGyro(0), state.scaleFactorGyro(1), state.scaleFactorGyro(2), | ||
| 224 | // state.misalignmentAccel.x(), state.misalignmentAccel.y(), state.misalignmentAccel.z(), state.misalignmentAccel.w(), | ||
| 225 | // state.misalignmentGyro.x(), state.misalignmentGyro.y(), state.misalignmentGyro.z(), state.misalignmentGyro.w()); | ||
| 226 | // }; | ||
| 227 | // LOG_DATA("{}: Frame {}, Algorithm {}", nameId, to_string(_integrationFrame), to_string(_integrationAlgorithm)); | ||
| 228 | // printState(state__t0, "t0", nameId); | ||
| 229 | // printState(state__t1, "t1", nameId); | ||
| 230 | // #endif | ||
| 231 | |||
| 232 |
1/2✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
|
49997 | Eigen::Vector<T, 10> y; |
| 233 | // 0 1 2 3 4 5 6 7 8 9 | ||
| 234 | // NED [ 𝜙, λ, h, v_N, v_E, v_D, n_q_bx, n_q_by, n_q_bz, n_q_bw]^T | ||
| 235 | // ECEF [ x, y, z, v_x, v_y, v_z, e_q_bx, e_q_by, e_q_bz, e_q_bw]^T | ||
| 236 |
2/4✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
|
49997 | y.template segment<3>(0) = state__t1.position; |
| 237 |
2/4✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
|
49997 | y.template segment<3>(3) = state__t1.velocity; |
| 238 |
2/4✓ Branch 2 taken 49997 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 49997 times.
✗ Branch 6 not taken.
|
49997 | y.template segment<4>(6) = state__t1.attitude.coeffs(); |
| 239 | |||
| 240 | 99994 | auto p_accel = [](const GenericState<T>& state) -> Eigen::Vector3<T> { | |
| 241 |
4/8✓ Branch 2 taken 99994 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 99994 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 99994 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 99994 times.
✗ Branch 12 not taken.
|
99994 | return (state.misalignmentAccel * state.m.p_acceleration.template cast<T>()).cwiseProduct(state.scaleFactorAccel) + state.p_biasAcceleration; |
| 242 | }; | ||
| 243 | 99994 | auto p_gyro = [](const GenericState<T>& state) -> Eigen::Vector3<T> { | |
| 244 |
4/8✓ Branch 2 taken 99994 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 99994 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 99994 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 99994 times.
✗ Branch 12 not taken.
|
99994 | return (state.misalignmentGyro * state.m.p_angularRate.template cast<T>()).cwiseProduct(state.scaleFactorGyro) + state.p_biasAngularRate; |
| 245 | }; | ||
| 246 | |||
| 247 | // LOG_DATA("{}: imuPos.b_quat_p() [{}]", nameId, imuPos.b_quat_p()); | ||
| 248 |
2/4✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 49997 times.
✗ Branch 7 not taken.
|
49997 | Eigen::Vector3<T> b_accel__t0 = imuPos.b_quat_p().cast<T>() * p_accel(state__t0); |
| 249 |
2/4✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 49997 times.
✗ Branch 7 not taken.
|
49997 | Eigen::Vector3<T> b_accel__t1 = imuPos.b_quat_p().cast<T>() * p_accel(state__t1); |
| 250 |
2/4✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 49997 times.
✗ Branch 7 not taken.
|
49997 | Eigen::Vector3<T> b_gyro__t0 = imuPos.b_quat_p().cast<T>() * p_gyro(state__t0); |
| 251 |
2/4✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 49997 times.
✗ Branch 7 not taken.
|
49997 | Eigen::Vector3<T> b_gyro__t1 = imuPos.b_quat_p().cast<T>() * p_gyro(state__t1); |
| 252 | // LOG_DATA("{}: b_accel__t0 [{}, {}, {}], b_accel__t1 [{}, {}, {}]", nameId, b_accel__t0(0), b_accel__t0(1), b_accel__t0(2), b_accel__t1(0), b_accel__t1(1), b_accel__t1(2)); | ||
| 253 | // LOG_DATA("{}: b_gyro__t0 [{}, {}, {}], b_gyro__t1 [{}, {}, {}]", nameId, b_gyro__t0(0), b_gyro__t0(1), b_gyro__t0(2), b_gyro__t1(0), b_gyro__t1(1), b_gyro__t1(2)); | ||
| 254 | |||
| 255 | // LOG_DATA("{}: integrationAlgorithm = {}", nameId, to_string(_integrationAlgorithm)); | ||
| 256 |
1/10✗ Branch 0 not taken.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 49997 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
49997 | switch (_integrationAlgorithm) |
| 257 | { | ||
| 258 | ✗ | case IntegrationAlgorithm::SingleStepRungeKutta1: | |
| 259 | { | ||
| 260 | ✗ | std::array<Eigen::Vector<T, 6>, 1> z; | |
| 261 | ✗ | if (state__t0.m.averagedMeasurement) { z[0] << b_accel__t0, b_gyro__t0; } | |
| 262 | ✗ | else { z[0] << b_accel__t1, b_gyro__t1; } | |
| 263 | // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5)); | ||
| 264 | |||
| 265 | ✗ | switch (_integrationFrame) | |
| 266 | { | ||
| 267 | ✗ | case IntegrationFrame::NED: | |
| 268 | ✗ | y = RungeKutta1(y, z, state__t0.m.dt, n_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants); | |
| 269 | ✗ | break; | |
| 270 | ✗ | case IntegrationFrame::ECEF: | |
| 271 | ✗ | y = RungeKutta1(y, z, state__t0.m.dt, e_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants); | |
| 272 | ✗ | break; | |
| 273 | } | ||
| 274 | ✗ | break; | |
| 275 | } | ||
| 276 | ✗ | case IntegrationAlgorithm::SingleStepRungeKutta2: | |
| 277 | { | ||
| 278 | ✗ | std::array<Eigen::Vector<T, 6>, 2> z; | |
| 279 | ✗ | if (state__t0.m.averagedMeasurement) | |
| 280 | { | ||
| 281 | ✗ | z[0] << b_accel__t0, b_gyro__t0; | |
| 282 | ✗ | z[1] << b_accel__t0, b_gyro__t0; | |
| 283 | } | ||
| 284 | else | ||
| 285 | { | ||
| 286 | ✗ | z[0] << b_accel__t1, b_gyro__t1; | |
| 287 | ✗ | z[1] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 0.5); | |
| 288 | } | ||
| 289 | // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5)); | ||
| 290 | // LOG_DATA("{}: z[1] = {}, {}, {}, {}, {}, {}", nameId, z[1](0), z[1](1), z[1](2), z[1](3), z[1](4), z[1](5)); | ||
| 291 | ✗ | switch (_integrationFrame) | |
| 292 | { | ||
| 293 | ✗ | case IntegrationFrame::NED: | |
| 294 | ✗ | y = RungeKutta2(y, z, state__t0.m.dt, n_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants); | |
| 295 | ✗ | break; | |
| 296 | ✗ | case IntegrationFrame::ECEF: | |
| 297 | ✗ | y = RungeKutta2(y, z, state__t0.m.dt, e_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants); | |
| 298 | ✗ | break; | |
| 299 | } | ||
| 300 | ✗ | break; | |
| 301 | } | ||
| 302 | ✗ | case IntegrationAlgorithm::SingleStepHeun2: | |
| 303 | { | ||
| 304 | ✗ | std::array<Eigen::Vector<T, 6>, 2> z; | |
| 305 | ✗ | if (state__t0.m.averagedMeasurement) | |
| 306 | { | ||
| 307 | ✗ | z[0] << b_accel__t0, b_gyro__t0; | |
| 308 | ✗ | z[1] << b_accel__t0, b_gyro__t0; | |
| 309 | } | ||
| 310 | else | ||
| 311 | { | ||
| 312 | ✗ | z[0] << b_accel__t1, b_gyro__t1; | |
| 313 | ✗ | z[1] << b_accel__t0, b_gyro__t0; | |
| 314 | } | ||
| 315 | // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5)); | ||
| 316 | // LOG_DATA("{}: z[1] = {}, {}, {}, {}, {}, {}", nameId, z[1](0), z[1](1), z[1](2), z[1](3), z[1](4), z[1](5)); | ||
| 317 | ✗ | switch (_integrationFrame) | |
| 318 | { | ||
| 319 | ✗ | case IntegrationFrame::NED: | |
| 320 | ✗ | y = Heun2(y, z, state__t0.m.dt, n_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants); | |
| 321 | ✗ | break; | |
| 322 | ✗ | case IntegrationFrame::ECEF: | |
| 323 | ✗ | y = Heun2(y, z, state__t0.m.dt, e_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants); | |
| 324 | ✗ | break; | |
| 325 | } | ||
| 326 | ✗ | break; | |
| 327 | } | ||
| 328 | 49997 | case IntegrationAlgorithm::SingleStepRungeKutta3: | |
| 329 | { | ||
| 330 |
1/2✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
|
49997 | std::array<Eigen::Vector<T, 6>, 3> z; |
| 331 |
1/2✓ Branch 0 taken 49997 times.
✗ Branch 1 not taken.
|
49997 | if (state__t0.m.averagedMeasurement) |
| 332 | { | ||
| 333 |
2/4✓ Branch 2 taken 49997 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 49997 times.
✗ Branch 6 not taken.
|
49997 | z[0] << b_accel__t0, b_gyro__t0; |
| 334 |
2/4✓ Branch 2 taken 49997 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 49997 times.
✗ Branch 6 not taken.
|
49997 | z[1] << b_accel__t0, b_gyro__t0; |
| 335 |
2/4✓ Branch 2 taken 49997 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 49997 times.
✗ Branch 6 not taken.
|
49997 | z[2] << b_accel__t0, b_gyro__t0; |
| 336 | } | ||
| 337 | else | ||
| 338 | { | ||
| 339 | ✗ | z[0] << b_accel__t1, b_gyro__t1; | |
| 340 | ✗ | z[1] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 0.5); | |
| 341 | ✗ | z[2] << b_accel__t0, b_gyro__t0; | |
| 342 | } | ||
| 343 | // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5)); | ||
| 344 | // LOG_DATA("{}: z[1] = {}, {}, {}, {}, {}, {}", nameId, z[1](0), z[1](1), z[1](2), z[1](3), z[1](4), z[1](5)); | ||
| 345 | // LOG_DATA("{}: z[2] = {}, {}, {}, {}, {}, {}", nameId, z[2](0), z[2](1), z[2](2), z[2](3), z[2](4), z[2](5)); | ||
| 346 |
2/3✓ Branch 0 taken 37109 times.
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
|
49997 | switch (_integrationFrame) |
| 347 | { | ||
| 348 | 37109 | case IntegrationFrame::NED: | |
| 349 |
1/2✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
|
37109 | y = RungeKutta3(y, z, state__t0.m.dt, n_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants); |
| 350 | 37109 | break; | |
| 351 | 12888 | case IntegrationFrame::ECEF: | |
| 352 |
1/2✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
|
12888 | y = RungeKutta3(y, z, state__t0.m.dt, e_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants); |
| 353 | 12888 | break; | |
| 354 | } | ||
| 355 | 49997 | break; | |
| 356 | } | ||
| 357 | ✗ | case IntegrationAlgorithm::SingleStepHeun3: | |
| 358 | { | ||
| 359 | ✗ | std::array<Eigen::Vector<T, 6>, 3> z; | |
| 360 | ✗ | if (state__t0.m.averagedMeasurement) | |
| 361 | { | ||
| 362 | ✗ | z[0] << b_accel__t0, b_gyro__t0; | |
| 363 | ✗ | z[1] << b_accel__t0, b_gyro__t0; | |
| 364 | ✗ | z[2] << b_accel__t0, b_gyro__t0; | |
| 365 | } | ||
| 366 | else | ||
| 367 | { | ||
| 368 | ✗ | z[0] << b_accel__t1, b_gyro__t1; | |
| 369 | ✗ | z[1] << math::lerp(b_accel__t1, b_accel__t0, 1.0 / 3.0), math::lerp(b_gyro__t1, b_gyro__t0, 1.0 / 3.0); | |
| 370 | ✗ | z[2] << math::lerp(b_accel__t1, b_accel__t0, 2.0 / 3.0), math::lerp(b_gyro__t1, b_gyro__t0, 2.0 / 3.0); | |
| 371 | } | ||
| 372 | // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5)); | ||
| 373 | // LOG_DATA("{}: z[1] = {}, {}, {}, {}, {}, {}", nameId, z[1](0), z[1](1), z[1](2), z[1](3), z[1](4), z[1](5)); | ||
| 374 | // LOG_DATA("{}: z[2] = {}, {}, {}, {}, {}, {}", nameId, z[2](0), z[2](1), z[2](2), z[2](3), z[2](4), z[2](5)); | ||
| 375 | ✗ | switch (_integrationFrame) | |
| 376 | { | ||
| 377 | ✗ | case IntegrationFrame::NED: | |
| 378 | ✗ | y = RungeKutta3(y, z, state__t0.m.dt, n_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants); | |
| 379 | ✗ | break; | |
| 380 | ✗ | case IntegrationFrame::ECEF: | |
| 381 | ✗ | y = RungeKutta3(y, z, state__t0.m.dt, e_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants); | |
| 382 | ✗ | break; | |
| 383 | } | ||
| 384 | ✗ | break; | |
| 385 | } | ||
| 386 | ✗ | case IntegrationAlgorithm::SingleStepRungeKutta4: | |
| 387 | { | ||
| 388 | ✗ | std::array<Eigen::Vector<T, 6>, 4> z; | |
| 389 | ✗ | if (state__t0.m.averagedMeasurement) | |
| 390 | { | ||
| 391 | ✗ | z[0] << b_accel__t0, b_gyro__t0; | |
| 392 | ✗ | z[1] << b_accel__t0, b_gyro__t0; | |
| 393 | ✗ | z[2] << b_accel__t0, b_gyro__t0; | |
| 394 | ✗ | z[3] << b_accel__t0, b_gyro__t0; | |
| 395 | } | ||
| 396 | else | ||
| 397 | { | ||
| 398 | ✗ | z[0] << b_accel__t1, b_gyro__t1; | |
| 399 | ✗ | z[1] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 0.5); | |
| 400 | ✗ | z[2] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 0.5); | |
| 401 | ✗ | z[3] << b_accel__t0, b_gyro__t0; | |
| 402 | } | ||
| 403 | // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5)); | ||
| 404 | // LOG_DATA("{}: z[1] = {}, {}, {}, {}, {}, {}", nameId, z[1](0), z[1](1), z[1](2), z[1](3), z[1](4), z[1](5)); | ||
| 405 | // LOG_DATA("{}: z[2] = {}, {}, {}, {}, {}, {}", nameId, z[2](0), z[2](1), z[2](2), z[2](3), z[2](4), z[2](5)); | ||
| 406 | // LOG_DATA("{}: z[3] = {}, {}, {}, {}, {}, {}", nameId, z[3](0), z[3](1), z[3](2), z[3](3), z[3](4), z[3](5)); | ||
| 407 | ✗ | switch (_integrationFrame) | |
| 408 | { | ||
| 409 | ✗ | case IntegrationFrame::NED: | |
| 410 | ✗ | y = RungeKutta4(y, z, state__t0.m.dt, n_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants); | |
| 411 | ✗ | break; | |
| 412 | ✗ | case IntegrationFrame::ECEF: | |
| 413 | ✗ | y = RungeKutta4(y, z, state__t0.m.dt, e_calcPosVelAttDerivative<T>, _posVelAttDerivativeConstants); | |
| 414 | ✗ | break; | |
| 415 | } | ||
| 416 | ✗ | break; | |
| 417 | } | ||
| 418 | ✗ | case IntegrationAlgorithm::MultiStepRK3: | |
| 419 | { | ||
| 420 | ✗ | LOG_CRITICAL("Not implemented yet"); | |
| 421 | break; | ||
| 422 | } | ||
| 423 | ✗ | case IntegrationAlgorithm::MultiStepRK4: | |
| 424 | { | ||
| 425 | ✗ | LOG_CRITICAL("Not implemented yet"); | |
| 426 | break; | ||
| 427 | } | ||
| 428 | ✗ | case IntegrationAlgorithm::COUNT: | |
| 429 | { | ||
| 430 | ✗ | LOG_CRITICAL("Unreachable"); | |
| 431 | break; | ||
| 432 | } | ||
| 433 | } | ||
| 434 | |||
| 435 |
5/10✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 49997 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 49997 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 49997 times.
✗ Branch 15 not taken.
|
49997 | y.template segment<4>(6) = Eigen::Quaternion<T>(y.template segment<4>(6)).normalized().coeffs(); |
| 436 | |||
| 437 | 99994 | return y; | |
| 438 | } | ||
| 439 | |||
| 440 | private: | ||
| 441 | /// @brief Adds the measurement to the tracking buffer | ||
| 442 | /// @param[in] epoch Epoch of the measurement | ||
| 443 | /// @param[in] dt Time between observation and last state in [s] | ||
| 444 | /// @param[in] p_acceleration Acceleration in platform frame coordinates in [m/s^2] | ||
| 445 | /// @param[in] p_angularRate Angular rate in platform frame coordinates in [rad/s] | ||
| 446 | /// @param[in] nameId NameId of the calling node for logging | ||
| 447 | void addMeasurement(const InsTime& epoch, double dt, | ||
| 448 | const Eigen::Vector3d& p_acceleration, const Eigen::Vector3d& p_angularRate, const char* nameId); | ||
| 449 | |||
| 450 | /// @brief Adds the measurement to the tracking buffer | ||
| 451 | /// @param[in] epoch Epoch of the measurement | ||
| 452 | /// @param[in] dt Time between observation and last state in [s] | ||
| 453 | /// @param[in] deltaTime Delta time over which the deltaVelocity and deltaTheta were measured in [s] | ||
| 454 | /// @param[in] p_deltaVelocity Integrated acceleration in platform frame coordinates in [m/s] | ||
| 455 | /// @param[in] p_deltaTheta Integrated angular rate in platform frame coordinates in [rad] | ||
| 456 | /// @param[in] nameId NameId of the calling node for logging | ||
| 457 | void addDeltaMeasurement(const InsTime& epoch, double dt, double deltaTime, | ||
| 458 | const Eigen::Vector3d& p_deltaVelocity, const Eigen::Vector3d& p_deltaTheta, const char* nameId); | ||
| 459 | |||
| 460 | /// @brief Returns the last state as PosVelAtt | ||
| 461 | [[nodiscard]] std::shared_ptr<PosVelAtt> lastStateAsPosVelAtt() const; | ||
| 462 | |||
| 463 | /// @brief Calculates the inertial navigation solution | ||
| 464 | /// @param[in] imuPos IMU platform frame position with respect to body frame | ||
| 465 | /// @param[in] nameId NameId of the calling node for logging | ||
| 466 | /// @return The new state at the observation time | ||
| 467 | std::shared_ptr<PosVelAtt> calcInertialSolutionFromMeasurementBuffer(const ImuPos& imuPos, const char* nameId); | ||
| 468 | |||
| 469 | /// @brief Resizes the measurement and state buffers depending on the integration algorithm | ||
| 470 | void setBufferSizes(); | ||
| 471 | |||
| 472 | /// List of states. Length depends on algorithm used | ||
| 473 | ScrollingBuffer<State> _states = ScrollingBuffer<State>(1); | ||
| 474 | |||
| 475 | Eigen::Vector3d _p_lastBiasAcceleration = Eigen::Vector3d::Zero(); ///< Initial values for the acceleration bias [m/s^2] | ||
| 476 | Eigen::Vector3d _p_lastBiasAngularRate = Eigen::Vector3d::Zero(); ///< Initial values for the angular rate bias [rad/s] | ||
| 477 | |||
| 478 | // ######################################################################################################################################### | ||
| 479 | |||
| 480 | /// Frame to integrate the observations in | ||
| 481 | IntegrationFrame _integrationFrame = IntegrationFrame::NED; | ||
| 482 | |||
| 483 | /// Wether to lock the integration frame | ||
| 484 | bool _lockIntegrationFrame = false; | ||
| 485 | |||
| 486 | /// Integration algorithm used for the update | ||
| 487 | IntegrationAlgorithm _integrationAlgorithm = IntegrationAlgorithm::SingleStepRungeKutta3; | ||
| 488 | |||
| 489 | /// If true, then the measurements are accumulated values over the last epoch. (always true when using delta measurements, so GUI has no effect) | ||
| 490 | bool _accelerationsAreAveragedMeasurements = true; | ||
| 491 | |||
| 492 | // ######################################################################################################################################### | ||
| 493 | |||
| 494 | /// Settings for the models to use | ||
| 495 | PosVelAttDerivativeConstants _posVelAttDerivativeConstants; | ||
| 496 | |||
| 497 | friend bool InertialIntegratorGui(const char* label, InertialIntegrator& integrator, bool& preferAccelerationOverDeltaMeasurements, float width); | ||
| 498 | friend void to_json(json& j, const InertialIntegrator& data); | ||
| 499 | friend void from_json(const json& j, InertialIntegrator& data); | ||
| 500 | }; | ||
| 501 | |||
| 502 | /// @brief Shows a GUI for advanced configuration of the InertialIntegrator | ||
| 503 | /// @param[in] label Label to show. This has to be a unique id for ImGui. | ||
| 504 | /// @param[in] integrator Reference to the integrator to configure | ||
| 505 | /// @param[in] preferAccelerationOverDeltaMeasurements Wether to prefer accelerations over delta measurements | ||
| 506 | /// @param[in] width Width of the widget | ||
| 507 | bool InertialIntegratorGui(const char* label, InertialIntegrator& integrator, bool& preferAccelerationOverDeltaMeasurements, float width = 250.0F); | ||
| 508 | |||
| 509 | /// @brief Write info to a json object | ||
| 510 | /// @param[out] j Json output | ||
| 511 | /// @param[in] data Object to read info from | ||
| 512 | void to_json(json& j, const InertialIntegrator& data); | ||
| 513 | /// @brief Read info from a json object | ||
| 514 | /// @param[in] j Json variable to read info from | ||
| 515 | /// @param[out] data Output object | ||
| 516 | void from_json(const json& j, InertialIntegrator& data); | ||
| 517 | |||
| 518 | /// @brief Converts the enum to a string | ||
| 519 | /// @param[in] algorithm Enum value to convert into text | ||
| 520 | /// @return String representation of the enum | ||
| 521 | const char* to_string(InertialIntegrator::IntegrationAlgorithm algorithm); | ||
| 522 | |||
| 523 | /// @brief Converts the enum to a string | ||
| 524 | /// @param[in] frame Enum value to convert into text | ||
| 525 | /// @return String representation of the enum | ||
| 526 | const char* to_string(InertialIntegrator::IntegrationFrame frame); | ||
| 527 | |||
| 528 | } // namespace NAV | ||
| 529 |