| 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 InertialPreIntegrator.hpp | ||
| 10 | /// @brief Inertial Measurement Preintegrator | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2025-06-03 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include "Navigation/Math/Math.hpp" | ||
| 17 | #include "Navigation/Transformations/CoordinateFrames.hpp" | ||
| 18 | #include "NodeData/IMU/ImuPos.hpp" | ||
| 19 | |||
| 20 | #include "Navigation/Gravity/Gravity.hpp" | ||
| 21 | #include "Navigation/Time/InsTime.hpp" | ||
| 22 | |||
| 23 | #include "util/Eigen.hpp" | ||
| 24 | #include "util/Logger.hpp" | ||
| 25 | #include "util/Json.hpp" | ||
| 26 | |||
| 27 | namespace NAV | ||
| 28 | { | ||
| 29 | |||
| 30 | /// @brief Inertial Measurement Integrator | ||
| 31 | class InertialPreIntegrator | ||
| 32 | { | ||
| 33 | public: | ||
| 34 | /// Available Integration Frames | ||
| 35 | enum class IntegrationFrame : uint8_t | ||
| 36 | { | ||
| 37 | ECEF, ///< Earth-Centered Earth-Fixed frame | ||
| 38 | NED, ///< Local North-East-Down frame | ||
| 39 | }; | ||
| 40 | |||
| 41 | /// IMU measurement | ||
| 42 | struct ImuMeasurement | ||
| 43 | { | ||
| 44 | double dt = 0.0; ///< Time since previous observation in [s] | ||
| 45 | Eigen::Vector3d p_acceleration; ///< Acceleration in platform frame coordinates in [m/s^2] | ||
| 46 | Eigen::Vector3d p_angularRate; ///< Angular rate in platform frame coordinates in [rad/s] | ||
| 47 | }; | ||
| 48 | |||
| 49 | /// IMU state | ||
| 50 | template<typename T> | ||
| 51 | struct ImuState | ||
| 52 | { | ||
| 53 | Eigen::Vector3<T> p_biasAcceleration = Eigen::Vector3<T>::Zero(); ///< Acceleration bias in platform frame coordinates in [m/s^2] | ||
| 54 | Eigen::Vector3<T> p_biasAngularRate = Eigen::Vector3<T>::Zero(); ///< Angular rate bias in platform frame coordinates in [rad/s] | ||
| 55 | |||
| 56 | // Eigen::Vector3<T> scaleFactorAccel = Eigen::Vector3<T>::Ones(); ///< Scale factor of the accelerometer [-] | ||
| 57 | // Eigen::Vector3<T> scaleFactorGyro = Eigen::Vector3<T>::Ones(); ///< Scale factor of the gyroscope [-] | ||
| 58 | |||
| 59 | // Eigen::Quaternion<T> misalignmentAccel = Eigen::Quaternion<T>::Identity(); ///< Misalignment of the accelerometer sensor axes | ||
| 60 | // Eigen::Quaternion<T> misalignmentGyro = Eigen::Quaternion<T>::Identity(); ///< Misalignment of the gyroscope sensor axes | ||
| 61 | |||
| 62 | /// @brief Equal comparison | ||
| 63 | /// @param[in] rhs Right hand side of the operator | ||
| 64 | /// @return True if the elements are equal | ||
| 65 | ✗ | constexpr bool operator==(const ImuState<T>& rhs) const | |
| 66 | { | ||
| 67 | ✗ | return p_biasAcceleration == rhs.p_biasAcceleration && p_biasAngularRate == rhs.p_biasAngularRate; | |
| 68 | // && scaleFactorAccel == rhs.scaleFactorAccel && scaleFactorGyro == rhs.scaleFactorGyro | ||
| 69 | // && misalignmentAccel == rhs.misalignmentAccel && misalignmentGyro == rhs.misalignmentGyro; | ||
| 70 | } | ||
| 71 | }; | ||
| 72 | |||
| 73 | /// Inertial Measurement for a generic type | ||
| 74 | template<typename T> | ||
| 75 | struct GenericMeasurement | ||
| 76 | { | ||
| 77 | ImuMeasurement meas; ///< IMU measurement | ||
| 78 | ImuState<T> imu; ///< IMU state | ||
| 79 | }; | ||
| 80 | |||
| 81 | /// Position, velocity and attitude state | ||
| 82 | template<typename T> | ||
| 83 | struct PVAState | ||
| 84 | { | ||
| 85 | Eigen::Vector3<T> position; ///< IMU position (e_pos / lla_pos) | ||
| 86 | Eigen::Vector3<T> velocity; ///< IMU velocity (e_vel / n_vel) | ||
| 87 | Eigen::Quaternion<T> attitude; ///< IMU attitude (e_Quat_b / n_Quat_b) | ||
| 88 | }; | ||
| 89 | |||
| 90 | /// Inertial measurements | ||
| 91 | using Measurement = GenericMeasurement<double>; | ||
| 92 | |||
| 93 | /// @brief Default Constructor | ||
| 94 |
6/12✓ Branch 2 taken 115 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 115 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 115 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 115 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 115 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 115 times.
✗ Branch 18 not taken.
|
115 | InertialPreIntegrator() = default; |
| 95 | |||
| 96 | /// @brief Constructor | ||
| 97 | /// @param integrationFrame Integration frame to lock to | ||
| 98 | explicit InertialPreIntegrator(IntegrationFrame integrationFrame); | ||
| 99 | |||
| 100 | /// @brief Clears all internal data | ||
| 101 | void reset(); | ||
| 102 | |||
| 103 | /// @brief Set the IMU Noise Scale Matrix | ||
| 104 | /// @param[in] W IMU Noise Scale Matrix | ||
| 105 | void setImuNoiseScaleMatrix(const Eigen::Matrix6d& W); | ||
| 106 | |||
| 107 | /// @brief Adds a inertial measurement to the relative motion increments | ||
| 108 | /// @param[in] meas Inertial measurement | ||
| 109 | /// @param[in] imuPos IMU mounting position connecting the platform to the body frame | ||
| 110 | /// @param[in] nameId Name and id of the calling node for logging | ||
| 111 | void addInertialMeasurement(const Measurement& meas, const ImuPos& imuPos, [[maybe_unused]] const std::string& nameId); | ||
| 112 | |||
| 113 | /// @brief Calculates the state using the preintegrated relative motion increments | ||
| 114 | /// @param[in] pvaOld Position, velocity and attitude at the start of the preintegration interval | ||
| 115 | /// @param[in] imuNew The IMU state at the moment of evaluation | ||
| 116 | /// @param[in] imuPos IMU mounting position connecting the platform to the body frame | ||
| 117 | /// @param[in] nameId Name and id of the calling node for logging | ||
| 118 | /// @return The position, velocity and attitude at the end of the preintegration interval | ||
| 119 | template<typename T> | ||
| 120 | ✗ | PVAState<T> calcIntegratedState(const PVAState<T>& pvaOld, const ImuState<T>& imuNew, const ImuPos& imuPos, [[maybe_unused]] const std::string& nameId) | |
| 121 | { | ||
| 122 | // LOG_DATA("{}: calcIntegratedState (dt = {})", nameId, dt); | ||
| 123 | // LOG_DATA("{}: pvaOld.attitude = {}", nameId, pvaOld.attitude); | ||
| 124 | // LOG_DATA("{}: pvaOld.velocity = {}", nameId, pvaOld.velocity.transpose()); | ||
| 125 | // LOG_DATA("{}: pvaOld.position = {}", nameId, pvaOld.position.transpose()); | ||
| 126 | |||
| 127 | // LOG_DATA("{}: attitudeIncrement = {}", nameId, _attitudeIncrement); | ||
| 128 | // LOG_DATA("{}: velocityIncrement = {}", nameId, _velocityIncrement.transpose()); | ||
| 129 | // LOG_DATA("{}: positionIncrement = {}", nameId, _positionIncrement.transpose()); | ||
| 130 | |||
| 131 | ✗ | Eigen::Vector3d deltaBiasAngularRate = imuPos.b_quat_p() * (imuNew.p_biasAngularRate - _imuState.p_biasAngularRate); | |
| 132 | ✗ | Eigen::Vector3d deltaBiasAcceleration = imuPos.b_quat_p() * (imuNew.p_biasAcceleration - _imuState.p_biasAcceleration); | |
| 133 | |||
| 134 | ✗ | Eigen::Quaterniond attitudeIncrement = _attitudeIncrement * math::expMapQuat(_pAtt_pOmega * deltaBiasAngularRate); | |
| 135 | ✗ | Eigen::Vector3d velocityIncrement = _velocityIncrement + _pVel_pAccel * deltaBiasAcceleration + _pVel_pOmega * deltaBiasAngularRate; | |
| 136 | ✗ | Eigen::Vector3d positionIncrement = _positionIncrement + _pPos_pAccel * deltaBiasAcceleration + _pPos_pOmega * deltaBiasAngularRate; | |
| 137 | |||
| 138 | ✗ | auto lla_positionOld = trafo::ecef2lla_WGS84(pvaOld.position); | |
| 139 | ✗ | Eigen::Vector3<T> gravitation = n_calcGravitation(lla_positionOld, _gravitationModel); | |
| 140 | ✗ | if (_integrationFrame == IntegrationFrame::ECEF) | |
| 141 | { | ||
| 142 | ✗ | gravitation = trafo::e_Quat_n(lla_positionOld(0), lla_positionOld(1)) * gravitation; | |
| 143 | } | ||
| 144 | // LOG_DATA("{}: gravitation = {} (vel = {}, pos = {})", nameId, gravitation.transpose(), (gravitation * dt).transpose(), (0.5 * gravitation * dt * dt).transpose()); | ||
| 145 | |||
| 146 | ✗ | PVAState<T> pvaNew; | |
| 147 | ✗ | pvaNew.attitude = pvaOld.attitude * attitudeIncrement; | |
| 148 | ✗ | pvaNew.velocity = pvaOld.velocity | |
| 149 | ✗ | + pvaOld.attitude * velocityIncrement | |
| 150 | ✗ | + gravitation * _timeIncrement; | |
| 151 | ✗ | pvaNew.position = pvaOld.position | |
| 152 | ✗ | + pvaOld.velocity * _timeIncrement | |
| 153 | ✗ | + pvaOld.attitude * positionIncrement | |
| 154 | ✗ | + 0.5 * gravitation * _timeIncrement * _timeIncrement; | |
| 155 | |||
| 156 | ✗ | return pvaNew; | |
| 157 | } | ||
| 158 | |||
| 159 | /// @brief Get the Covariance Matrix of the preintegrated measurements (3x attitude, 3x velocity, 3x position) | ||
| 160 | [[nodiscard]] const Eigen::Matrix9d& getCovMatrix() const; | ||
| 161 | |||
| 162 | /// @brief Returns the selected integration frame | ||
| 163 | [[nodiscard]] IntegrationFrame getIntegrationFrame() const; | ||
| 164 | |||
| 165 | private: | ||
| 166 | /// Frame to integrate the observations in | ||
| 167 | IntegrationFrame _integrationFrame = IntegrationFrame::ECEF; | ||
| 168 | |||
| 169 | /// Gravity Model to use | ||
| 170 | GravitationModel _gravitationModel = GravitationModel::EGM96; | ||
| 171 | |||
| 172 | /// Wether to lock the integration frame | ||
| 173 | bool _lockIntegrationFrame = false; | ||
| 174 | |||
| 175 | // ######################################################################################################################################### | ||
| 176 | |||
| 177 | double _timeIncrement = 0.0; ///< Time accumulated for each increment | ||
| 178 | Eigen::Quaterniond _attitudeIncrement = Eigen::Quaterniond::Identity(); ///< Relative motion increment for the attitude bi_q_bj | ||
| 179 | Eigen::Vector3d _velocityIncrement = Eigen::Vector3d::Zero(); ///< Relative motion increment for the velocity | ||
| 180 | Eigen::Vector3d _positionIncrement = Eigen::Vector3d::Zero(); ///< Relative motion increment for the position | ||
| 181 | |||
| 182 | Eigen::Matrix9d _covMatrix = Eigen::Matrix9d::Zero(); ///< Incremental covariance matrix (3x attitude, 3x velocity, 3x position) | ||
| 183 | |||
| 184 | Eigen::Matrix3d _pAtt_pOmega; ///< Partial derivative of the attitude increment after the angular rate bias | ||
| 185 | Eigen::Matrix3d _pVel_pAccel; ///< Partial derivative of the velocity increment after the acceleration bias | ||
| 186 | Eigen::Matrix3d _pVel_pOmega; ///< Partial derivative of the velocity increment after the angular rate bias | ||
| 187 | Eigen::Matrix3d _pPos_pAccel; ///< Partial derivative of the position increment after the acceleration bias | ||
| 188 | Eigen::Matrix3d _pPos_pOmega; ///< Partial derivative of the position increment after the angular rate bias | ||
| 189 | |||
| 190 | ImuState<double> _imuState; ///< IMU state used to calculate the increments | ||
| 191 | |||
| 192 | std::optional<Eigen::Matrix6d> _imuNoiseScale_W; ///< Noise scale matrix of the IMU | ||
| 193 | |||
| 194 | // ######################################################################################################################################### | ||
| 195 | |||
| 196 | // Forward declaring external function | ||
| 197 | friend bool InertialPreIntegratorGui(const char* label, InertialPreIntegrator& integrator, float width); | ||
| 198 | friend void to_json(json& j, const InertialPreIntegrator& data); | ||
| 199 | friend void from_json(const json& j, InertialPreIntegrator& data); | ||
| 200 | friend const char* to_string(InertialPreIntegrator::IntegrationFrame frame); | ||
| 201 | }; | ||
| 202 | |||
| 203 | /// @brief Shows a GUI for advanced configuration of the InertialPreIntegrator | ||
| 204 | /// @param[in] label Label to show. This has to be a unique id for ImGui. | ||
| 205 | /// @param[in] integrator Reference to the integrator to configure | ||
| 206 | /// @param[in] width Width of the widget | ||
| 207 | bool InertialPreIntegratorGui(const char* label, InertialPreIntegrator& integrator, float width = 250.0F); | ||
| 208 | |||
| 209 | /// @brief Write info to a json object | ||
| 210 | /// @param[out] j Json output | ||
| 211 | /// @param[in] data Object to read info from | ||
| 212 | void to_json(json& j, const InertialPreIntegrator& data); | ||
| 213 | /// @brief Read info from a json object | ||
| 214 | /// @param[in] j Json variable to read info from | ||
| 215 | /// @param[out] data Output object | ||
| 216 | void from_json(const json& j, InertialPreIntegrator& data); | ||
| 217 | |||
| 218 | /// @brief Converts the enum to a string | ||
| 219 | /// @param[in] frame Enum value to convert into text | ||
| 220 | /// @return String representation of the enum | ||
| 221 | const char* to_string(InertialPreIntegrator::IntegrationFrame frame); | ||
| 222 | |||
| 223 | } // namespace NAV | ||
| 224 |