0.4.1
Loading...
Searching...
No Matches
Mechanization.hpp
Go to the documentation of this file.
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
28#include "util/Logger.hpp"
29
30#include <cmath>
31
32namespace 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}
51template<typename DerivedA, typename DerivedB>
52Eigen::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}
88template<typename DerivedA, typename DerivedB, typename DerivedC, typename DerivedD>
89Eigen::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 - 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}
118template<typename Derived>
119Eigen::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 const auto& v_N = n_velocity(0);
125 // Velocity East in [m/s]
126 const auto& v_E = n_velocity(1);
127 // Velocity Down in [m/s]
128 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 -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, n_q_bx, n_q_by, n_q_bz, n_q_bw]^T
140template<typename T>
141Eigen::Vector<T, 10> 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 9
144 // ∂/∂t [ 𝜙, λ, h, v_N, v_E, v_D, n_q_bx, n_q_by, n_q_bz, n_q_bw]^T
145 Eigen::Vector<T, 10> y_dot = Eigen::Vector<T, 10>::Zero();
146
147 Eigen::Quaternion<T> n_Quat_b{ y.template segment<4>(6) };
148 n_Quat_b.normalize();
149 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 auto R_N = calcEarthRadius_N(y(0));
156 LOG_DATA("R_N = {} [m]", R_N);
157 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 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 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 Eigen::Vector3<T> b_omega_nb = z.template segment<3>(3)
168 - n_Quat_b.conjugate()
169 * ((c.angularRateEarthRotationCompensationEnabled ? n_omega_ie : Eigen::Vector3<T>::Zero())
170 + (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 Eigen::Vector3<T> n_coriolisAcceleration = c.coriolisAccelerationCompensationEnabled
175 ? 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 Eigen::Vector3<T> n_centrifugalAcceleration = c.centrifgalAccelerationCompensationEnabled
180 ? 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 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 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 y(0), // 𝜙 Latitude in [rad]
189 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 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 y_dot.template segment<4>(6) = calcTimeDerivativeFor_n_Quat_b(b_omega_nb, // ω_nb_b Body rate with respect to the navigation frame, expressed in the body frame
199 y.template segment<4>(6)); // n_Quat_b_coeffs Coefficients of the quaternion n_Quat_b
200
201 LOG_DATA("lla_position_dot = {} [rad/s, rad/s, m/s]", y_dot.template segment<3>(0).transpose());
202 LOG_DATA("n_velocity_dot = {} [m/s^2]", y_dot.template segment<3>(3).transpose());
203 LOG_DATA("n_Quat_b_dot = {} ", y_dot.template segment<4>(6).transpose());
204
205 return y_dot;
206}
207
208} // namespace NAV
Holds all Constants.
Transformation collection.
Functions concerning the ellipsoid model.
Different Gravity Models.
Inertial Navigation Helper Functions.
Utility class for logging to console and file.
#define LOG_DATA
All output which occurs repeatedly every time observations are received.
Definition Logger.hpp:29
Simple Math functions.
Inertial Navigation Mechanization Functions.
Provides Numerical integration methods.
static const Eigen::Vector3< double > e_omega_ie
ω_ie_e = ω_ie_i Nominal mean angular velocity of the Earth in [rad/s], in earth coordinates
Eigen::Quaternion< Scalar > n_Quat_e(const Scalar &latitude, const Scalar &longitude)
Quaternion for rotations from Earth-fixed to navigation frame.
Eigen::Vector3< typename Derived::Scalar > quat2eulerZYX(const Eigen::QuaternionBase< Derived > &q)
Converts the quaternion to Euler rotation angles with rotation sequence ZYX.
Eigen::Vector3< typename Derived::Scalar > lla2ecef_WGS84(const Eigen::MatrixBase< Derived > &lla_position)
Converts latitude, longitude and altitude into Earth-centered-Earth-fixed coordinates using WGS84.
Eigen::Vector3< typename DerivedA::Scalar > n_calcTimeDerivativeForVelocity(const Eigen::MatrixBase< DerivedA > &n_measuredForce, const Eigen::MatrixBase< DerivedB > &n_coriolisAcceleration, const Eigen::MatrixBase< DerivedC > &n_gravitation, const Eigen::MatrixBase< DerivedD > &n_centrifugalAcceleration)
Calculates the time derivative of the velocity in local-navigation frame coordinates.
Eigen::Vector3< typename Derived::Scalar > n_calcGravitation(const Eigen::MatrixBase< Derived > &lla_position, GravitationModel gravitationModel=GravitationModel::EGM96)
Calculates the gravitation (acceleration due to mass attraction of the Earth)
Definition Gravity.hpp:193
Eigen::Vector3< typename DerivedA::Scalar > e_calcCentrifugalAcceleration(const Eigen::MatrixBase< DerivedA > &e_position, const Eigen::MatrixBase< DerivedB > &e_omega_ie=InsConst::e_omega_ie)
Calculates the centrifugal acceleration in [m/s^2] (acceleration that makes a body follow a curved pa...
Definition Functions.hpp:72
Eigen::Vector3< typename DerivedA::Scalar > n_calcCoriolisAcceleration(const Eigen::MatrixBase< DerivedA > &n_omega_ie, const Eigen::MatrixBase< DerivedB > &n_omega_en, const Eigen::MatrixBase< DerivedC > &n_velocity)
Calculates the coriolis acceleration in [m/s^2] (acceleration due to motion in rotating reference fra...
Definition Functions.hpp:90
Scalar calcEarthRadius_N(const Scalar &latitude, const Scalar &a=InsConst::WGS84::a, const Scalar &e_squared=InsConst::WGS84::e_squared)
Calculates the North/South (meridian) earth radius.
Definition Ellipsoid.hpp:42
const char * to_string(gui::widgets::PositionWithFrame::ReferenceFrame refFrame)
Converts the enum to a string.
Eigen::Vector3< typename DerivedA::Scalar > n_calcTransportRate(const Eigen::MatrixBase< DerivedA > &lla_position, const Eigen::MatrixBase< DerivedB > &n_velocity, const auto &R_N, const auto &R_E)
Calculates the transport rate ω_en_n (rotation rate of the Earth frame relative to the navigation fra...
Definition Functions.hpp:39
Eigen::Vector4< typename DerivedA::Scalar > calcTimeDerivativeFor_n_Quat_b(const Eigen::MatrixBase< DerivedA > &b_omega_nb, const Eigen::MatrixBase< DerivedB > &n_Quat_b_coeffs)
Calculates the time derivative of the quaternion n_Quat_b.
Eigen::Vector< T, 10 > n_calcPosVelAttDerivative(const Eigen::Vector< T, 10 > &y, const Eigen::Vector< T, 6 > &z, const PosVelAttDerivativeConstants &c, double=0.0)
Calculates the derivative of the quaternion, velocity and curvilinear position.
Eigen::Vector3< typename Derived::Scalar > lla_calcTimeDerivativeForPosition(const Eigen::MatrixBase< Derived > &n_velocity, const auto &phi, const auto &h, const auto &R_N, const auto &R_E)
Calculates the time derivative of the curvilinear position.
constexpr auto rad2deg(const T &rad)
Convert Radians to Degree.
Definition Units.hpp:39
Scalar calcEarthRadius_E(const Scalar &latitude, const Scalar &a=InsConst::WGS84::a, const Scalar &e_squared=InsConst::WGS84::e_squared)
Calculates the East/West (prime vertical) earth radius.
Definition Ellipsoid.hpp:58
Values needed to calculate the PosVelAttDerivative for the local-navigation frame.
bool centrifgalAccelerationCompensationEnabled
Apply the centrifugal acceleration compensation to the measured accelerations.
bool angularRateEarthRotationCompensationEnabled
Apply the Earth rotation rate compensation to the measured angular rates.
bool angularRateTransportRateCompensationEnabled
Apply the transport rate compensation to the measured angular rates.
GravitationModel gravitationModel
Gravity Model to use.
bool coriolisAccelerationCompensationEnabled
Apply the Coriolis acceleration compensation to the measured accelerations.