| 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 Functions.hpp | ||
| 10 | /// @brief Inertial Navigation Helper Functions | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2020-09-02 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <Eigen/Core> | ||
| 17 | |||
| 18 | #include "Navigation/Constants.hpp" | ||
| 19 | #include "Navigation/Math/Math.hpp" | ||
| 20 | |||
| 21 | namespace NAV | ||
| 22 | { | ||
| 23 | /// @brief Calculates the transport rate ω_en_n (rotation rate of the Earth frame relative to the navigation frame) | ||
| 24 | /// | ||
| 25 | /// \anchor eq-INS-Mechanization-TransportRate \f{equation}{ \label{eq:eq-INS-Mechanization-TransportRate} | ||
| 26 | /// \boldsymbol{\omega}_{en}^n = \begin{bmatrix} \dfrac{v_E}{R_E + h} & \dfrac{-v_N}{R_N + h} & \dfrac{-v_E \tan{\phi}}{R_E + h} \end{bmatrix}^T | ||
| 27 | /// \f} | ||
| 28 | /// | ||
| 29 | /// @param[in] lla_position [𝜙, λ, h] Latitude, Longitude and altitude in [rad, rad, m] | ||
| 30 | /// @param[in] n_velocity Velocity in [m/s], in navigation coordinate | ||
| 31 | /// @param[in] R_N North/South (meridian) earth radius [m] | ||
| 32 | /// @param[in] R_E East/West (prime vertical) earth radius [m] | ||
| 33 | /// @return ω_en_n Transport Rate in local-navigation coordinates in [rad/s] | ||
| 34 | /// | ||
| 35 | /// @note See \cite Groves2013 Groves, ch. 5.4.1, eq. 5.44, p. 177 | ||
| 36 | /// @note See \cite Gleason2009 Gleason, ch. 6.2.3.2, eq. 6.15, p. 155 | ||
| 37 | /// @note See \cite Titterton2004 Titterton, ch. 3.7.2, eq. 3.87, p. 50 (mistake in denominator 3rd term) | ||
| 38 | template<typename DerivedA, typename DerivedB> | ||
| 39 | 5887909 | [[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> n_calcTransportRate(const Eigen::MatrixBase<DerivedA>& lla_position, | |
| 40 | const Eigen::MatrixBase<DerivedB>& n_velocity, | ||
| 41 | const typename DerivedA::Scalar& R_N, | ||
| 42 | const typename DerivedA::Scalar& R_E) | ||
| 43 | { | ||
| 44 | // 𝜙 Latitude in [rad] | ||
| 45 |
1/2✓ Branch 1 taken 113682 times.
✗ Branch 2 not taken.
|
5887909 | const auto& latitude = lla_position(0); |
| 46 | // h Altitude in [m] | ||
| 47 |
1/2✓ Branch 1 taken 113682 times.
✗ Branch 2 not taken.
|
5877244 | const auto& altitude = lla_position(2); |
| 48 | |||
| 49 | // Velocity North in [m/s] | ||
| 50 |
1/2✓ Branch 1 taken 113682 times.
✗ Branch 2 not taken.
|
5882551 | const auto& v_N = n_velocity(0); |
| 51 | // Velocity East in [m/s] | ||
| 52 |
1/2✓ Branch 1 taken 113682 times.
✗ Branch 2 not taken.
|
5884640 | const auto& v_E = n_velocity(1); |
| 53 | |||
| 54 |
1/2✓ Branch 1 taken 113682 times.
✗ Branch 2 not taken.
|
5886075 | Eigen::Vector3<typename DerivedA::Scalar> n_omega_en__t1; |
| 55 |
1/2✓ Branch 1 taken 113682 times.
✗ Branch 2 not taken.
|
5879503 | n_omega_en__t1(0) = v_E / (R_E + altitude); |
| 56 |
1/2✓ Branch 1 taken 113682 times.
✗ Branch 2 not taken.
|
5884331 | n_omega_en__t1(1) = -v_N / (R_N + altitude); |
| 57 |
2/4✓ Branch 1 taken 113682 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 113682 times.
✗ Branch 5 not taken.
|
5886490 | n_omega_en__t1(2) = -n_omega_en__t1(0) * std::tan(latitude); |
| 58 | |||
| 59 | 6000283 | return n_omega_en__t1; | |
| 60 | } | ||
| 61 | |||
| 62 | /// @brief Calculates the centrifugal acceleration in [m/s^2] (acceleration that makes a body follow a curved path) | ||
| 63 | /// | ||
| 64 | /// \anchor eq-INS-Mechanization-CentrifugalAcceleration \f{equation}{ \label{eq:eq-INS-Mechanization-CentrifugalAcceleration} | ||
| 65 | /// \boldsymbol{\omega}_{ie}^e \times [ \boldsymbol{\omega}_{ie}^e \times \mathbf{x}^e ] | ||
| 66 | /// \f} | ||
| 67 | /// | ||
| 68 | /// @param[in] e_position Position in coordinates in [m] | ||
| 69 | /// @param[in] e_omega_ie Angular rate of the Earth rotation in [rad/s] in the Earth coordinate frame | ||
| 70 | /// @return Centrifugal acceleration in the Earth coordinate frame in [m/s^2] | ||
| 71 | template<typename DerivedA, typename DerivedB = DerivedA> | ||
| 72 | 6025582 | [[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> e_calcCentrifugalAcceleration(const Eigen::MatrixBase<DerivedA>& e_position, | |
| 73 | const Eigen::MatrixBase<DerivedB>& e_omega_ie = InsConst::e_omega_ie) | ||
| 74 | { | ||
| 75 | // ω_ie_e ⨯ [ω_ie_e ⨯ x_e] | ||
| 76 |
2/4✓ Branch 1 taken 3016044 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3019201 times.
✗ Branch 5 not taken.
|
6025582 | return e_omega_ie.cross(e_omega_ie.cross(e_position)); |
| 77 | } | ||
| 78 | |||
| 79 | /// @brief Calculates the coriolis acceleration in [m/s^2] (acceleration due to motion in rotating reference frame) | ||
| 80 | /// | ||
| 81 | /// \anchor eq-INS-Mechanization-CoriolisAcceleration \f{equation}{ \label{eq:eq-INS-Mechanization-CoriolisAcceleration} | ||
| 82 | /// (2 \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{v}^n | ||
| 83 | /// \f} | ||
| 84 | /// | ||
| 85 | /// @param[in] n_omega_ie ω_ie_n Angular rate of the Earth rotation in [rad/s] in local-navigation coordinates | ||
| 86 | /// @param[in] n_omega_en ω_en_n Transport rate in [rad/s] in local-navigation coordinates | ||
| 87 | /// @param[in] n_velocity Velocity in local-navigation frame coordinates in [m/s^2] | ||
| 88 | /// @return Coriolis acceleration in local-navigation coordinates in [m/s^2] | ||
| 89 | template<typename DerivedA, typename DerivedB, typename DerivedC> | ||
| 90 | 2961769 | [[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> n_calcCoriolisAcceleration(const Eigen::MatrixBase<DerivedA>& n_omega_ie, | |
| 91 | const Eigen::MatrixBase<DerivedB>& n_omega_en, | ||
| 92 | const Eigen::MatrixBase<DerivedC>& n_velocity) | ||
| 93 | { | ||
| 94 |
3/6✓ Branch 1 taken 2969984 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2967641 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2975294 times.
✗ Branch 8 not taken.
|
5937063 | return (2 * n_omega_ie + n_omega_en).cross(n_velocity); |
| 95 | } | ||
| 96 | |||
| 97 | /// @brief Calculates the coriolis acceleration in [m/s^2] (acceleration due to motion in rotating reference frame) | ||
| 98 | /// | ||
| 99 | /// \anchor eq-INS-Mechanization-CoriolisAcceleration-e \f{equation}{ \label{eq:eq-INS-Mechanization-CoriolisAcceleration-e} | ||
| 100 | /// 2 \boldsymbol{\omega}_{ie}^e \times \boldsymbol{v}^e | ||
| 101 | /// \f} | ||
| 102 | /// | ||
| 103 | /// @param[in] e_omega_ie ω_ie_e Angular rate of the Earth rotation in [rad/s] in ECEF coordinates | ||
| 104 | /// @param[in] e_velocity Velocity in ECEF frame coordinates in [m/s^2] | ||
| 105 | /// @return Coriolis acceleration in ECEF coordinates in [m/s^2] | ||
| 106 | template<typename DerivedA, typename DerivedB> | ||
| 107 | 38664 | [[nodiscard]] Eigen::Vector3<typename DerivedA::Scalar> e_calcCoriolisAcceleration(const Eigen::MatrixBase<DerivedA>& e_omega_ie, const Eigen::MatrixBase<DerivedB>& e_velocity) | |
| 108 | { | ||
| 109 |
2/4✓ Branch 1 taken 38664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38664 times.
✗ Branch 5 not taken.
|
77328 | return (2 * e_omega_ie).cross(e_velocity); |
| 110 | } | ||
| 111 | |||
| 112 | /// @brief Calculates the roll angle from a static acceleration measurement | ||
| 113 | /// @param[in] b_accel Acceleration measurement in static condition in [m/s^2] | ||
| 114 | /// @return The roll angle in [rad] | ||
| 115 | /// | ||
| 116 | /// @note See E.-H. Shin (2005) - Estimation Techniques for Low-Cost Inertial Navigation (Chapter 2.6) | ||
| 117 | template<typename Derived> | ||
| 118 | ✗ | [[nodiscard]] typename Derived::Scalar calcRollFromStaticAcceleration(const Eigen::MatrixBase<Derived>& b_accel) | |
| 119 | { | ||
| 120 | // See E.-H. Shin (2005) - Estimation Techniques for Low-Cost Inertial Navigation (Chapter 2.6 - eq. 2.72a) | ||
| 121 | ✗ | return math::sgn(b_accel.z()) * std::asin(b_accel.y() / b_accel.norm()); | |
| 122 | |||
| 123 | // Another possible calculation would be: | ||
| 124 | // return std::atan2(b_accel.y(), b_accel.z()); | ||
| 125 | } | ||
| 126 | |||
| 127 | /// @brief Calculates the pitch angle from a static acceleration measurement | ||
| 128 | /// @param[in] b_accel Acceleration measurement in static condition in [m/s^2] | ||
| 129 | /// @return The pitch angle in [rad] | ||
| 130 | /// | ||
| 131 | /// @note See E.-H. Shin (2005) - Estimation Techniques for Low-Cost Inertial Navigation (Chapter 2.6) | ||
| 132 | template<typename Derived> | ||
| 133 | ✗ | [[nodiscard]] typename Derived::Scalar calcPitchFromStaticAcceleration(const Eigen::MatrixBase<Derived>& b_accel) | |
| 134 | { | ||
| 135 | // See E.-H. Shin (2005) - Estimation Techniques for Low-Cost Inertial Navigation (Chapter 2.6 - eq. 2.72b) | ||
| 136 | ✗ | return -math::sgn(b_accel.z()) * std::asin(b_accel.x() / b_accel.norm()); | |
| 137 | |||
| 138 | // Another possible calculation would be: | ||
| 139 | // return std::atan2((-b_accel.x()), sqrt(std::pow(b_accel.y(), 2) + std::pow(b_accel.z(), 2))); | ||
| 140 | } | ||
| 141 | |||
| 142 | /// @brief Calculates the Yaw angle from the trajectory defined by the given velocity | ||
| 143 | /// | ||
| 144 | /// \anchor eq-INS-Mechanization-Yaw \f{equation}{ \label{eq:eq-INS-Mechanization-Yaw} | ||
| 145 | /// Y = \tan^{-1}\left(\frac{v_E}{v_N}\right) | ||
| 146 | /// \f} | ||
| 147 | /// | ||
| 148 | /// @param[in] n_velocity Velocity in [m/s] in local-navigation frame coordinates | ||
| 149 | /// @return Yaw angle in [rad] | ||
| 150 | /// | ||
| 151 | /// @note See \cite Groves2013 Groves, ch. 6.1.4, eq. 6.14, p. 225 | ||
| 152 | template<typename Derived> | ||
| 153 | 1344080 | [[nodiscard]] typename Derived::Scalar calcYawFromVelocity(const Eigen::MatrixBase<Derived>& n_velocity) | |
| 154 | { | ||
| 155 | 1344080 | return std::atan2(n_velocity(1), n_velocity(0)); | |
| 156 | } | ||
| 157 | |||
| 158 | /// @brief Calculates the Pitch angle from the trajectory defined by the given velocity | ||
| 159 | /// | ||
| 160 | /// \anchor eq-INS-Mechanization-Pitch \f{equation}{ \label{eq:eq-INS-Mechanization-Pitch} | ||
| 161 | /// P = \tan^{-1}\left(\frac{-v_D}{\sqrt{v_N^2 + v_E^2}}\right) | ||
| 162 | /// \f} | ||
| 163 | /// | ||
| 164 | /// @param[in] n_velocity Velocity in [m/s] in local-navigation frame coordinates | ||
| 165 | /// @return Pitch angle in [rad] | ||
| 166 | /// | ||
| 167 | /// @note See \cite Groves2013 Groves, ch. 6.1.4, eq. 6.17, p. 225 | ||
| 168 | template<typename Derived> | ||
| 169 | 1344080 | [[nodiscard]] typename Derived::Scalar calcPitchFromVelocity(const Eigen::MatrixBase<Derived>& n_velocity) | |
| 170 | { | ||
| 171 | 1344080 | return std::atan(-n_velocity(2) / std::sqrt(std::pow(n_velocity(0), 2) + std::pow(n_velocity(1), 2))); | |
| 172 | } | ||
| 173 | |||
| 174 | } // namespace NAV | ||
| 175 |