INSTINCT Code Coverage Report

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