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 ECEF frame
11/// @author T. Topp (topp@ins.uni-stuttgart.de)
12/// @date 2022-06-12
13
14#pragma once
15
17
18#include <Eigen/Core>
19#include <Eigen/Dense>
20
27#include "util/Logger.hpp"
28
29#include <cmath>
30
31namespace 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}
50template<typename DerivedA, typename DerivedB>
51Eigen::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}
87template<typename DerivedA, typename DerivedB, typename DerivedC, typename DerivedD>
88Eigen::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 - 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}
109template<typename Derived>
110Eigen::Vector3<typename Derived::Scalar> e_calcTimeDerivativeForPosition(const Eigen::MatrixBase<Derived>& e_velocity)
111{
112 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, e_q_bx, e_q_by, e_q_bz, e_q_bw]^T
120template<typename T>
121Eigen::Vector<T, 10> 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 9
124 // ∂/∂t [ x, y, z, v_x, v_y, v_z, e_q_bx, e_q_by, e_q_bz, e_q_bw]^T
125 Eigen::Vector<T, 10> y_dot = Eigen::Vector<T, 10>::Zero();
126
127 Eigen::Vector3<T> lla_position = trafo::ecef2lla_WGS84(y.template segment<3>(0));
128
129 Eigen::Quaternion<T> e_Quat_b{ y.template segment<4>(6) };
130 e_Quat_b.normalize();
131 Eigen::Quaternion<T> n_Quat_e = trafo::n_Quat_e(lla_position(0), lla_position(1));
132
133 Eigen::Quaternion<T> b_Quat_e = e_Quat_b.conjugate();
134 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 Eigen::Vector3<T> b_omega_eb = z.template segment<3>(3)
141 - 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 Eigen::Vector3<T> e_coriolisAcceleration = c.coriolisAccelerationCompensationEnabled
146 ? 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 Eigen::Vector3<T> e_centrifugalAcceleration = c.centrifgalAccelerationCompensationEnabled
151 ? 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 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 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 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 y_dot.template segment<4>(6) = calcTimeDerivativeFor_e_Quat_b(b_omega_eb, // ω_eb_b Body rate with respect to the ECEF frame, expressed in the body frame
166 y.template segment<4>(6)); // e_Quat_b_coeffs Coefficients of the quaternion e_Quat_b
167
168 LOG_DATA("e_position_dot = {} [m/s]", y_dot.template segment<3>(0).transpose());
169 LOG_DATA("e_velocity_dot = {} [m/s^2]", y_dot.template segment<3>(3).transpose());
170 LOG_DATA("e_Quat_b_dot = {} ", y_dot.template segment<4>(6).transpose());
171
172 return y_dot;
173}
174
175} // 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.
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::Vector3< typename Derived::Scalar > ecef2lla_WGS84(const Eigen::MatrixBase< Derived > &e_position)
Converts Earth-centered-Earth-fixed coordinates into latitude, longitude and altitude using WGS84.
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 > 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::Vector4< typename DerivedA::Scalar > calcTimeDerivativeFor_e_Quat_b(const Eigen::MatrixBase< DerivedA > &b_omega_eb, const Eigen::MatrixBase< DerivedB > &e_Quat_b_coeffs)
Calculates the time derivative of the quaternion e_Quat_b.
const char * to_string(gui::widgets::PositionWithFrame::ReferenceFrame refFrame)
Converts the enum to a string.
Eigen::Vector3< typename DerivedB::Scalar > e_calcCoriolisAcceleration(const Eigen::MatrixBase< DerivedA > &e_omega_ie, const Eigen::MatrixBase< DerivedB > &e_velocity)
Calculates the coriolis acceleration in [m/s^2] (acceleration due to motion in rotating reference fra...
Eigen::Vector< T, 10 > e_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 position in ECEF coordinates.
Eigen::Vector3< typename DerivedA::Scalar > e_calcTimeDerivativeForVelocity(const Eigen::MatrixBase< DerivedA > &e_measuredForce, const Eigen::MatrixBase< DerivedB > &e_coriolisAcceleration, const Eigen::MatrixBase< DerivedC > &e_gravitation, const Eigen::MatrixBase< DerivedD > &e_centrifugalAcceleration)
Calculates the time derivative of the velocity in ECEF frame coordinates.
Eigen::Vector3< typename Derived::Scalar > e_calcTimeDerivativeForPosition(const Eigen::MatrixBase< Derived > &e_velocity)
Calculates the time derivative of the ECEF position.
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.
GravitationModel gravitationModel
Gravity Model to use.
bool coriolisAccelerationCompensationEnabled
Apply the Coriolis acceleration compensation to the measured accelerations.