| 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 ErrorEquations.hpp | ||
| 10 | /// @brief Error Equations for the ECEF frame | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2022-06-12 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <Eigen/Core> | ||
| 17 | |||
| 18 | #include "Navigation/Constants.hpp" | ||
| 19 | #include "Navigation/Math/Math.hpp" | ||
| 20 | #include "Navigation/GNSS/SystemModel/MotionModel.hpp" | ||
| 21 | #include "Navigation/INS/Keys.hpp" | ||
| 22 | #include "util/Container/KeyedMatrix.hpp" | ||
| 23 | |||
| 24 | namespace NAV | ||
| 25 | { | ||
| 26 | /// @brief Calculates the matrix 𝐅_𝜓'_𝜓 | ||
| 27 | /// @param[in] omega_ie Angular velocity of the Earth in [rad/s] | ||
| 28 | /// @return 3x3 matrix in [rad / s] | ||
| 29 | /// @note See \cite Groves2013 Groves, ch. 14.2.3, eq. 14.48, p. 583 - 𝐅_11 | ||
| 30 | template<typename T> | ||
| 31 | 12888 | [[nodiscard]] Eigen::Matrix3<T> e_F_dpsi_dpsi(const T& omega_ie) | |
| 32 | { | ||
| 33 | // Math: \mathbf{F}_{11}^e = -\mathbf{\Omega}_{ie}^e | ||
| 34 | 12888 | Eigen::Matrix3<T> e_F_11; | |
| 35 | // clang-format off | ||
| 36 |
4/8✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12888 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12888 times.
✗ Branch 11 not taken.
|
12888 | e_F_11 << 0 , omega_ie, 0, |
| 37 |
3/6✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12888 times.
✗ Branch 8 not taken.
|
12888 | -omega_ie, 0 , 0, |
| 38 |
2/4✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
|
12888 | 0 , 0 , 0; |
| 39 | // clang-format on | ||
| 40 | 12888 | return e_F_11; | |
| 41 | } | ||
| 42 | |||
| 43 | /// @brief Calculates the matrix 𝐅_𝜓'_𝛿ω | ||
| 44 | /// @param[in] e_Dcm_b DCM from body to Earth frame | ||
| 45 | /// @return 3x3 matrix in [-] | ||
| 46 | /// @note See \cite Groves2013 Groves, ch. 14.2.3, eq. 14.48, p. 583 - 𝐅_15 | ||
| 47 | template<typename Derived> | ||
| 48 | 12888 | [[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 3, 3> e_F_dpsi_dw(const Eigen::MatrixBase<Derived>& e_Dcm_b) | |
| 49 | { | ||
| 50 | 12888 | return e_Dcm_b; | |
| 51 | } | ||
| 52 | |||
| 53 | /// @brief Calculates the matrix 𝐅_𝛿v'_𝜓 | ||
| 54 | /// @param[in] e_force_ib Specific force of the body with respect to inertial frame in [m / s^2], resolved in Earth frame coordinates | ||
| 55 | /// @return 3x3 matrix in [m / s^2] | ||
| 56 | /// @note See \cite Groves2013 Groves, ch. 14.2.3, eq. 14.49, p. 584 - 𝐅_21 | ||
| 57 | template<typename Derived> | ||
| 58 | 12888 | [[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 3, 3> e_F_dv_dpsi(const Eigen::MatrixBase<Derived>& e_force_ib) | |
| 59 | { | ||
| 60 | // Math: \mathbf{F}_{21}^e = \begin{bmatrix} (\mathbf{C}_{b}^e \hat{f}_{ib}^b) \land \end{bmatrix} | ||
| 61 | 12888 | Eigen::Matrix<typename Derived::Scalar, 3, 3> e_F_21; | |
| 62 | // clang-format off | ||
| 63 |
5/10✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12888 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12888 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12888 times.
✗ Branch 14 not taken.
|
12888 | e_F_21 << 0 , e_force_ib.z(), -e_force_ib.y(), |
| 64 |
5/10✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12888 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12888 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12888 times.
✗ Branch 14 not taken.
|
12888 | -e_force_ib.z(), 0 , e_force_ib.x(), |
| 65 |
5/10✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12888 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12888 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12888 times.
✗ Branch 14 not taken.
|
12888 | e_force_ib.y(), -e_force_ib.x(), 0 ; |
| 66 | // clang-format on | ||
| 67 | 12888 | return e_F_21; | |
| 68 | } | ||
| 69 | |||
| 70 | /// @brief Calculates the matrix 𝐅_𝛿v'_𝛿v | ||
| 71 | /// @param[in] omega_ie omega_ie Angular velocity of the Earth in [rad/s] | ||
| 72 | /// @return 3x3 matrix in [1 / s] | ||
| 73 | /// @note See \cite Groves2013 Groves, ch. 14.2.3, eq. 14.48, p. 583 - 𝐅_22 | ||
| 74 | template<typename T> | ||
| 75 | 12888 | [[nodiscard]] Eigen::Matrix3<T> e_F_dv_dv(double omega_ie) | |
| 76 | { | ||
| 77 | // Math: \mathbf{F}_{22}^e = -2\mathbf{\Omega}_{ie}^e | ||
| 78 |
1/2✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
|
12888 | Eigen::Matrix3d e_F_22; |
| 79 | // clang-format off | ||
| 80 |
3/6✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12888 times.
✗ Branch 8 not taken.
|
12888 | e_F_22 << 0.0 , 2.0 * omega_ie, 0.0, |
| 81 |
3/6✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12888 times.
✗ Branch 8 not taken.
|
12888 | -2.0 * omega_ie, 0.0 , 0.0, |
| 82 |
3/6✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12888 times.
✗ Branch 8 not taken.
|
12888 | 0.0 , 0.0 , 0.0; |
| 83 | // clang-format on | ||
| 84 |
1/2✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
|
25776 | return e_F_22.cast<T>(); |
| 85 | } | ||
| 86 | |||
| 87 | /// @brief Calculates the matrix 𝐅_𝛿v'_𝛿r | ||
| 88 | /// @param[in] e_position Position in ECEF coordinates in [m] | ||
| 89 | /// @param[in] e_gravitation Gravitational acceleration in [m/s^2] | ||
| 90 | /// @param[in] r_eS_e Geocentric radius. The distance of a point on the Earth's surface from the center of the Earth in [m] | ||
| 91 | /// @param[in] e_omega_ie Angular velocity of Earth with respect to inertial system, represented in e-sys in [rad/s] | ||
| 92 | /// @return 3x3 matrix in [1 / s^2] | ||
| 93 | /// @note See \cite Groves2013 Groves, ch. 14.2.3, eq. 14.49, p. 584 - 𝐅_23 | ||
| 94 | template<typename Derived1, typename Derived2, typename T> | ||
| 95 | 12888 | [[nodiscard]] Eigen::Matrix<typename Derived1::Scalar, 3, 3> e_F_dv_dr(const Eigen::MatrixBase<Derived1>& e_position, | |
| 96 | const Eigen::MatrixBase<Derived2>& e_gravitation, | ||
| 97 | const T& r_eS_e, | ||
| 98 | const Eigen::Vector3d& e_omega_ie) | ||
| 99 | { | ||
| 100 |
1/2✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
|
12888 | Eigen::Matrix3d e_Omega_ie = math::skewSymmetricMatrix(e_omega_ie); |
| 101 | |||
| 102 | // Math: \mathbf{F}_{23}^e = -( \dfrac{2 \boldsymbol{\gamma}_{ib}^e}{r_{eS}^e} \dfrac{{\mathbf{r}_{eb}^e}^T}{\left| \mathbf{r}_{eb}^e \right|} + \boldsymbol{\Omega}_{ie}^e \boldsymbol{\Omega}_{ie}^e ) | ||
| 103 |
9/18✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12888 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12888 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12888 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 12888 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 12888 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 12888 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 12888 times.
✗ Branch 26 not taken.
|
12888 | Eigen::Matrix<typename Derived1::Scalar, 3, 3> e_F_23 = -((2.0 * e_gravitation * e_position.transpose()) / (r_eS_e * e_position.norm()) + e_Omega_ie * e_Omega_ie); |
| 104 | |||
| 105 | 25776 | return e_F_23; | |
| 106 | } | ||
| 107 | |||
| 108 | /// @brief Calculate the linearized velocity differential equation partially derived for the attitude | ||
| 109 | /// @param p_accelBias Acceleration bias in platform frame coordinates [m/s^2] | ||
| 110 | /// @param p_accelScale Acceleration scale factor in platform frame coordinates [m/s^2] | ||
| 111 | /// @param p_quatAccel_ps Accelerometer misalignment | ||
| 112 | /// @param ps_f_ip Accelerometer measurement [m/s^2] | ||
| 113 | /// @param b_quat_p Quaternion from IMU platform frame to body frame | ||
| 114 | /// @param e_quat_b Quaternion from body to Earth frame | ||
| 115 | template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5, typename Derived6> | ||
| 116 | Eigen::Matrix<typename Derived6::Scalar, 3, 4> e_F_dv_dq(const Eigen::MatrixBase<Derived1>& p_accelBias, | ||
| 117 | const Eigen::MatrixBase<Derived2>& p_accelScale, | ||
| 118 | const Eigen::QuaternionBase<Derived3>& p_quatAccel_ps, | ||
| 119 | const Eigen::MatrixBase<Derived4>& ps_f_ip, | ||
| 120 | const Eigen::QuaternionBase<Derived5>& b_quat_p, | ||
| 121 | const Eigen::QuaternionBase<Derived6>& e_quat_b) | ||
| 122 | { | ||
| 123 | Eigen::Vector3<typename Derived6::Scalar> b_f_ip = b_quat_p.template cast<typename Derived6::Scalar>() | ||
| 124 | * (p_accelScale.asDiagonal() * p_quatAccel_ps * ps_f_ip.template cast<typename Derived6::Scalar>() + p_accelBias); | ||
| 125 | |||
| 126 | Eigen::Matrix<typename Derived6::Scalar, 3, 4> F_dv_dq; | ||
| 127 | |||
| 128 | F_dv_dq(0, 0) = 2.0 * (b_f_ip.x() * e_quat_b.x() + b_f_ip.y() * e_quat_b.y() + b_f_ip.z() * e_quat_b.z()); | ||
| 129 | F_dv_dq(0, 1) = -2.0 * (b_f_ip.x() * e_quat_b.y() - b_f_ip.y() * e_quat_b.x() - b_f_ip.z() * e_quat_b.w()); | ||
| 130 | F_dv_dq(0, 2) = -2.0 * (b_f_ip.x() * e_quat_b.z() + b_f_ip.y() * e_quat_b.w() - b_f_ip.z() * e_quat_b.x()); | ||
| 131 | F_dv_dq(0, 3) = 2.0 * (b_f_ip.x() * e_quat_b.w() - b_f_ip.y() * e_quat_b.z() + b_f_ip.z() * e_quat_b.y()); | ||
| 132 | |||
| 133 | F_dv_dq(1, 0) = -F_dv_dq(0, 1); | ||
| 134 | F_dv_dq(1, 1) = F_dv_dq(0, 0); | ||
| 135 | F_dv_dq(1, 2) = F_dv_dq(0, 3); | ||
| 136 | F_dv_dq(1, 3) = -F_dv_dq(0, 2); | ||
| 137 | |||
| 138 | F_dv_dq(2, 0) = -F_dv_dq(0, 2); | ||
| 139 | F_dv_dq(2, 1) = -F_dv_dq(0, 3); | ||
| 140 | F_dv_dq(2, 2) = F_dv_dq(0, 0); | ||
| 141 | F_dv_dq(2, 3) = F_dv_dq(0, 1); | ||
| 142 | |||
| 143 | return F_dv_dq; | ||
| 144 | } | ||
| 145 | |||
| 146 | /// @brief Calculates the matrix 𝐅_𝜓'_𝛿f_b | ||
| 147 | /// @param[in] e_Dcm_b DCM from body to Earth frame | ||
| 148 | /// @return 3x3 matrix in [-] | ||
| 149 | /// @note See \cite Groves2013 Groves, ch. 14.2.3, eq. 14.48, p. 583 - 𝐅_24 | ||
| 150 | template<typename Derived> | ||
| 151 | 12888 | [[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 3, 3> e_F_dv_df_b(const Eigen::MatrixBase<Derived>& e_Dcm_b) | |
| 152 | { | ||
| 153 | 12888 | return e_Dcm_b; | |
| 154 | } | ||
| 155 | |||
| 156 | /// @brief Calculates the matrix 𝐅_𝜓'_𝛿f_p | ||
| 157 | /// @param[in] b_quat_p Quaternion from IMU platform frame to body frame | ||
| 158 | /// @param[in] e_quat_b Quaternion from body to Earth frame | ||
| 159 | /// @return 3x3 matrix in [-] | ||
| 160 | template<typename Derived1, typename Derived2> | ||
| 161 | [[nodiscard]] Eigen::Matrix<typename Derived2::Scalar, 3, 3> e_F_dv_dAccelBias(const Eigen::QuaternionBase<Derived1>& b_quat_p, const Eigen::QuaternionBase<Derived2>& e_quat_b) | ||
| 162 | { | ||
| 163 | return (e_quat_b * b_quat_p.template cast<typename Derived2::Scalar>()).toRotationMatrix(); | ||
| 164 | } | ||
| 165 | |||
| 166 | /// @brief Calculate the linearized velocity differential equation partially derived for the acceleration bias | ||
| 167 | /// @param p_quatAccel_ps Accelerometer misalignment | ||
| 168 | /// @param ps_f_ip Accelerometer measurement [m/s^2] | ||
| 169 | /// @param b_quat_p Quaternion from IMU platform frame to body frame | ||
| 170 | /// @param e_quat_b Quaternion from body to Earth frame | ||
| 171 | template<typename Derived1, typename Derived2, typename Derived3, typename Derived4> | ||
| 172 | [[nodiscard]] Eigen::Matrix<typename Derived4::Scalar, 3, 3> e_F_dv_dAccelScale(const Eigen::QuaternionBase<Derived1>& p_quatAccel_ps, | ||
| 173 | const Eigen::MatrixBase<Derived2>& ps_f_ip, | ||
| 174 | const Eigen::QuaternionBase<Derived3>& b_quat_p, | ||
| 175 | const Eigen::QuaternionBase<Derived4>& e_quat_b) | ||
| 176 | { | ||
| 177 | return e_quat_b | ||
| 178 | * b_quat_p.template cast<typename Derived4::Scalar>() | ||
| 179 | * p_quatAccel_ps | ||
| 180 | * ps_f_ip.template cast<typename Derived4::Scalar>().asDiagonal(); | ||
| 181 | } | ||
| 182 | |||
| 183 | /// @brief Calculate the linearized velocity differential equation partially derived for the accelerometer misalignment | ||
| 184 | /// @param p_accelScale Acceleration scale factor in platform frame coordinates [m/s^2] | ||
| 185 | /// @param p_quatAccel_ps Accelerometer misalignment | ||
| 186 | /// @param ps_f_ip Accelerometer measurement [m/s^2] | ||
| 187 | /// @param b_quat_p Quaternion from IMU platform frame to body frame | ||
| 188 | /// @param e_quat_b Quaternion from body to Earth frame | ||
| 189 | template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5> | ||
| 190 | [[nodiscard]] Eigen::Matrix<typename Derived5::Scalar, 3, 4> e_F_dv_dAccelMisalignment(const Eigen::MatrixBase<Derived1>& p_accelScale, | ||
| 191 | const Eigen::QuaternionBase<Derived2>& p_quatAccel_ps, | ||
| 192 | const Eigen::MatrixBase<Derived3>& ps_f_ip, | ||
| 193 | const Eigen::QuaternionBase<Derived4>& b_quat_p, | ||
| 194 | const Eigen::QuaternionBase<Derived5>& e_quat_b) | ||
| 195 | { | ||
| 196 | Eigen::Matrix3<typename Derived5::Scalar> e_dcmAccelScaled_p = e_quat_b * b_quat_p.template cast<typename Derived5::Scalar>() * p_accelScale.asDiagonal(); | ||
| 197 | |||
| 198 | auto a = p_quatAccel_ps.w() * e_dcmAccelScaled_p(0, 2) + p_quatAccel_ps.y() * e_dcmAccelScaled_p(0, 0) - p_quatAccel_ps.x() * e_dcmAccelScaled_p(0, 1); | ||
| 199 | auto b = p_quatAccel_ps.w() * e_dcmAccelScaled_p(1, 2) + p_quatAccel_ps.y() * e_dcmAccelScaled_p(1, 0) - p_quatAccel_ps.x() * e_dcmAccelScaled_p(1, 1); | ||
| 200 | auto c = p_quatAccel_ps.w() * e_dcmAccelScaled_p(2, 2) + p_quatAccel_ps.y() * e_dcmAccelScaled_p(2, 0) - p_quatAccel_ps.x() * e_dcmAccelScaled_p(2, 1); | ||
| 201 | auto d = p_quatAccel_ps.w() * e_dcmAccelScaled_p(0, 1) + p_quatAccel_ps.x() * e_dcmAccelScaled_p(0, 2) - p_quatAccel_ps.z() * e_dcmAccelScaled_p(0, 0); | ||
| 202 | auto e = p_quatAccel_ps.w() * e_dcmAccelScaled_p(1, 1) + p_quatAccel_ps.x() * e_dcmAccelScaled_p(1, 2) - p_quatAccel_ps.z() * e_dcmAccelScaled_p(1, 0); | ||
| 203 | auto f = p_quatAccel_ps.w() * e_dcmAccelScaled_p(2, 1) + p_quatAccel_ps.x() * e_dcmAccelScaled_p(2, 2) - p_quatAccel_ps.z() * e_dcmAccelScaled_p(2, 0); | ||
| 204 | auto g = p_quatAccel_ps.w() * e_dcmAccelScaled_p(0, 0) + p_quatAccel_ps.z() * e_dcmAccelScaled_p(0, 1) - p_quatAccel_ps.y() * e_dcmAccelScaled_p(0, 2); | ||
| 205 | auto h = p_quatAccel_ps.w() * e_dcmAccelScaled_p(1, 0) + p_quatAccel_ps.z() * e_dcmAccelScaled_p(1, 1) - p_quatAccel_ps.y() * e_dcmAccelScaled_p(1, 2); | ||
| 206 | auto i = p_quatAccel_ps.w() * e_dcmAccelScaled_p(2, 0) + p_quatAccel_ps.z() * e_dcmAccelScaled_p(2, 1) - p_quatAccel_ps.y() * e_dcmAccelScaled_p(2, 2); | ||
| 207 | auto j = p_quatAccel_ps.x() * e_dcmAccelScaled_p(0, 0) + p_quatAccel_ps.y() * e_dcmAccelScaled_p(0, 1) + p_quatAccel_ps.z() * e_dcmAccelScaled_p(0, 2); | ||
| 208 | auto k = p_quatAccel_ps.x() * e_dcmAccelScaled_p(1, 0) + p_quatAccel_ps.y() * e_dcmAccelScaled_p(1, 1) + p_quatAccel_ps.z() * e_dcmAccelScaled_p(1, 2); | ||
| 209 | auto l = p_quatAccel_ps.x() * e_dcmAccelScaled_p(2, 0) + p_quatAccel_ps.y() * e_dcmAccelScaled_p(2, 1) + p_quatAccel_ps.z() * e_dcmAccelScaled_p(2, 2); | ||
| 210 | |||
| 211 | Eigen::Matrix<typename Derived5::Scalar, 3, 4> e_F_dv_dAccelMisalignment; | ||
| 212 | |||
| 213 | e_F_dv_dAccelMisalignment(0, 0) = 2.0 * (j * ps_f_ip.x() + a * ps_f_ip.y() - d * ps_f_ip.z()); | ||
| 214 | e_F_dv_dAccelMisalignment(0, 1) = -2.0 * (a * ps_f_ip.x() - j * ps_f_ip.y() - g * ps_f_ip.z()); | ||
| 215 | e_F_dv_dAccelMisalignment(0, 2) = 2.0 * (d * ps_f_ip.x() - g * ps_f_ip.y() + j * ps_f_ip.z()); | ||
| 216 | e_F_dv_dAccelMisalignment(0, 3) = 2.0 * (g * ps_f_ip.x() + d * ps_f_ip.y() + a * ps_f_ip.z()); | ||
| 217 | |||
| 218 | e_F_dv_dAccelMisalignment(1, 0) = 2.0 * (k * ps_f_ip.x() + b * ps_f_ip.y() - e * ps_f_ip.z()); | ||
| 219 | e_F_dv_dAccelMisalignment(1, 1) = -2.0 * (b * ps_f_ip.x() - k * ps_f_ip.y() - h * ps_f_ip.z()); | ||
| 220 | e_F_dv_dAccelMisalignment(1, 2) = 2.0 * (e * ps_f_ip.x() - h * ps_f_ip.y() + k * ps_f_ip.z()); | ||
| 221 | e_F_dv_dAccelMisalignment(1, 3) = 2.0 * (h * ps_f_ip.x() + e * ps_f_ip.y() + b * ps_f_ip.z()); | ||
| 222 | |||
| 223 | e_F_dv_dAccelMisalignment(2, 0) = 2.0 * (l * ps_f_ip.x() + c * ps_f_ip.y() - f * ps_f_ip.z()); | ||
| 224 | e_F_dv_dAccelMisalignment(2, 1) = -2.0 * (c * ps_f_ip.x() - l * ps_f_ip.y() - i * ps_f_ip.z()); | ||
| 225 | e_F_dv_dAccelMisalignment(2, 2) = 2.0 * (f * ps_f_ip.x() - i * ps_f_ip.y() + l * ps_f_ip.z()); | ||
| 226 | e_F_dv_dAccelMisalignment(2, 3) = 2.0 * (i * ps_f_ip.x() + f * ps_f_ip.y() + c * ps_f_ip.z()); | ||
| 227 | |||
| 228 | return e_F_dv_dAccelMisalignment; | ||
| 229 | } | ||
| 230 | |||
| 231 | /// @brief Calculate the linearized attitude quaternion differential equation partially derived for the attitude quaternion | ||
| 232 | /// @param p_gyroBias Angular rate bias in platform frame coordinates [rad/s] | ||
| 233 | /// @param p_gyroScale Angular rate scale factor in platform frame coordinates [rad/s] | ||
| 234 | /// @param p_quatGyro_ps Gyroscope misalignment | ||
| 235 | /// @param ps_omega_ip Gyroscope measurement [m/s^2] | ||
| 236 | /// @param b_quat_p Quaternion from IMU platform frame to body frame | ||
| 237 | /// @param e_quat_b Quaternion from body to Earth frame | ||
| 238 | /// @param omega_ie Earth rotation rate [rad/s] | ||
| 239 | template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5, typename Derived6> | ||
| 240 | [[nodiscard]] Eigen::Matrix4<typename Derived6::Scalar> e_F_dq_dq(const Eigen::MatrixBase<Derived1>& p_gyroBias, | ||
| 241 | const Eigen::MatrixBase<Derived2>& p_gyroScale, | ||
| 242 | const Eigen::QuaternionBase<Derived3>& p_quatGyro_ps, | ||
| 243 | const Eigen::MatrixBase<Derived4>& ps_omega_ip, | ||
| 244 | const Eigen::QuaternionBase<Derived5>& b_quat_p, | ||
| 245 | const Eigen::QuaternionBase<Derived6>& e_quat_b, | ||
| 246 | double omega_ie) | ||
| 247 | { | ||
| 248 | Eigen::Vector3<typename Derived6::Scalar> b_omega_ib = b_quat_p.template cast<typename Derived6::Scalar>() | ||
| 249 | * (p_gyroScale.asDiagonal() * p_quatGyro_ps * ps_omega_ip.template cast<typename Derived6::Scalar>() | ||
| 250 | + p_gyroBias); | ||
| 251 | |||
| 252 | Eigen::Matrix4<typename Derived6::Scalar> F_dq_dq; | ||
| 253 | |||
| 254 | F_dq_dq(0, 0) = omega_ie * e_quat_b.x() * e_quat_b.y(); | ||
| 255 | F_dq_dq(0, 1) = 0.5 * b_omega_ib.z() + 0.5 * omega_ie * std::pow(e_quat_b.w(), 2) + 0.5 * omega_ie * std::pow(e_quat_b.x(), 2) + 0.5 * omega_ie * std::pow(e_quat_b.z(), 2) + 1.5 * omega_ie * std::pow(e_quat_b.y(), 2); | ||
| 256 | F_dq_dq(0, 2) = -0.5 * b_omega_ib.y() + omega_ie * e_quat_b.y() * e_quat_b.z(); | ||
| 257 | F_dq_dq(0, 3) = 0.5 * b_omega_ib.x() + omega_ie * e_quat_b.w() * e_quat_b.y(); | ||
| 258 | |||
| 259 | F_dq_dq(1, 0) = -0.5 * b_omega_ib.z() - 0.5 * omega_ie * std::pow(e_quat_b.w(), 2) - 0.5 * omega_ie * std::pow(e_quat_b.y(), 2) - 0.5 * omega_ie * std::pow(e_quat_b.z(), 2) - 1.5 * omega_ie * std::pow(e_quat_b.x(), 2); | ||
| 260 | F_dq_dq(1, 1) = -F_dq_dq(0, 0); | ||
| 261 | F_dq_dq(1, 2) = 0.5 * b_omega_ib.x() - omega_ie * e_quat_b.x() * e_quat_b.z(); | ||
| 262 | F_dq_dq(1, 3) = 0.5 * b_omega_ib.y() - omega_ie * e_quat_b.w() * e_quat_b.x(); | ||
| 263 | |||
| 264 | F_dq_dq(2, 0) = F_dq_dq(1, 3); | ||
| 265 | F_dq_dq(2, 1) = -0.5 * b_omega_ib.x() - omega_ie * e_quat_b.w() * e_quat_b.y(); | ||
| 266 | F_dq_dq(2, 2) = -omega_ie * e_quat_b.w() * e_quat_b.z(); | ||
| 267 | F_dq_dq(2, 3) = 0.5 * b_omega_ib.z() - 0.5 * omega_ie * std::pow(e_quat_b.x(), 2) - 0.5 * omega_ie * std::pow(e_quat_b.y(), 2) - 0.5 * omega_ie * std::pow(e_quat_b.z(), 2) - 1.5 * omega_ie * std::pow(e_quat_b.w(), 2); | ||
| 268 | |||
| 269 | F_dq_dq(3, 0) = -0.5 * b_omega_ib.x() + omega_ie * e_quat_b.x() * e_quat_b.z(); | ||
| 270 | F_dq_dq(3, 1) = F_dq_dq(0, 2); | ||
| 271 | F_dq_dq(3, 2) = -0.5 * b_omega_ib.z() + 0.5 * omega_ie * std::pow(e_quat_b.w(), 2) + 0.5 * omega_ie * std::pow(e_quat_b.x(), 2) + 0.5 * omega_ie * std::pow(e_quat_b.y(), 2) + 1.5 * omega_ie * std::pow(e_quat_b.z(), 2); | ||
| 272 | F_dq_dq(3, 3) = -F_dq_dq(2, 2); | ||
| 273 | |||
| 274 | return F_dq_dq; | ||
| 275 | } | ||
| 276 | |||
| 277 | /// @brief Calculate the linearized attitude quaternion differential equation partially derived for the angular rate bias | ||
| 278 | /// @param b_quat_p Quaternion from IMU platform frame to body frame | ||
| 279 | /// @param e_quat_b Quaternion from body to Earth frame | ||
| 280 | template<typename Derived1, typename Derived2> | ||
| 281 | [[nodiscard]] Eigen::Matrix<typename Derived2::Scalar, 4, 3> e_F_dq_dGyroBias(const Eigen::QuaternionBase<Derived1>& b_quat_p, | ||
| 282 | const Eigen::QuaternionBase<Derived2>& e_quat_b) | ||
| 283 | { | ||
| 284 | Eigen::Matrix<typename Derived2::Scalar, 4, 3> F_dq_dGyroBias; | ||
| 285 | |||
| 286 | auto a = std::pow(b_quat_p.w(), 2) + std::pow(b_quat_p.x(), 2) - std::pow(b_quat_p.y(), 2) - std::pow(b_quat_p.z(), 2); | ||
| 287 | auto b = std::pow(b_quat_p.w(), 2) + std::pow(b_quat_p.y(), 2) - std::pow(b_quat_p.x(), 2) - std::pow(b_quat_p.z(), 2); | ||
| 288 | auto c = std::pow(b_quat_p.w(), 2) + std::pow(b_quat_p.z(), 2) - std::pow(b_quat_p.x(), 2) - std::pow(b_quat_p.y(), 2); | ||
| 289 | auto d = b_quat_p.w() * b_quat_p.x() + b_quat_p.y() * b_quat_p.z(); | ||
| 290 | auto e = b_quat_p.w() * b_quat_p.x() - b_quat_p.y() * b_quat_p.z(); | ||
| 291 | auto f = b_quat_p.w() * b_quat_p.y() + b_quat_p.x() * b_quat_p.z(); | ||
| 292 | auto g = b_quat_p.w() * b_quat_p.y() - b_quat_p.x() * b_quat_p.z(); | ||
| 293 | auto h = b_quat_p.w() * b_quat_p.z() + b_quat_p.x() * b_quat_p.y(); | ||
| 294 | auto i = b_quat_p.w() * b_quat_p.z() - b_quat_p.x() * b_quat_p.y(); | ||
| 295 | |||
| 296 | F_dq_dGyroBias(0, 0) = 0.5 * a * e_quat_b.w() - g * e_quat_b.y() - h * e_quat_b.z(); | ||
| 297 | F_dq_dGyroBias(0, 1) = d * e_quat_b.y() - 0.5 * b * e_quat_b.z() - i * e_quat_b.w(); | ||
| 298 | F_dq_dGyroBias(0, 2) = 0.5 * c * e_quat_b.y() + e * e_quat_b.z() + f * e_quat_b.w(); | ||
| 299 | |||
| 300 | F_dq_dGyroBias(1, 0) = 0.5 * a * e_quat_b.z() + g * e_quat_b.x() + h * e_quat_b.w(); | ||
| 301 | F_dq_dGyroBias(1, 1) = 0.5 * b * e_quat_b.w() - d * e_quat_b.x() - i * e_quat_b.z(); | ||
| 302 | F_dq_dGyroBias(1, 2) = f * e_quat_b.z() - 0.5 * c * e_quat_b.x() - e * e_quat_b.w(); | ||
| 303 | |||
| 304 | F_dq_dGyroBias(2, 0) = h * e_quat_b.x() - 0.5 * a * e_quat_b.y() - g * e_quat_b.w(); | ||
| 305 | F_dq_dGyroBias(2, 1) = 0.5 * b * e_quat_b.x() + d * e_quat_b.w() + i * e_quat_b.y(); | ||
| 306 | F_dq_dGyroBias(2, 2) = 0.5 * c * e_quat_b.w() - e * e_quat_b.x() - f * e_quat_b.y(); | ||
| 307 | |||
| 308 | F_dq_dGyroBias(3, 0) = g * e_quat_b.z() - 0.5 * a * e_quat_b.x() - h * e_quat_b.y(); | ||
| 309 | F_dq_dGyroBias(3, 1) = i * e_quat_b.x() - 0.5 * b * e_quat_b.y() - d * e_quat_b.z(); | ||
| 310 | F_dq_dGyroBias(3, 2) = e * e_quat_b.y() - 0.5 * c * e_quat_b.z() - f * e_quat_b.x(); | ||
| 311 | |||
| 312 | return F_dq_dGyroBias; | ||
| 313 | } | ||
| 314 | |||
| 315 | /// @brief Calculate the linearized attitude quaternion differential equation partially derived for the angular rate scale factor | ||
| 316 | /// @param p_quatGyro_ps Gyroscope misalignment | ||
| 317 | /// @param ps_omega_ip Gyroscope measurement [m/s^2] | ||
| 318 | /// @param b_quat_p Quaternion from IMU platform frame to body frame | ||
| 319 | /// @param e_quat_b Quaternion from body to Earth frame | ||
| 320 | template<typename Derived1, typename Derived2, typename Derived3, typename Derived4> | ||
| 321 | [[nodiscard]] Eigen::Matrix<typename Derived4::Scalar, 4, 3> e_F_dq_dGyroScale(const Eigen::QuaternionBase<Derived1>& p_quatGyro_ps, | ||
| 322 | const Eigen::MatrixBase<Derived2>& ps_omega_ip, | ||
| 323 | const Eigen::QuaternionBase<Derived3>& b_quat_p, | ||
| 324 | const Eigen::QuaternionBase<Derived4>& e_quat_b) | ||
| 325 | { | ||
| 326 | Eigen::Vector3<typename Derived4::Scalar> p_omega_ip = p_quatGyro_ps * ps_omega_ip.template cast<typename Derived4::Scalar>(); | ||
| 327 | |||
| 328 | return e_F_dq_dGyroBias(b_quat_p, e_quat_b) * p_omega_ip.asDiagonal(); | ||
| 329 | } | ||
| 330 | |||
| 331 | /// @brief Calculate the linearized attitude quaternion differential equation partially derived for the gyroscope misalignment | ||
| 332 | /// @param p_gyroScale Angular rate scale factor in platform frame coordinates [rad/s] | ||
| 333 | /// @param p_quatGyro_ps Gyroscope misalignment | ||
| 334 | /// @param ps_omega_ip Gyroscope measurement [m/s^2] | ||
| 335 | /// @param b_quat_p Quaternion from IMU platform frame to body frame | ||
| 336 | /// @param e_quat_b Quaternion from body to Earth frame | ||
| 337 | template<typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5> | ||
| 338 | [[nodiscard]] Eigen::Matrix4<typename Derived5::Scalar> e_F_dq_dGyroMisalignment(const Eigen::MatrixBase<Derived1>& p_gyroScale, | ||
| 339 | const Eigen::QuaternionBase<Derived2>& p_quatGyro_ps, | ||
| 340 | const Eigen::MatrixBase<Derived3>& ps_omega_ip, | ||
| 341 | const Eigen::QuaternionBase<Derived4>& b_quat_p, | ||
| 342 | const Eigen::QuaternionBase<Derived5>& e_quat_b) | ||
| 343 | { | ||
| 344 | Eigen::Matrix3<typename Derived1::Scalar> b_dcmGyroScaled_p = b_quat_p.template cast<typename Derived1::Scalar>() * p_gyroScale.asDiagonal(); | ||
| 345 | |||
| 346 | auto a = p_quatGyro_ps.x() * b_dcmGyroScaled_p(0, 0) + p_quatGyro_ps.y() * b_dcmGyroScaled_p(0, 1) + p_quatGyro_ps.z() * b_dcmGyroScaled_p(0, 2); | ||
| 347 | auto b = p_quatGyro_ps.w() * b_dcmGyroScaled_p(0, 2) + p_quatGyro_ps.y() * b_dcmGyroScaled_p(0, 0) - p_quatGyro_ps.x() * b_dcmGyroScaled_p(0, 1); | ||
| 348 | auto c = p_quatGyro_ps.w() * b_dcmGyroScaled_p(0, 1) + p_quatGyro_ps.x() * b_dcmGyroScaled_p(0, 2) - p_quatGyro_ps.z() * b_dcmGyroScaled_p(0, 0); | ||
| 349 | auto d = p_quatGyro_ps.x() * b_dcmGyroScaled_p(2, 0) + p_quatGyro_ps.y() * b_dcmGyroScaled_p(2, 1) + p_quatGyro_ps.z() * b_dcmGyroScaled_p(2, 2); | ||
| 350 | auto e = p_quatGyro_ps.w() * b_dcmGyroScaled_p(2, 2) + p_quatGyro_ps.y() * b_dcmGyroScaled_p(2, 0) - p_quatGyro_ps.x() * b_dcmGyroScaled_p(2, 1); | ||
| 351 | auto f = p_quatGyro_ps.w() * b_dcmGyroScaled_p(2, 1) + p_quatGyro_ps.x() * b_dcmGyroScaled_p(2, 2) - p_quatGyro_ps.z() * b_dcmGyroScaled_p(2, 0); | ||
| 352 | auto g = p_quatGyro_ps.x() * b_dcmGyroScaled_p(1, 0) + p_quatGyro_ps.y() * b_dcmGyroScaled_p(1, 1) + p_quatGyro_ps.z() * b_dcmGyroScaled_p(1, 2); | ||
| 353 | auto h = p_quatGyro_ps.w() * b_dcmGyroScaled_p(1, 2) + p_quatGyro_ps.y() * b_dcmGyroScaled_p(1, 0) - p_quatGyro_ps.x() * b_dcmGyroScaled_p(1, 1); | ||
| 354 | auto i = p_quatGyro_ps.w() * b_dcmGyroScaled_p(1, 1) + p_quatGyro_ps.x() * b_dcmGyroScaled_p(1, 2) - p_quatGyro_ps.z() * b_dcmGyroScaled_p(1, 0); | ||
| 355 | auto j = p_quatGyro_ps.w() * b_dcmGyroScaled_p(0, 0) + p_quatGyro_ps.z() * b_dcmGyroScaled_p(0, 1) - p_quatGyro_ps.y() * b_dcmGyroScaled_p(0, 2); | ||
| 356 | auto k = p_quatGyro_ps.w() * b_dcmGyroScaled_p(2, 0) + p_quatGyro_ps.z() * b_dcmGyroScaled_p(2, 1) - p_quatGyro_ps.y() * b_dcmGyroScaled_p(2, 2); | ||
| 357 | auto l = p_quatGyro_ps.w() * b_dcmGyroScaled_p(1, 0) + p_quatGyro_ps.z() * b_dcmGyroScaled_p(1, 1) - p_quatGyro_ps.y() * b_dcmGyroScaled_p(1, 2); | ||
| 358 | auto m = a * ps_omega_ip.x() + b * ps_omega_ip.y() - c * ps_omega_ip.z(); | ||
| 359 | auto n = d * ps_omega_ip.x() + e * ps_omega_ip.y() - f * ps_omega_ip.z(); | ||
| 360 | auto o = g * ps_omega_ip.x() + h * ps_omega_ip.y() - i * ps_omega_ip.z(); | ||
| 361 | auto p = a * ps_omega_ip.y() + j * ps_omega_ip.z() - b * ps_omega_ip.x(); | ||
| 362 | auto q = d * ps_omega_ip.y() + k * ps_omega_ip.z() - e * ps_omega_ip.x(); | ||
| 363 | auto r = g * ps_omega_ip.y() + l * ps_omega_ip.z() - h * ps_omega_ip.x(); | ||
| 364 | auto s = a * ps_omega_ip.z() + c * ps_omega_ip.x() - j * ps_omega_ip.y(); | ||
| 365 | auto t = d * ps_omega_ip.z() + f * ps_omega_ip.x() - k * ps_omega_ip.y(); | ||
| 366 | auto u = g * ps_omega_ip.z() + i * ps_omega_ip.x() - l * ps_omega_ip.y(); | ||
| 367 | auto v = b * ps_omega_ip.z() + c * ps_omega_ip.y() + j * ps_omega_ip.x(); | ||
| 368 | auto w = e * ps_omega_ip.z() + f * ps_omega_ip.y() + k * ps_omega_ip.x(); | ||
| 369 | auto x = h * ps_omega_ip.z() + i * ps_omega_ip.y() + l * ps_omega_ip.x(); | ||
| 370 | |||
| 371 | Eigen::Matrix4<typename Derived5::Scalar> F_dq_dGyroMisalignment; | ||
| 372 | |||
| 373 | F_dq_dGyroMisalignment(0, 0) = m * e_quat_b.w() + n * e_quat_b.y() - o * e_quat_b.z(); | ||
| 374 | F_dq_dGyroMisalignment(0, 1) = p * e_quat_b.w() + q * e_quat_b.y() - r * e_quat_b.z(); | ||
| 375 | F_dq_dGyroMisalignment(0, 2) = s * e_quat_b.w() + t * e_quat_b.y() - u * e_quat_b.z(); | ||
| 376 | F_dq_dGyroMisalignment(0, 3) = v * e_quat_b.w() + w * e_quat_b.y() - x * e_quat_b.z(); | ||
| 377 | |||
| 378 | F_dq_dGyroMisalignment(1, 0) = m * e_quat_b.z() + o * e_quat_b.w() - n * e_quat_b.x(); | ||
| 379 | F_dq_dGyroMisalignment(1, 1) = p * e_quat_b.z() + r * e_quat_b.w() - q * e_quat_b.x(); | ||
| 380 | F_dq_dGyroMisalignment(1, 2) = s * e_quat_b.z() + u * e_quat_b.w() - t * e_quat_b.x(); | ||
| 381 | F_dq_dGyroMisalignment(1, 3) = v * e_quat_b.z() + x * e_quat_b.w() - w * e_quat_b.x(); | ||
| 382 | |||
| 383 | F_dq_dGyroMisalignment(2, 0) = n * e_quat_b.w() + o * e_quat_b.x() - m * e_quat_b.y(); | ||
| 384 | F_dq_dGyroMisalignment(2, 1) = q * e_quat_b.w() + r * e_quat_b.x() - p * e_quat_b.y(); | ||
| 385 | F_dq_dGyroMisalignment(2, 2) = t * e_quat_b.w() + u * e_quat_b.x() - s * e_quat_b.y(); | ||
| 386 | F_dq_dGyroMisalignment(2, 3) = w * e_quat_b.w() + x * e_quat_b.x() - v * e_quat_b.y(); | ||
| 387 | |||
| 388 | F_dq_dGyroMisalignment(3, 0) = -m * e_quat_b.x() - n * e_quat_b.z() - o * e_quat_b.y(); | ||
| 389 | F_dq_dGyroMisalignment(3, 1) = -p * e_quat_b.x() - q * e_quat_b.z() - r * e_quat_b.y(); | ||
| 390 | F_dq_dGyroMisalignment(3, 2) = -s * e_quat_b.x() - t * e_quat_b.z() - u * e_quat_b.y(); | ||
| 391 | F_dq_dGyroMisalignment(3, 3) = -v * e_quat_b.x() - w * e_quat_b.z() - x * e_quat_b.y(); | ||
| 392 | |||
| 393 | return F_dq_dGyroMisalignment; | ||
| 394 | } | ||
| 395 | |||
| 396 | /// @brief Calculates the matrix 𝐅_𝛿r'_𝛿v | ||
| 397 | /// @return 3x3 matrix in [-] | ||
| 398 | /// @note See \cite Groves2013 Groves, ch. 14.2.3, eq. 14.48, p. 583 - 𝐅_32 | ||
| 399 | template<typename T> | ||
| 400 | 12888 | [[nodiscard]] Eigen::Matrix3<T> e_F_dr_dv() | |
| 401 | { | ||
| 402 |
2/4✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
|
12888 | return Eigen::Matrix3<T>::Identity(); |
| 403 | } | ||
| 404 | |||
| 405 | /// @brief Calculates the matrix 𝐅_𝛿f'_𝛿f | ||
| 406 | /// @param[in] beta_a Gauss-Markov constant for the accelerometer 𝛽 = 1 / 𝜏 (𝜏 correlation length) | ||
| 407 | /// @return 3x3 matrix in [1 / s] | ||
| 408 | /// @note See T. Hobiger (2021) Inertialnavigation V06 - equation (6.3) | ||
| 409 | template<typename Derived> | ||
| 410 | 6444 | [[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 3, 3> e_F_df_df(const Eigen::MatrixBase<Derived>& beta_a) | |
| 411 | { | ||
| 412 | // Math: \mathbf{F}_{a} = - \begin{bmatrix} \beta_{a,1} & 0 & 0 \\ 0 & \beta_{a,2} & 0 \\ 0 & 0 & \beta_{a,2} \end{bmatrix} \quad \text{T. Hobiger}\,(6.3) | ||
| 413 |
3/6✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6444 times.
✗ Branch 8 not taken.
|
6444 | return -1.0 * beta_a.asDiagonal(); |
| 414 | } | ||
| 415 | |||
| 416 | /// @brief Calculates the matrix 𝐅_𝛿ω'_𝛿ω | ||
| 417 | /// @param[in] beta_omega Gauss-Markov constant for the gyroscope 𝛽 = 1 / 𝜏 (𝜏 correlation length) | ||
| 418 | /// @return 3x3 matrix in [1 / s] | ||
| 419 | /// @note See T. Hobiger (2021) Inertialnavigation V06 - equation (6.3) | ||
| 420 | template<typename Derived> | ||
| 421 | 6444 | [[nodiscard]] Eigen::Matrix<typename Derived::Scalar, 3, 3> e_F_dw_dw(const Eigen::MatrixBase<Derived>& beta_omega) | |
| 422 | { | ||
| 423 | // Math: \mathbf{F}_{\omega} = - \begin{bmatrix} \beta_{\omega,1} & 0 & 0 \\ 0 & \beta_{\omega,2} & 0 \\ 0 & 0 & \beta_{\omega,2} \end{bmatrix} \quad \text{T. Hobiger}\,(6.3) | ||
| 424 |
3/6✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6444 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6444 times.
✗ Branch 8 not taken.
|
6444 | return -1.0 * beta_omega.asDiagonal(); |
| 425 | } | ||
| 426 | |||
| 427 | /// @brief Calculate the linearized system matrix in Earth frame coordinates | ||
| 428 | /// @param ps_f_ip Accelerometer measurement [m/s^2] | ||
| 429 | /// @param ps_omega_ip Gyroscope measurement [m/s^2] | ||
| 430 | /// @param b_quat_p Quaternion from IMU platform frame to body frame | ||
| 431 | /// @param e_gravitation Gravitation in [m/s^2] in ECEF coordinates | ||
| 432 | /// @param r_eS_e Geocentric radius in [m] | ||
| 433 | /// @param tau_bad Correlation length of the accel dynamic bias in [s] | ||
| 434 | /// @param tau_bgd Correlation length of the gyro dynamic bias in [s] | ||
| 435 | /// @param e_position Position in ECEF coordinates in [m] | ||
| 436 | /// @param e_quat_b Quaternion from body to Earth frame | ||
| 437 | /// @param p_accelBias Acceleration bias in platform frame coordinates [m/s^2] | ||
| 438 | /// @param p_gyroBias Angular rate bias in platform frame coordinates [rad/s] | ||
| 439 | /// @param p_accelScale Acceleration scale factor in platform frame coordinates [m/s^2] | ||
| 440 | /// @param p_gyroScale Angular rate scale factor in platform frame coordinates [rad/s] | ||
| 441 | /// @param p_quatAccel_ps Accelerometer misalignment | ||
| 442 | /// @param p_quatGyro_ps Gyroscope misalignment | ||
| 443 | /// @param config Integration config | ||
| 444 | /// @param accelBiases Flag wether to calculate the rows and columns for the acceleration bias | ||
| 445 | /// @param gyroBiases Flag wether to calculate the rows and columns for the angular rate bias | ||
| 446 | /// @param accelScaleFactor Flag wether to calculate the rows and columns for the acceleration scale factor | ||
| 447 | /// @param gyroScaleFactor Flag wether to calculate the rows and columns for the angular rate scale factor | ||
| 448 | /// @param accelMisalignment Flag wether to calculate the rows and columns for the accelerometer misalignment | ||
| 449 | /// @param gyroMisalignment Flag wether to calculate the rows and columns for the gyroscope misalignment | ||
| 450 | template<typename KeyType, typename T> | ||
| 451 | [[nodiscard]] KeyedMatrixX<T, KeyType, KeyType> e_systemMatrix_F(const Eigen::Vector3d& ps_f_ip, | ||
| 452 | const Eigen::Vector3d& ps_omega_ip, | ||
| 453 | const Eigen::Quaterniond& b_quat_p, | ||
| 454 | const Eigen::Vector3<T>& e_gravitation, | ||
| 455 | const T& r_eS_e, | ||
| 456 | std::optional<double> tau_bad, | ||
| 457 | std::optional<double> tau_bgd, | ||
| 458 | const Eigen::Vector3<T>& e_position, | ||
| 459 | const Eigen::Quaternion<T>& e_quat_b, | ||
| 460 | const Eigen::Vector3<T>& p_accelBias, | ||
| 461 | const Eigen::Vector3<T>& p_gyroBias, | ||
| 462 | const Eigen::Vector3<T>& p_accelScale, | ||
| 463 | const Eigen::Vector3<T>& p_gyroScale, | ||
| 464 | const Eigen::Quaternion<T>& p_quatAccel_ps, | ||
| 465 | const Eigen::Quaternion<T>& p_quatGyro_ps, | ||
| 466 | const PosVelAttDerivativeConstants& config, | ||
| 467 | bool accelBiases, bool gyroBiases, | ||
| 468 | bool accelScaleFactor, bool gyroScaleFactor, | ||
| 469 | bool accelMisalignment, bool gyroMisalignment) | ||
| 470 | { | ||
| 471 | std::vector<KeyType> keys; | ||
| 472 | keys.reserve(3 + 3 + 4 | ||
| 473 | + 3 * (static_cast<size_t>(accelBiases) + static_cast<size_t>(gyroBiases)) | ||
| 474 | + 3 * (static_cast<size_t>(accelScaleFactor) + static_cast<size_t>(gyroScaleFactor)) | ||
| 475 | + 4 * (static_cast<size_t>(accelMisalignment) + static_cast<size_t>(gyroMisalignment))); | ||
| 476 | |||
| 477 | keys.insert(keys.end(), { Keys::PosX, Keys::PosY, Keys::PosZ, | ||
| 478 | Keys::VelX, Keys::VelY, Keys::VelZ, | ||
| 479 | Keys::AttQ1, Keys::AttQ2, Keys::AttQ3, Keys::AttQ0 }); | ||
| 480 | |||
| 481 | if (accelBiases) { keys.insert(keys.end(), Keys::AccelBias<KeyType>.begin(), Keys::AccelBias<KeyType>.end()); } | ||
| 482 | if (gyroBiases) { keys.insert(keys.end(), Keys::GyroBias<KeyType>.begin(), Keys::GyroBias<KeyType>.end()); } | ||
| 483 | if (accelScaleFactor) { keys.insert(keys.end(), Keys::AccelScaleFactor<KeyType>.begin(), Keys::AccelScaleFactor<KeyType>.end()); } | ||
| 484 | if (gyroScaleFactor) { keys.insert(keys.end(), Keys::GyroScaleFactor<KeyType>.begin(), Keys::GyroScaleFactor<KeyType>.end()); } | ||
| 485 | if (accelMisalignment) { keys.insert(keys.end(), Keys::AccelMisalignment<KeyType>.begin(), Keys::AccelMisalignment<KeyType>.end()); } | ||
| 486 | if (gyroMisalignment) { keys.insert(keys.end(), Keys::GyroMisalignment<KeyType>.begin(), Keys::GyroMisalignment<KeyType>.end()); } | ||
| 487 | |||
| 488 | KeyedMatrixX<T, KeyType, KeyType> F(Eigen::MatrixX<T>::Zero(static_cast<int>(keys.size()), static_cast<int>(keys.size())), keys, keys); | ||
| 489 | |||
| 490 | F(Keys::Pos<KeyType>, Keys::Vel<KeyType>) = e_F_dr_dv<T>(); | ||
| 491 | F(Keys::Vel<KeyType>, Keys::Pos<KeyType>) = e_F_dv_dr(e_position, | ||
| 492 | config.gravitationModel != GravitationModel::None ? e_gravitation : Eigen::Vector3<T>::Zero(), | ||
| 493 | r_eS_e, config.centrifgalAccelerationCompensationEnabled ? InsConst::e_omega_ie : Eigen::Vector3d::Zero()); | ||
| 494 | F(Keys::Vel<KeyType>, Keys::Vel<KeyType>) = e_F_dv_dv<T>(config.coriolisAccelerationCompensationEnabled ? InsConst::omega_ie : 0.0); | ||
| 495 | F(Keys::Vel<KeyType>, Keys::Att<KeyType>) = e_F_dv_dq(p_accelBias, p_accelScale, p_quatAccel_ps, ps_f_ip, b_quat_p, e_quat_b); | ||
| 496 | F(Keys::Att<KeyType>, Keys::Att<KeyType>) = e_F_dq_dq(p_gyroBias, p_gyroScale, p_quatGyro_ps, ps_omega_ip, b_quat_p, e_quat_b, | ||
| 497 | config.angularRateEarthRotationCompensationEnabled ? InsConst::omega_ie : 0.0); | ||
| 498 | |||
| 499 | if (accelBiases) | ||
| 500 | { | ||
| 501 | F(Keys::Vel<KeyType>, Keys::AccelBias<KeyType>) = e_F_dv_dAccelBias(b_quat_p, e_quat_b); | ||
| 502 | if (tau_bad) { F(Keys::AccelBias<KeyType>, Keys::AccelBias<KeyType>).diagonal().setConstant(T(-1.0 / *tau_bad)); } | ||
| 503 | } | ||
| 504 | if (gyroBiases) | ||
| 505 | { | ||
| 506 | F(Keys::Att<KeyType>, Keys::GyroBias<KeyType>) = e_F_dq_dGyroBias(b_quat_p, e_quat_b); | ||
| 507 | if (tau_bgd) { F(Keys::GyroBias<KeyType>, Keys::GyroBias<KeyType>).diagonal().setConstant(T(-1.0 / *tau_bgd)); } | ||
| 508 | } | ||
| 509 | if (accelScaleFactor) | ||
| 510 | { | ||
| 511 | F(Keys::Vel<KeyType>, Keys::AccelScaleFactor<KeyType>) = e_F_dv_dAccelScale(p_quatAccel_ps, ps_f_ip, b_quat_p, e_quat_b); | ||
| 512 | } | ||
| 513 | if (gyroScaleFactor) | ||
| 514 | { | ||
| 515 | F(Keys::Att<KeyType>, Keys::GyroScaleFactor<KeyType>) = e_F_dq_dGyroScale(p_quatGyro_ps, ps_omega_ip, b_quat_p, e_quat_b); | ||
| 516 | } | ||
| 517 | if (accelMisalignment) | ||
| 518 | { | ||
| 519 | F(Keys::Vel<KeyType>, Keys::AccelMisalignment<KeyType>) = e_F_dv_dAccelMisalignment(p_accelScale, p_quatAccel_ps, ps_f_ip, b_quat_p, e_quat_b); | ||
| 520 | } | ||
| 521 | if (gyroMisalignment) | ||
| 522 | { | ||
| 523 | F(Keys::Att<KeyType>, Keys::GyroMisalignment<KeyType>) = e_F_dq_dGyroMisalignment(p_gyroScale, p_quatGyro_ps, ps_omega_ip, b_quat_p, e_quat_b); | ||
| 524 | } | ||
| 525 | |||
| 526 | return F; | ||
| 527 | } | ||
| 528 | |||
| 529 | } // namespace NAV | ||
| 530 |