| 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 PosVelAtt.hpp | ||
| 10 | /// @brief Position, Velocity and Attitude Storage Class | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2020-08-21 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include "Navigation/Transformations/CoordinateFrames.hpp" | ||
| 17 | |||
| 18 | #include "Navigation/Transformations/Units.hpp" | ||
| 19 | #include "NodeData/State/PosVel.hpp" | ||
| 20 | |||
| 21 | namespace NAV | ||
| 22 | { | ||
| 23 | /// Position, Velocity and Attitude Storage Class | ||
| 24 | class PosVelAtt : public PosVel | ||
| 25 | { | ||
| 26 | public: | ||
| 27 | /// @brief Returns the type of the data class | ||
| 28 | /// @return The data type | ||
| 29 | 317804 | [[nodiscard]] static std::string type() | |
| 30 | { | ||
| 31 |
1/2✓ Branch 1 taken 317802 times.
✗ Branch 2 not taken.
|
635606 | return "PosVelAtt"; |
| 32 | } | ||
| 33 | |||
| 34 | /// @brief Returns the type of the data class | ||
| 35 | /// @return The data type | ||
| 36 | ✗ | [[nodiscard]] std::string getType() const override { return type(); } | |
| 37 | |||
| 38 | /// @brief Returns the parent types of the data class | ||
| 39 | /// @return The parent data types | ||
| 40 | 224 | [[nodiscard]] static std::vector<std::string> parentTypes() | |
| 41 | { | ||
| 42 | 224 | auto parent = PosVel::parentTypes(); | |
| 43 |
2/4✓ Branch 1 taken 224 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 224 times.
✗ Branch 5 not taken.
|
224 | parent.push_back(PosVel::type()); |
| 44 | 224 | return parent; | |
| 45 | ✗ | } | |
| 46 | |||
| 47 | /// @brief Returns a vector of data descriptors | ||
| 48 | 581372 | [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors() | |
| 49 | { | ||
| 50 | 581372 | auto desc = PosVel::GetStaticDataDescriptors(); | |
| 51 |
1/2✓ Branch 2 taken 581372 times.
✗ Branch 3 not taken.
|
581372 | desc.reserve(GetStaticDescriptorCount()); |
| 52 |
1/2✓ Branch 1 taken 581372 times.
✗ Branch 2 not taken.
|
581372 | desc.emplace_back("Roll [deg]"); |
| 53 |
1/2✓ Branch 1 taken 581372 times.
✗ Branch 2 not taken.
|
581372 | desc.emplace_back("Pitch [deg]"); |
| 54 |
1/2✓ Branch 1 taken 581372 times.
✗ Branch 2 not taken.
|
581372 | desc.emplace_back("Yaw [deg]"); |
| 55 |
1/2✓ Branch 1 taken 581372 times.
✗ Branch 2 not taken.
|
581372 | desc.emplace_back("Quaternion::w"); |
| 56 |
1/2✓ Branch 1 taken 581372 times.
✗ Branch 2 not taken.
|
581372 | desc.emplace_back("Quaternion::x"); |
| 57 |
1/2✓ Branch 1 taken 581372 times.
✗ Branch 2 not taken.
|
581372 | desc.emplace_back("Quaternion::y"); |
| 58 |
1/2✓ Branch 1 taken 581372 times.
✗ Branch 2 not taken.
|
581372 | desc.emplace_back("Quaternion::z"); |
| 59 | 581372 | return desc; | |
| 60 | ✗ | } | |
| 61 | |||
| 62 | /// @brief Get the amount of descriptors | ||
| 63 | 1744052 | [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return PosVel::GetStaticDescriptorCount() + 7; } | |
| 64 | |||
| 65 | /// @brief Returns a vector of data descriptors | ||
| 66 | ✗ | [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); } | |
| 67 | |||
| 68 | /// @brief Get the amount of descriptors | ||
| 69 | 290664 | [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); } | |
| 70 | |||
| 71 | /// @brief Get the value at the index | ||
| 72 | /// @param idx Index corresponding to data descriptor order | ||
| 73 | /// @return Value if in the observation | ||
| 74 | 290664 | [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override | |
| 75 | { | ||
| 76 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 290664 times.
|
290664 | INS_ASSERT(idx < GetStaticDescriptorCount()); |
| 77 |
2/2✓ Branch 1 taken 145332 times.
✓ Branch 2 taken 145332 times.
|
290664 | if (idx < PosVel::GetStaticDescriptorCount()) { return PosVel::getValueAt(idx); } |
| 78 |
3/8✓ Branch 0 taken 48444 times.
✓ Branch 1 taken 48444 times.
✓ Branch 2 taken 48444 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
145332 | switch (idx) |
| 79 | { | ||
| 80 | 48444 | case PosVel::GetStaticDescriptorCount() + 0: // Roll [deg] | |
| 81 |
3/6✓ Branch 1 taken 48444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48444 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 48444 times.
✗ Branch 8 not taken.
|
48444 | if (_e_Quat_b.norm() != 0) { return rad2deg(rollPitchYaw().x()); } |
| 82 | ✗ | break; | |
| 83 | 48444 | case PosVel::GetStaticDescriptorCount() + 1: // Pitch [deg] | |
| 84 |
3/6✓ Branch 1 taken 48444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48444 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 48444 times.
✗ Branch 8 not taken.
|
48444 | if (_e_Quat_b.norm() != 0) { return rad2deg(rollPitchYaw().y()); } |
| 85 | ✗ | break; | |
| 86 | 48444 | case PosVel::GetStaticDescriptorCount() + 2: // Yaw [deg] | |
| 87 |
3/6✓ Branch 1 taken 48444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48444 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 48444 times.
✗ Branch 8 not taken.
|
48444 | if (_e_Quat_b.norm() != 0) { return rad2deg(rollPitchYaw().z()); } |
| 88 | ✗ | break; | |
| 89 | ✗ | case PosVel::GetStaticDescriptorCount() + 3: // Quaternion::w | |
| 90 | ✗ | if (_e_Quat_b.norm() != 0) { return n_Quat_b().w(); } | |
| 91 | ✗ | break; | |
| 92 | ✗ | case PosVel::GetStaticDescriptorCount() + 4: // Quaternion::x | |
| 93 | ✗ | if (_e_Quat_b.norm() != 0) { return n_Quat_b().x(); } | |
| 94 | ✗ | break; | |
| 95 | ✗ | case PosVel::GetStaticDescriptorCount() + 5: // Quaternion::y | |
| 96 | ✗ | if (_e_Quat_b.norm() != 0) { return n_Quat_b().y(); } | |
| 97 | ✗ | break; | |
| 98 | ✗ | case PosVel::GetStaticDescriptorCount() + 6: // Quaternion::z | |
| 99 | ✗ | if (_e_Quat_b.norm() != 0) { return n_Quat_b().z(); } | |
| 100 | ✗ | break; | |
| 101 | ✗ | default: | |
| 102 | ✗ | return std::nullopt; | |
| 103 | } | ||
| 104 | ✗ | return std::nullopt; | |
| 105 | } | ||
| 106 | |||
| 107 | /// @brief Set the value at the index | ||
| 108 | /// @param idx Index corresponding to data descriptor order | ||
| 109 | /// @param value Value to set | ||
| 110 | /// @return True if the value was updated | ||
| 111 | ✗ | [[nodiscard]] bool setValueAt(size_t idx, double value) override | |
| 112 | { | ||
| 113 | ✗ | INS_ASSERT(idx < GetStaticDescriptorCount()); | |
| 114 | ✗ | if (idx < PosVel::GetStaticDescriptorCount()) { return PosVel::setValueAt(idx, value); } | |
| 115 | // switch (idx) | ||
| 116 | // { | ||
| 117 | // case PosVel::GetStaticDescriptorCount() + 0: // Roll [deg] | ||
| 118 | // case PosVel::GetStaticDescriptorCount() + 1: // Pitch [deg] | ||
| 119 | // case PosVel::GetStaticDescriptorCount() + 2: // Yaw [deg] | ||
| 120 | // case PosVel::GetStaticDescriptorCount() + 3: // Quaternion::w | ||
| 121 | // case PosVel::GetStaticDescriptorCount() + 4: // Quaternion::x | ||
| 122 | // case PosVel::GetStaticDescriptorCount() + 5: // Quaternion::y | ||
| 123 | // case PosVel::GetStaticDescriptorCount() + 6: // Quaternion::z | ||
| 124 | // default: | ||
| 125 | // } | ||
| 126 | |||
| 127 | ✗ | return false; | |
| 128 | } | ||
| 129 | |||
| 130 | /* -------------------------------------------------------------------------------------------------------- */ | ||
| 131 | /* Rotation Quaternions */ | ||
| 132 | /* -------------------------------------------------------------------------------------------------------- */ | ||
| 133 | |||
| 134 | /// @brief Returns the Quaternion from body to navigation frame (NED) | ||
| 135 | /// @return The Quaternion for the rotation from body to navigation coordinates | ||
| 136 | 713129 | [[nodiscard]] const Eigen::Quaterniond& n_Quat_b() const | |
| 137 | { | ||
| 138 | 713129 | return _n_Quat_b; | |
| 139 | } | ||
| 140 | |||
| 141 | /// @brief Returns the Quaternion from navigation to body frame (NED) | ||
| 142 | /// @return The Quaternion for the rotation from navigation to body coordinates | ||
| 143 | 2 | [[nodiscard]] Eigen::Quaterniond b_Quat_n() const | |
| 144 | { | ||
| 145 | 2 | return n_Quat_b().conjugate(); | |
| 146 | } | ||
| 147 | |||
| 148 | /// @brief Returns the Quaternion from body to Earth-fixed frame | ||
| 149 | /// @return The Quaternion for the rotation from body to earth coordinates | ||
| 150 | 114753 | [[nodiscard]] const Eigen::Quaterniond& e_Quat_b() const | |
| 151 | { | ||
| 152 | 114753 | return _e_Quat_b; | |
| 153 | } | ||
| 154 | |||
| 155 | /// @brief Returns the Quaternion from Earth-fixed to body frame | ||
| 156 | /// @return The Quaternion for the rotation from earth to body coordinates | ||
| 157 | 2 | [[nodiscard]] Eigen::Quaterniond b_Quat_e() const | |
| 158 | { | ||
| 159 | 2 | return e_Quat_b().conjugate(); | |
| 160 | } | ||
| 161 | |||
| 162 | /// @brief Returns the Roll, Pitch and Yaw angles in [rad] | ||
| 163 | /// @return [roll, pitch, yaw]^T | ||
| 164 | 354599 | [[nodiscard]] Eigen::Vector3d rollPitchYaw() const | |
| 165 | { | ||
| 166 | 354599 | return trafo::quat2eulerZYX(n_Quat_b()); | |
| 167 | } | ||
| 168 | |||
| 169 | // ########################################################################################################### | ||
| 170 | // Setter | ||
| 171 | // ########################################################################################################### | ||
| 172 | |||
| 173 | /// @brief Set the Quaternion from body to earth frame | ||
| 174 | /// @param[in] e_Quat_b Quaternion from body to earth frame | ||
| 175 | template<typename Derived> | ||
| 176 | 54162 | void setAttitude_e_Quat_b(const Eigen::QuaternionBase<Derived>& e_Quat_b) | |
| 177 | { | ||
| 178 | 54162 | _e_Quat_b = e_Quat_b; | |
| 179 |
2/4✓ Branch 1 taken 54162 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54162 times.
✗ Branch 5 not taken.
|
54162 | _n_Quat_b = n_Quat_e() * e_Quat_b; |
| 180 | 54162 | } | |
| 181 | |||
| 182 | /// @brief Set the Quaternion from body to navigation frame | ||
| 183 | /// @param[in] n_Quat_b Quaternion from body to navigation frame | ||
| 184 | template<typename Derived> | ||
| 185 | 355098 | void setAttitude_n_Quat_b(const Eigen::QuaternionBase<Derived>& n_Quat_b) | |
| 186 | { | ||
| 187 |
2/4✓ Branch 1 taken 355096 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 355095 times.
✗ Branch 5 not taken.
|
355098 | _e_Quat_b = e_Quat_n() * n_Quat_b; |
| 188 | 355094 | _n_Quat_b = n_Quat_b; | |
| 189 | 355098 | } | |
| 190 | |||
| 191 | /// @brief Set the State | ||
| 192 | /// @param[in] e_position New Position in ECEF coordinates | ||
| 193 | /// @param[in] e_velocity The new velocity in the earth frame | ||
| 194 | /// @param[in] e_Quat_b Quaternion from body to earth frame | ||
| 195 | template<typename DerivedP, typename DerivedV, typename DerivedA> | ||
| 196 | 12906 | void setState_e(const Eigen::MatrixBase<DerivedP>& e_position, const Eigen::MatrixBase<DerivedV>& e_velocity, const Eigen::QuaternionBase<DerivedA>& e_Quat_b) | |
| 197 | { | ||
| 198 | 12906 | setPosition_e(e_position); | |
| 199 | 12906 | setVelocity_e(e_velocity); | |
| 200 | 12906 | setAttitude_e_Quat_b(e_Quat_b); | |
| 201 | 12906 | } | |
| 202 | |||
| 203 | /// @brief Set the State | ||
| 204 | /// @param[in] lla_position New Position in LatLonAlt coordinates [rad, rad, m] | ||
| 205 | /// @param[in] n_velocity The new velocity in the NED frame [m/s, m/s, m/s] | ||
| 206 | /// @param[in] n_Quat_b Quaternion from body to navigation frame | ||
| 207 | template<typename DerivedP, typename DerivedV, typename DerivedA> | ||
| 208 | 62903 | void setState_n(const Eigen::MatrixBase<DerivedP>& lla_position, const Eigen::MatrixBase<DerivedV>& n_velocity, const Eigen::QuaternionBase<DerivedA>& n_Quat_b) | |
| 209 | { | ||
| 210 | 62903 | setPosition_lla(lla_position); | |
| 211 | 62903 | setVelocity_n(n_velocity); | |
| 212 | 62903 | setAttitude_n_Quat_b(n_Quat_b); | |
| 213 | 62903 | } | |
| 214 | |||
| 215 | /// @brief Set the State and the standard deviations | ||
| 216 | /// @param[in] e_position New Position in ECEF coordinates | ||
| 217 | /// @param[in] e_positionCovarianceMatrix Standard deviation of Position in ECEF coordinates [m] | ||
| 218 | /// @param[in] e_velocity The new velocity in the earth frame | ||
| 219 | /// @param[in] e_velocityCovarianceMatrix Covariance matrix of Velocity in earth coordinates [m/s] | ||
| 220 | /// @param[in] e_Quat_b Quaternion from body to earth frame | ||
| 221 | template<typename DerivedP, typename DerivedP2, typename DerivedV, typename DerivedV2, typename DerivedA> | ||
| 222 | 79920 | void setStateAndStdDev_e(const Eigen::MatrixBase<DerivedP>& e_position, const Eigen::MatrixBase<DerivedP2>& e_positionCovarianceMatrix, | |
| 223 | const Eigen::MatrixBase<DerivedV>& e_velocity, const Eigen::MatrixBase<DerivedV2>& e_velocityCovarianceMatrix, | ||
| 224 | const Eigen::QuaternionBase<DerivedA>& e_Quat_b) | ||
| 225 | { | ||
| 226 | 79920 | setPositionAndStdDev_e(e_position, e_positionCovarianceMatrix); | |
| 227 | 79920 | setVelocityAndStdDev_e(e_velocity, e_velocityCovarianceMatrix); | |
| 228 | 79920 | setAttitude_e_Quat_b(e_Quat_b); | |
| 229 | 79920 | } | |
| 230 | |||
| 231 | /// @brief Set the State and the standard deviations | ||
| 232 | /// @param[in] lla_position New Position in LatLonAlt coordinates [rad, rad, m] | ||
| 233 | /// @param[in] n_positionCovarianceMatrix Standard deviation of Position in NED coordinates [m] | ||
| 234 | /// @param[in] n_velocity The new velocity in the NED frame [m/s, m/s, m/s] | ||
| 235 | /// @param[in] n_velocityCovarianceMatrix Covariance matrix of Velocity in NED coordinates [m/s] | ||
| 236 | /// @param[in] n_Quat_b Quaternion from body to navigation frame | ||
| 237 | template<typename DerivedP, typename DerivedP2, typename DerivedV, typename DerivedV2, typename DerivedA> | ||
| 238 | 87635 | void setStateAndStdDev_n(const Eigen::MatrixBase<DerivedP>& lla_position, const Eigen::MatrixBase<DerivedP2>& n_positionCovarianceMatrix, | |
| 239 | const Eigen::MatrixBase<DerivedV>& n_velocity, const Eigen::MatrixBase<DerivedV2>& n_velocityCovarianceMatrix, | ||
| 240 | const Eigen::QuaternionBase<DerivedA>& n_Quat_b) | ||
| 241 | { | ||
| 242 | 87635 | setPositionAndStdDev_lla(lla_position, n_positionCovarianceMatrix); | |
| 243 | 87635 | setVelocityAndStdDev_n(n_velocity, n_velocityCovarianceMatrix); | |
| 244 | 87635 | setAttitude_n_Quat_b(n_Quat_b); | |
| 245 | 87635 | } | |
| 246 | |||
| 247 | /* -------------------------------------------------------------------------------------------------------- */ | ||
| 248 | /* Member variables */ | ||
| 249 | /* -------------------------------------------------------------------------------------------------------- */ | ||
| 250 | |||
| 251 | private: | ||
| 252 | /// Quaternion body to earth frame | ||
| 253 | Eigen::Quaterniond _e_Quat_b{ 0, 0, 0, 0 }; | ||
| 254 | /// Quaternion body to navigation frame (roll, pitch, yaw) | ||
| 255 | Eigen::Quaterniond _n_Quat_b{ 0, 0, 0, 0 }; | ||
| 256 | }; | ||
| 257 | |||
| 258 | } // namespace NAV | ||
| 259 |