| 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 Mechanization.hpp | ||
| 10 | /// @brief Inertial Navigation Mechanization Functions in ECEF frame | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2022-06-12 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include "Navigation/Gravity/Gravity.hpp" | ||
| 17 | |||
| 18 | #include <Eigen/Core> | ||
| 19 | #include <Eigen/Dense> | ||
| 20 | |||
| 21 | #include "Navigation/Constants.hpp" | ||
| 22 | #include "Navigation/Ellipsoid/Ellipsoid.hpp" | ||
| 23 | #include "Navigation/INS/Functions.hpp" | ||
| 24 | #include "Navigation/INS/Mechanization.hpp" | ||
| 25 | #include "Navigation/Math/Math.hpp" | ||
| 26 | #include "Navigation/Transformations/CoordinateFrames.hpp" | ||
| 27 | #include "util/Logger.hpp" | ||
| 28 | |||
| 29 | #include <cmath> | ||
| 30 | |||
| 31 | namespace NAV | ||
| 32 | { | ||
| 33 | /// @brief Calculates the time derivative of the quaternion e_Quat_b | ||
| 34 | /// | ||
| 35 | /// \anchor eq-INS-Mechanization-e_Quat_b-dot \f{equation}{ \label{eq:eq-INS-Mechanization-e_Quat_b-dot} | ||
| 36 | /// \mathbf{\dot{q}}_b^e | ||
| 37 | /// = \begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{z} \\ \dot{w} \end{bmatrix} | ||
| 38 | /// = \frac{1}{2} \begin{bmatrix} 0 & \omega_{eb,z}^b & -\omega_{eb,y}^b & \omega_{eb,x}^b \\ | ||
| 39 | /// -\omega_{eb,z}^b & 0 & \omega_{eb,x}^b & \omega_{eb,y}^b \\ | ||
| 40 | /// \omega_{eb,y}^b & -\omega_{eb,x}^b & 0 & \omega_{eb,z}^b \\ | ||
| 41 | /// -\omega_{eb,x}^b & -\omega_{eb,y}^b & -\omega_{eb,z}^b & 0 \end{bmatrix} | ||
| 42 | /// \begin{bmatrix} x \\ y \\ z \\ w \end{bmatrix} | ||
| 43 | /// \f} | ||
| 44 | /// | ||
| 45 | /// @param[in] b_omega_eb ω_eb_b Body rate with respect to the ECEF frame, expressed in the body frame | ||
| 46 | /// @param[in] e_Quat_b_coeffs Coefficients of the quaternion e_Quat_b in order x, y, z, w (q = w + ix + jy + kz) | ||
| 47 | /// @return The time derivative of the coefficients of the quaternion e_Quat_b in order x, y, z, w (q = w + ix + jy + kz) | ||
| 48 | /// | ||
| 49 | /// @note See \ref ImuIntegrator-Mechanization-e-Attitude-Quaternion equation \eqref{eq-ImuIntegrator-Mechanization-e-Attitude-Quaternion-matrix} | ||
| 50 | template<typename DerivedA, typename DerivedB> | ||
| 51 | Eigen::Vector4<typename DerivedA::Scalar> calcTimeDerivativeFor_e_Quat_b(const Eigen::MatrixBase<DerivedA>& b_omega_eb, | ||
| 52 | const Eigen::MatrixBase<DerivedB>& e_Quat_b_coeffs) | ||
| 53 | { | ||
| 54 | using T = typename DerivedA::Scalar; | ||
| 55 | |||
| 56 | // Angular rates in matrix form (Titterton (2005), eq. (11.35)) | ||
| 57 | Eigen::Matrix4<T> A; | ||
| 58 | |||
| 59 | // clang-format off | ||
| 60 | A << T(0.0) , b_omega_eb(2), -b_omega_eb(1), b_omega_eb(0), | ||
| 61 | -b_omega_eb(2), T(0.0) , b_omega_eb(0), b_omega_eb(1), | ||
| 62 | b_omega_eb(1), -b_omega_eb(0), T(0.0) , b_omega_eb(2), | ||
| 63 | -b_omega_eb(0), -b_omega_eb(1), -b_omega_eb(2), T(0.0) ; | ||
| 64 | // clang-format on | ||
| 65 | |||
| 66 | // Propagation of an attitude Quaternion with time (Titterton, ch. 11.2.5, eq. 11.33-11.35, p. 319) | ||
| 67 | return 0.5 * A * e_Quat_b_coeffs; // (x, y, z, w) | ||
| 68 | } | ||
| 69 | |||
| 70 | /// @brief Calculates the time derivative of the velocity in ECEF frame coordinates | ||
| 71 | /// | ||
| 72 | /// \anchor eq-INS-Mechanization-v_e-dot \f{equation}{ \label{eq:eq-INS-Mechanization-v_e-dot} | ||
| 73 | /// \boldsymbol{\dot{v}}^e | ||
| 74 | /// = \overbrace{\boldsymbol{f}^e}^{\hidewidth\text{measured}\hidewidth} | ||
| 75 | /// -\ \underbrace{2 \boldsymbol{\omega}_{ie}^e \times \boldsymbol{v}^e}_{\text{coriolis acceleration}} | ||
| 76 | /// +\ \overbrace{\mathbf{g}^e}^{\hidewidth\text{gravitation}\hidewidth} | ||
| 77 | /// -\ \underbrace{\left(\boldsymbol{\omega}_{ie}^e \times [ \boldsymbol{\omega}_{ie}^e \times \mathbf{x}^e ] \right)}_{\text{centrifugal acceleration}} | ||
| 78 | /// \f} | ||
| 79 | /// | ||
| 80 | /// @param[in] e_measuredForce f_e Specific force vector as measured by a triad of accelerometers and resolved into ECEF frame coordinates | ||
| 81 | /// @param[in] e_coriolisAcceleration Coriolis acceleration in ECEF coordinates in [m/s^2] | ||
| 82 | /// @param[in] e_gravitation Local gravitation vector (caused by effects of mass attraction) in ECEF frame coordinates [m/s^2] | ||
| 83 | /// @param[in] e_centrifugalAcceleration Centrifugal acceleration in ECEF coordinates in [m/s^2] | ||
| 84 | /// @return The time derivative of the velocity in ECEF frame coordinates | ||
| 85 | /// | ||
| 86 | /// @note See \ref ImuIntegrator-Mechanization-e-Velocity equation \eqref{eq-ImuIntegrator-Mechanization-e-Velocity} | ||
| 87 | template<typename DerivedA, typename DerivedB, typename DerivedC, typename DerivedD> | ||
| 88 | 38664 | Eigen::Vector3<typename DerivedA::Scalar> e_calcTimeDerivativeForVelocity(const Eigen::MatrixBase<DerivedA>& e_measuredForce, | |
| 89 | const Eigen::MatrixBase<DerivedB>& e_coriolisAcceleration, | ||
| 90 | const Eigen::MatrixBase<DerivedC>& e_gravitation, | ||
| 91 | const Eigen::MatrixBase<DerivedD>& e_centrifugalAcceleration) | ||
| 92 | { | ||
| 93 | return e_measuredForce | ||
| 94 | - e_coriolisAcceleration | ||
| 95 | + e_gravitation | ||
| 96 |
4/8✓ Branch 1 taken 38664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38664 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38664 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 38664 times.
✗ Branch 11 not taken.
|
38664 | - e_centrifugalAcceleration; |
| 97 | } | ||
| 98 | |||
| 99 | /// @brief Calculates the time derivative of the ECEF position | ||
| 100 | /// | ||
| 101 | /// \anchor eq-INS-Mechanization-x_e-dot \f{equation}{ \label{eq:eq-INS-Mechanization-x_e-dot} | ||
| 102 | /// \boldsymbol{\dot{x}}^e = \boldsymbol{v}^e | ||
| 103 | /// \f} | ||
| 104 | /// | ||
| 105 | /// @param[in] e_velocity Velocity with respect to the Earth in ECEF frame coordinates [m/s] | ||
| 106 | /// @return The time derivative of the ECEF position | ||
| 107 | /// | ||
| 108 | /// @note See \ref ImuIntegrator-Mechanization-e-Position equation \eqref{eq-ImuIntegrator-Mechanization-e-Position} | ||
| 109 | template<typename Derived> | ||
| 110 | 38664 | Eigen::Vector3<typename Derived::Scalar> e_calcTimeDerivativeForPosition(const Eigen::MatrixBase<Derived>& e_velocity) | |
| 111 | { | ||
| 112 | 38664 | return e_velocity; | |
| 113 | } | ||
| 114 | |||
| 115 | /// @brief Calculates the derivative of the quaternion, velocity and position in ECEF coordinates | ||
| 116 | /// @param[in] y [x, y, z, v_x, v_y, v_z, e_q_bx, e_q_by, e_q_bz, e_q_bw]^T | ||
| 117 | /// @param[in] z [fx, fy, fz, ωx, ωy, ωz]^T | ||
| 118 | /// @param[in] c Constant values needed to calculate the derivatives | ||
| 119 | /// @return The derivative ∂/∂t [x, y, z, v_x, v_y, v_z, ωx, ωy, ωz]^T | ||
| 120 | template<typename T> | ||
| 121 | 38664 | Eigen::Vector<T, 9> e_calcPosVelAttDerivative(const Eigen::Vector<T, 10>& y, const Eigen::Vector<T, 6>& z, const PosVelAttDerivativeConstants& c, double /* t */ = 0.0) | |
| 122 | { | ||
| 123 | // 0 1 2 3 4 5 6 7 8 | ||
| 124 | // ∂/∂t [ x, y, z, v_x, v_y, v_z, ωx, ωy, ωz]^T | ||
| 125 |
2/4✓ Branch 1 taken 38664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38664 times.
✗ Branch 5 not taken.
|
38664 | Eigen::Vector<T, 9> y_dot = Eigen::Vector<T, 9>::Zero(); |
| 126 | |||
| 127 |
2/4✓ Branch 1 taken 38664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38664 times.
✗ Branch 5 not taken.
|
38664 | Eigen::Vector3<T> lla_position = trafo::ecef2lla_WGS84(y.template segment<3>(0)); |
| 128 | |||
| 129 |
2/4✓ Branch 1 taken 38664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38664 times.
✗ Branch 5 not taken.
|
38664 | Eigen::Quaternion<T> e_Quat_b{ y.template segment<4>(6) }; |
| 130 | // e_Quat_b.normalize(); // No need to normalize if quaternion is applied with matrix/quaternion exponential | ||
| 131 |
3/6✓ Branch 1 taken 38664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38664 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38664 times.
✗ Branch 8 not taken.
|
38664 | Eigen::Quaternion<T> n_Quat_e = trafo::n_Quat_e(lla_position(0), lla_position(1)); |
| 132 | |||
| 133 |
1/2✓ Branch 1 taken 38664 times.
✗ Branch 2 not taken.
|
38664 | Eigen::Quaternion<T> b_Quat_e = e_Quat_b.conjugate(); |
| 134 |
1/2✓ Branch 1 taken 38664 times.
✗ Branch 2 not taken.
|
38664 | Eigen::Quaternion<T> e_Quat_n = n_Quat_e.conjugate(); |
| 135 | |||
| 136 | LOG_DATA("e_velocity = {} [m/s]", y.template segment<3>(3).transpose()); | ||
| 137 | LOG_DATA("e_position = {} [m]", y.template segment<3>(0).transpose()); | ||
| 138 | |||
| 139 | // ω_eb_b = ω_ib_b - C_be * ω_ie_e | ||
| 140 |
4/8✓ Branch 1 taken 38664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38664 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38664 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 38664 times.
✗ Branch 11 not taken.
|
38664 | Eigen::Vector3<T> b_omega_eb = z.template segment<3>(3) |
| 141 |
2/8✓ Branch 0 taken 38664 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 38664 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
38664 | - b_Quat_e * (c.angularRateEarthRotationCompensationEnabled ? InsConst::e_omega_ie : Eigen::Vector3d::Zero()).template cast<T>(); |
| 142 | LOG_DATA("b_omega_eb = {} [rad/s]", b_omega_eb.transpose()); | ||
| 143 | |||
| 144 | // Coriolis acceleration in [m/s^2] (acceleration due to motion in rotating reference frame) | ||
| 145 |
1/6✓ Branch 1 taken 38664 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
38664 | Eigen::Vector3<T> e_coriolisAcceleration = c.coriolisAccelerationCompensationEnabled |
| 146 |
2/4✓ Branch 0 taken 38664 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 38664 times.
✗ Branch 4 not taken.
|
38664 | ? e_calcCoriolisAcceleration(InsConst::e_omega_ie, y.template segment<3>(3)) |
| 147 | : Eigen::Vector3<T>::Zero(); | ||
| 148 | LOG_DATA("e_coriolisAcceleration = {} [m/s^2]", e_coriolisAcceleration.transpose()); | ||
| 149 | // Centrifugal acceleration in [m/s^2] (acceleration that makes a body follow a curved path) | ||
| 150 |
1/6✓ Branch 1 taken 38664 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
38664 | Eigen::Vector3<T> e_centrifugalAcceleration = c.centrifgalAccelerationCompensationEnabled |
| 151 |
2/4✓ Branch 0 taken 38664 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 38664 times.
✗ Branch 4 not taken.
|
38664 | ? e_calcCentrifugalAcceleration(y.template segment<3>(0), InsConst::e_omega_ie) |
| 152 | : Eigen::Vector3<T>::Zero(); | ||
| 153 | LOG_DATA("e_centrifugalAcceleration = {} [m/s^2]", e_centrifugalAcceleration.transpose()); | ||
| 154 | |||
| 155 |
2/4✓ Branch 1 taken 38664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38664 times.
✗ Branch 5 not taken.
|
38664 | Eigen::Vector3<T> e_gravitation = e_Quat_n * n_calcGravitation(lla_position, c.gravitationModel); |
| 156 | LOG_DATA("e_gravitation = {} [m/s^2] ({})", e_gravitation.transpose(), to_string(c.gravitationModel)); | ||
| 157 | |||
| 158 |
4/8✓ Branch 1 taken 38664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38664 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38664 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 38664 times.
✗ Branch 11 not taken.
|
38664 | y_dot.template segment<3>(0) = e_calcTimeDerivativeForPosition(y.template segment<3>(3)); // Velocity with respect to the Earth in ECEF frame coordinates [m/s] |
| 159 | |||
| 160 |
5/10✓ Branch 1 taken 38664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38664 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38664 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 38664 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 38664 times.
✗ Branch 14 not taken.
|
38664 | y_dot.template segment<3>(3) = e_calcTimeDerivativeForVelocity(e_Quat_b * z.template segment<3>(0), // f_n Specific force vector as measured by a triad of accelerometers and resolved into ECEF frame coordinates |
| 161 | e_coriolisAcceleration, // Coriolis acceleration in ECEF coordinates in [m/s^2] | ||
| 162 | e_gravitation, // Local gravitation vector (caused by effects of mass attraction) in ECEF frame coordinates [m/s^2] | ||
| 163 | e_centrifugalAcceleration); // Centrifugal acceleration in ECEF coordinates in [m/s^2] | ||
| 164 | |||
| 165 |
2/4✓ Branch 1 taken 38664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38664 times.
✗ Branch 5 not taken.
|
38664 | y_dot.template segment<3>(6) = b_omega_eb; // ω_eb_b Body rate with respect to the ECEF frame, expressed in the body frame |
| 166 | |||
| 167 | LOG_DATA("e_position_dot = {} [m/s]", y_dot.template segment<3>(0).transpose()); | ||
| 168 | LOG_DATA("e_velocity_dot = {} [m/s^2]", y_dot.template segment<3>(3).transpose()); | ||
| 169 | LOG_DATA("ω_eb_b = {} ", y_dot.template segment<3>(6).transpose()); | ||
| 170 | |||
| 171 | 77328 | return y_dot; | |
| 172 | } | ||
| 173 | |||
| 174 | } // namespace NAV | ||
| 175 |