| 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 local navigation frame | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2020-09-02 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <Eigen/Core> | ||
| 17 | #include <Eigen/Dense> | ||
| 18 | |||
| 19 | #include "Navigation/Constants.hpp" | ||
| 20 | #include "Navigation/Ellipsoid/Ellipsoid.hpp" | ||
| 21 | #include "Navigation/INS/Functions.hpp" | ||
| 22 | #include "Navigation/INS/Mechanization.hpp" | ||
| 23 | #include "Navigation/Gravity/Gravity.hpp" | ||
| 24 | #include "Navigation/Math/Math.hpp" | ||
| 25 | #include "Navigation/Math/NumericalIntegration.hpp" | ||
| 26 | #include "Navigation/Transformations/CoordinateFrames.hpp" | ||
| 27 | #include "Navigation/Transformations/Units.hpp" | ||
| 28 | #include "util/Logger.hpp" | ||
| 29 | |||
| 30 | #include <cmath> | ||
| 31 | |||
| 32 | namespace NAV | ||
| 33 | { | ||
| 34 | /// @brief Calculates the time derivative of the quaternion n_Quat_b | ||
| 35 | /// | ||
| 36 | /// \anchor eq-INS-Mechanization-n_Quat_b-dot \f{equation}{ \label{eq:eq-INS-Mechanization-n_Quat_b-dot} | ||
| 37 | /// \mathbf{\dot{q}}_b^n | ||
| 38 | /// = \begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{z} \\ \dot{w} \end{bmatrix} | ||
| 39 | /// = \frac{1}{2} \begin{bmatrix} 0 & \omega_{nb,z}^b & -\omega_{nb,y}^b & \omega_{nb,x}^b \\ | ||
| 40 | /// -\omega_{nb,z}^b & 0 & \omega_{nb,x}^b & \omega_{nb,y}^b \\ | ||
| 41 | /// \omega_{nb,y}^b & -\omega_{nb,x}^b & 0 & \omega_{nb,z}^b \\ | ||
| 42 | /// -\omega_{nb,x}^b & -\omega_{nb,y}^b & -\omega_{nb,z}^b & 0 \end{bmatrix} | ||
| 43 | /// \begin{bmatrix} x \\ y \\ z \\ w \end{bmatrix} | ||
| 44 | /// \f} | ||
| 45 | /// | ||
| 46 | /// @param[in] b_omega_nb ω_nb_b Body rate with respect to the navigation frame, expressed in the body frame | ||
| 47 | /// @param[in] n_Quat_b_coeffs Coefficients of the quaternion n_Quat_b in order x, y, z, w (q = w + ix + jy + kz) | ||
| 48 | /// @return The time derivative of the coefficients of the quaternion n_Quat_b in order x, y, z, w (q = w + ix + jy + kz) | ||
| 49 | /// | ||
| 50 | /// @note See \ref ImuIntegrator-Mechanization-n-Attitude-Quaternion equation \eqref{eq-ImuIntegrator-Mechanization-n-Attitude-Quaternion-matrix-Titterton} | ||
| 51 | template<typename DerivedA, typename DerivedB> | ||
| 52 | Eigen::Vector4<typename DerivedA::Scalar> calcTimeDerivativeFor_n_Quat_b(const Eigen::MatrixBase<DerivedA>& b_omega_nb, | ||
| 53 | const Eigen::MatrixBase<DerivedB>& n_Quat_b_coeffs) | ||
| 54 | { | ||
| 55 | using T = typename DerivedA::Scalar; | ||
| 56 | |||
| 57 | // Angular rates in matrix form (Titterton (2005), eq. (11.35)) | ||
| 58 | Eigen::Matrix4<T> A; | ||
| 59 | |||
| 60 | // clang-format off | ||
| 61 | A << T(0.0) , b_omega_nb(2), -b_omega_nb(1), b_omega_nb(0), | ||
| 62 | -b_omega_nb(2), T(0.0) , b_omega_nb(0), b_omega_nb(1), | ||
| 63 | b_omega_nb(1), -b_omega_nb(0), T(0.0) , b_omega_nb(2), | ||
| 64 | -b_omega_nb(0), -b_omega_nb(1), -b_omega_nb(2), T(0.0) ; | ||
| 65 | // clang-format on | ||
| 66 | |||
| 67 | // Propagation of an attitude Quaternion with time (Titterton, ch. 11.2.5, eq. 11.33-11.35, p. 319) | ||
| 68 | return 0.5 * A * n_Quat_b_coeffs; // (x, y, z, w) | ||
| 69 | } | ||
| 70 | |||
| 71 | /// @brief Calculates the time derivative of the velocity in local-navigation frame coordinates | ||
| 72 | /// | ||
| 73 | /// \anchor eq-INS-Mechanization-v_n-dot \f{equation}{ \label{eq:eq-INS-Mechanization-v_n-dot} | ||
| 74 | /// \boldsymbol{\dot{v}}^n | ||
| 75 | /// = \overbrace{\boldsymbol{f}^n}^{\hidewidth\text{measured}\hidewidth} | ||
| 76 | /// -\ \underbrace{(2 \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{v}^n}_{\text{coriolis acceleration}} | ||
| 77 | /// +\ \overbrace{\mathbf{g}^n}^{\hidewidth\text{gravitation}\hidewidth} | ||
| 78 | /// -\ \mathbf{C}_e^n \cdot \underbrace{\left(\boldsymbol{\omega}_{ie}^e \times [ \boldsymbol{\omega}_{ie}^e \times \mathbf{x}^e ] \right)}_{\text{centrifugal acceleration}} | ||
| 79 | /// \f} | ||
| 80 | /// | ||
| 81 | /// @param[in] n_measuredForce f_n = [f_N f_E f_D]^T Specific force vector as measured by a triad of accelerometers and resolved into local-navigation frame coordinates | ||
| 82 | /// @param[in] n_coriolisAcceleration Coriolis acceleration in local-navigation coordinates in [m/s^2] | ||
| 83 | /// @param[in] n_gravitation Local gravitation vector (caused by effects of mass attraction) in local-navigation frame coordinates [m/s^2] | ||
| 84 | /// @param[in] n_centrifugalAcceleration Centrifugal acceleration in local-navigation coordinates in [m/s^2] | ||
| 85 | /// @return The time derivative of the velocity in local-navigation frame coordinates | ||
| 86 | /// | ||
| 87 | /// @note See \ref ImuIntegrator-Mechanization-n-Velocity equation \eqref{eq-ImuIntegrator-Mechanization-n-Velocity} | ||
| 88 | template<typename DerivedA, typename DerivedB, typename DerivedC, typename DerivedD> | ||
| 89 | 111327 | Eigen::Vector3<typename DerivedA::Scalar> n_calcTimeDerivativeForVelocity(const Eigen::MatrixBase<DerivedA>& n_measuredForce, | |
| 90 | const Eigen::MatrixBase<DerivedB>& n_coriolisAcceleration, | ||
| 91 | const Eigen::MatrixBase<DerivedC>& n_gravitation, | ||
| 92 | const Eigen::MatrixBase<DerivedD>& n_centrifugalAcceleration) | ||
| 93 | { | ||
| 94 | return n_measuredForce | ||
| 95 | - n_coriolisAcceleration | ||
| 96 | + n_gravitation | ||
| 97 |
4/8✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111327 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 111327 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 111327 times.
✗ Branch 11 not taken.
|
111327 | - n_centrifugalAcceleration; |
| 98 | } | ||
| 99 | |||
| 100 | /// @brief Calculates the time derivative of the curvilinear position | ||
| 101 | /// | ||
| 102 | /// \anchor eq-INS-Mechanization-p_lla-dot \f{equation}{ \label{eq:eq-INS-Mechanization-p_lla-dot} | ||
| 103 | /// \begin{aligned} | ||
| 104 | /// \dot{\phi} &= \frac{v_N}{R_N + h} \\ | ||
| 105 | /// \dot{\lambda} &= \frac{v_E}{(R_E + h) \cos{\phi}} \\ | ||
| 106 | /// \dot{h} &= -v_D | ||
| 107 | /// \end{aligned} | ||
| 108 | /// \f} | ||
| 109 | /// | ||
| 110 | /// @param[in] n_velocity [v_N v_E v_D]^T Velocity with respect to the Earth in local-navigation frame coordinates [m/s] | ||
| 111 | /// @param[in] phi ϕ Latitude [rad] | ||
| 112 | /// @param[in] h Altitude above the ellipsoid [m] | ||
| 113 | /// @param[in] R_N North/South (meridian) earth radius [m] | ||
| 114 | /// @param[in] R_E East/West (prime vertical) earth radius [m] | ||
| 115 | /// @return The time derivative of the curvilinear position | ||
| 116 | /// | ||
| 117 | /// @note See \ref ImuIntegrator-Mechanization-n-Position equation \eqref{eq-ImuIntegrator-Mechanization-n-Position} | ||
| 118 | template<typename Derived> | ||
| 119 | 111327 | Eigen::Vector3<typename Derived::Scalar> lla_calcTimeDerivativeForPosition(const Eigen::MatrixBase<Derived>& n_velocity, | |
| 120 | const auto& phi, const auto& h, | ||
| 121 | const auto& R_N, const auto& R_E) | ||
| 122 | { | ||
| 123 | // Velocity North in [m/s] | ||
| 124 |
1/2✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
|
111327 | const auto& v_N = n_velocity(0); |
| 125 | // Velocity East in [m/s] | ||
| 126 |
1/2✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
|
111327 | const auto& v_E = n_velocity(1); |
| 127 | // Velocity Down in [m/s] | ||
| 128 |
1/2✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
|
111327 | const auto& v_D = n_velocity(2); |
| 129 | |||
| 130 | ✗ | return { v_N / (R_N + h), | |
| 131 | ✗ | v_E / ((R_E + h) * std::cos(phi)), | |
| 132 |
1/2✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
|
111327 | -v_D }; |
| 133 | } | ||
| 134 | |||
| 135 | /// @brief Calculates the derivative of the quaternion, velocity and curvilinear position | ||
| 136 | /// @param[in] y [ 𝜙, λ, h, v_N, v_E, v_D, n_q_bx, n_q_by, n_q_bz, n_q_bw]^T | ||
| 137 | /// @param[in] z [fx, fy, fz, ωx, ωy, ωz]^T | ||
| 138 | /// @param[in] c Constant values needed to calculate the derivatives | ||
| 139 | /// @return The derivative ∂/∂t [ 𝜙, λ, h, v_N, v_E, v_D, ωx, ωy, ωz]^T | ||
| 140 | template<typename T> | ||
| 141 | 111327 | Eigen::Vector<T, 9> n_calcPosVelAttDerivative(const Eigen::Vector<T, 10>& y, const Eigen::Vector<T, 6>& z, const PosVelAttDerivativeConstants& c, double /* t */ = 0.0) | |
| 142 | { | ||
| 143 | // 0 1 2 3 4 5 6 7 8 | ||
| 144 | // ∂/∂t [ 𝜙, λ, h, v_N, v_E, v_D, ωx, ωy, ωz]^T | ||
| 145 |
2/4✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111327 times.
✗ Branch 5 not taken.
|
111327 | Eigen::Vector<T, 9> y_dot = Eigen::Vector<T, 9>::Zero(); |
| 146 | |||
| 147 |
2/4✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111327 times.
✗ Branch 5 not taken.
|
111327 | Eigen::Quaternion<T> n_Quat_b{ y.template segment<4>(6) }; |
| 148 | // n_Quat_b.normalize(); // No need to normalize if quaternion is applied with matrix/quaternion exponential | ||
| 149 |
3/6✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111327 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 111327 times.
✗ Branch 8 not taken.
|
111327 | Eigen::Quaternion<T> n_Quat_e = trafo::n_Quat_e(y(0), y(1)); |
| 150 | |||
| 151 | LOG_DATA("rollPitchYaw = {} [°]", rad2deg(trafo::quat2eulerZYX(n_Quat_b)).transpose()); | ||
| 152 | LOG_DATA("n_velocity = {} [m/s]", y.template segment<3>(3).transpose()); | ||
| 153 | LOG_DATA("lla_position = {}°, {}°, {} m", rad2deg(y(0)), rad2deg(y(1)), y(2)); | ||
| 154 | |||
| 155 |
1/2✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
|
111327 | auto R_N = calcEarthRadius_N(y(0)); |
| 156 | LOG_DATA("R_N = {} [m]", R_N); | ||
| 157 |
2/4✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111327 times.
✗ Branch 5 not taken.
|
111327 | auto R_E = calcEarthRadius_E(y(0)); |
| 158 | LOG_DATA("R_E = {} [m]", R_E); | ||
| 159 | |||
| 160 | // ω_ie_n Turn rate of the Earth expressed in local-navigation frame coordinates | ||
| 161 |
1/2✓ Branch 2 taken 111327 times.
✗ Branch 3 not taken.
|
111327 | Eigen::Vector3<T> n_omega_ie = n_Quat_e * InsConst::e_omega_ie.cast<T>(); |
| 162 | LOG_DATA("n_omega_ie = {} [rad/s]", n_omega_ie.transpose()); | ||
| 163 | // ω_en_n Turn rate of the local frame with respect to the Earth-fixed frame, called the transport rate, expressed in local-navigation frame coordinates | ||
| 164 |
3/6✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111327 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 111327 times.
✗ Branch 8 not taken.
|
111327 | Eigen::Vector3<T> n_omega_en = n_calcTransportRate(y.template segment<3>(0), y.template segment<3>(3), R_N, R_E); |
| 165 | LOG_DATA("n_omega_en = {} [rad/s]", n_omega_en.transpose()); | ||
| 166 | // ω_nb_b = ω_ib_b - C_bn * (ω_ie_n + ω_en_n) | ||
| 167 |
6/12✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111327 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 111327 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 111327 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 111327 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 111327 times.
✗ Branch 17 not taken.
|
111327 | Eigen::Vector3<T> b_omega_nb = z.template segment<3>(3) |
| 168 | - n_Quat_b.conjugate() | ||
| 169 |
2/8✓ Branch 0 taken 111327 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 111327 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
111327 | * ((c.angularRateEarthRotationCompensationEnabled ? n_omega_ie : Eigen::Vector3<T>::Zero()) |
| 170 |
2/8✓ Branch 0 taken 111327 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 111327 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
111327 | + (c.angularRateTransportRateCompensationEnabled ? n_omega_en : Eigen::Vector3<T>::Zero())); |
| 171 | LOG_DATA("b_omega_nb = {} [rad/s]", b_omega_nb.transpose()); | ||
| 172 | |||
| 173 | // Coriolis acceleration in [m/s^2] (acceleration due to motion in rotating reference frame) | ||
| 174 |
1/6✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
111327 | Eigen::Vector3<T> n_coriolisAcceleration = c.coriolisAccelerationCompensationEnabled |
| 175 |
2/4✓ Branch 0 taken 111327 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 111327 times.
✗ Branch 4 not taken.
|
111327 | ? n_calcCoriolisAcceleration(n_omega_ie, n_omega_en, y.template segment<3>(3)) |
| 176 | : Eigen::Vector3<T>::Zero(); | ||
| 177 | LOG_DATA("n_coriolisAcceleration = {} [m/s^2]", n_coriolisAcceleration.transpose()); | ||
| 178 | // Centrifugal acceleration in [m/s^2] (acceleration that makes a body follow a curved path) | ||
| 179 |
3/10✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111327 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 111327 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
111327 | Eigen::Vector3<T> n_centrifugalAcceleration = c.centrifgalAccelerationCompensationEnabled |
| 180 |
2/4✓ Branch 0 taken 111327 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 111327 times.
✗ Branch 4 not taken.
|
111327 | ? n_Quat_e * e_calcCentrifugalAcceleration(trafo::lla2ecef_WGS84(y.template segment<3>(0)), InsConst::e_omega_ie) |
| 181 | : Eigen::Vector3<T>::Zero(); | ||
| 182 | LOG_DATA("n_centrifugalAcceleration = {} [m/s^2]", n_centrifugalAcceleration.transpose()); | ||
| 183 | |||
| 184 |
2/4✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111327 times.
✗ Branch 5 not taken.
|
111327 | Eigen::Vector3<T> n_gravitation = n_calcGravitation(y.template segment<3>(0), c.gravitationModel); |
| 185 | LOG_DATA("n_gravitation = {} [m/s^2] ({})", n_gravitation.transpose(), to_string(c.gravitationModel)); | ||
| 186 | |||
| 187 |
3/6✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111327 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 111327 times.
✗ Branch 8 not taken.
|
111327 | y_dot.template segment<3>(0) = lla_calcTimeDerivativeForPosition(y.template segment<3>(3), // Velocity with respect to the Earth in local-navigation frame coordinates [m/s] |
| 188 |
1/2✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
|
111327 | y(0), // 𝜙 Latitude in [rad] |
| 189 |
2/4✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111327 times.
✗ Branch 5 not taken.
|
111327 | y(2), // h Altitude in [m] |
| 190 | R_N, // North/South (meridian) earth radius [m] | ||
| 191 | R_E); // East/West (prime vertical) earth radius [m] | ||
| 192 | |||
| 193 |
5/10✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111327 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 111327 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 111327 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 111327 times.
✗ Branch 14 not taken.
|
111327 | y_dot.template segment<3>(3) = n_calcTimeDerivativeForVelocity(n_Quat_b * z.template segment<3>(0), // f_n Specific force vector as measured by a triad of accelerometers and resolved into local-navigation frame coordinates |
| 194 | n_coriolisAcceleration, // Coriolis acceleration in local-navigation coordinates in [m/s^2] | ||
| 195 | n_gravitation, // Local gravitation vector (caused by effects of mass attraction) in local-navigation frame coordinates [m/s^2] | ||
| 196 | n_centrifugalAcceleration); // Centrifugal acceleration in local-navigation coordinates in [m/s^2] | ||
| 197 | |||
| 198 |
2/4✓ Branch 1 taken 111327 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111327 times.
✗ Branch 5 not taken.
|
111327 | y_dot.template segment<3>(6) = b_omega_nb; // ω_nb_b Body rate with respect to the navigation frame, expressed in the body frame |
| 199 | |||
| 200 | LOG_DATA("lla_position_dot = {} [rad/s, rad/s, m/s]", y_dot.template segment<3>(0).transpose()); | ||
| 201 | LOG_DATA("n_velocity_dot = {} [m/s^2]", y_dot.template segment<3>(3).transpose()); | ||
| 202 | LOG_DATA("ω_nb_b = {} ", y_dot.template segment<3>(6).transpose()); | ||
| 203 | |||
| 204 | 222654 | return y_dot; | |
| 205 | } | ||
| 206 | |||
| 207 | } // namespace NAV | ||
| 208 |