INSTINCT Code Coverage Report

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