| 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 Pos.hpp | ||
| 10 | /// @brief Position Storage Class | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2021-10-27 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <cstdint> | ||
| 17 | #include "Navigation/Transformations/CoordinateFrames.hpp" | ||
| 18 | #include "Navigation/Transformations/Units.hpp" | ||
| 19 | |||
| 20 | #include "util/Logger/CommonLog.hpp" | ||
| 21 | #include "NodeData/NodeData.hpp" | ||
| 22 | #include "util/Container/KeyedMatrix.hpp" | ||
| 23 | #include "util/Assert.h" | ||
| 24 | #include <Eigen/src/Core/MatrixBase.h> | ||
| 25 | |||
| 26 | namespace NAV | ||
| 27 | { | ||
| 28 | /// Position, Velocity and Attitude Storage Class | ||
| 29 | class Pos : public NodeData | ||
| 30 | { | ||
| 31 | public: | ||
| 32 | /// @brief Returns the type of the data class | ||
| 33 | /// @return The data type | ||
| 34 | 1546 | [[nodiscard]] static std::string type() | |
| 35 | { | ||
| 36 |
1/2✓ Branch 1 taken 1546 times.
✗ Branch 2 not taken.
|
3092 | return "Pos"; |
| 37 | } | ||
| 38 | |||
| 39 | /// @brief Returns the type of the data class | ||
| 40 | /// @return The data type | ||
| 41 | ✗ | [[nodiscard]] std::string getType() const override { return type(); } | |
| 42 | |||
| 43 | /// @brief Returns the parent types of the data class | ||
| 44 | /// @return The parent data types | ||
| 45 | 784 | [[nodiscard]] static std::vector<std::string> parentTypes() | |
| 46 | { | ||
| 47 |
3/6✓ Branch 1 taken 784 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 784 times.
✓ Branch 4 taken 784 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
2352 | return { NodeData::type() }; |
| 48 | 784 | } | |
| 49 | |||
| 50 | /// @brief Returns a vector of data descriptors | ||
| 51 | 581382 | [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors() | |
| 52 | { | ||
| 53 | return { | ||
| 54 | "Latitude [deg]", | ||
| 55 | "Longitude [deg]", | ||
| 56 | "Altitude [m]", | ||
| 57 | "North/South [m]", | ||
| 58 | "East/West [m]", | ||
| 59 | "X-ECEF [m]", | ||
| 60 | "Y-ECEF [m]", | ||
| 61 | "Z-ECEF [m]", | ||
| 62 | "X-ECEF StDev [m]", | ||
| 63 | "Y-ECEF StDev [m]", | ||
| 64 | "Z-ECEF StDev [m]", | ||
| 65 | "XY-ECEF StDev [m]", | ||
| 66 | "XZ-ECEF StDev [m]", | ||
| 67 | "YZ-ECEF StDev [m]", | ||
| 68 | "North StDev [m]", | ||
| 69 | "East StDev [m]", | ||
| 70 | "Down StDev [m]", | ||
| 71 | "NE StDev [m]", | ||
| 72 | "ND StDev [m]", | ||
| 73 | "ED StDev [m]", | ||
| 74 |
1/2✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
|
1744146 | }; |
| 75 | } | ||
| 76 | |||
| 77 | /// @brief Get the amount of descriptors | ||
| 78 | 3052104 | [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return 20; } | |
| 79 | |||
| 80 | /// @brief Returns a vector of data descriptors | ||
| 81 | ✗ | [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); } | |
| 82 | |||
| 83 | /// @brief Get the amount of descriptors | ||
| 84 | ✗ | [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); } | |
| 85 | |||
| 86 | /// @brief Get the value at the index | ||
| 87 | /// @param idx Index corresponding to data descriptor order | ||
| 88 | /// @return Value if in the observation | ||
| 89 | 145332 | [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override | |
| 90 | { | ||
| 91 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 145332 times.
|
145332 | INS_ASSERT(idx < GetStaticDescriptorCount()); |
| 92 |
3/21✗ Branch 0 not taken.
✗ Branch 1 not taken.
✓ Branch 2 taken 48444 times.
✓ Branch 3 taken 48444 times.
✓ Branch 4 taken 48444 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
145332 | switch (idx) |
| 93 | { | ||
| 94 | ✗ | case 0: // Latitude [deg] | |
| 95 | ✗ | return rad2deg(latitude()); | |
| 96 | ✗ | case 1: // Longitude [deg] | |
| 97 | ✗ | return rad2deg(longitude()); | |
| 98 | 48444 | case 2: // Altitude [m] | |
| 99 |
1/2✓ Branch 1 taken 48444 times.
✗ Branch 2 not taken.
|
48444 | return altitude(); |
| 100 | 48444 | case 3: // North/South [m] | |
| 101 |
1/2✓ Branch 2 taken 48444 times.
✗ Branch 3 not taken.
|
48444 | return CommonLog::calcLocalPosition(lla_position()).northSouth; |
| 102 | 48444 | case 4: // East/West [m] | |
| 103 |
1/2✓ Branch 2 taken 48444 times.
✗ Branch 3 not taken.
|
48444 | return CommonLog::calcLocalPosition(lla_position()).eastWest; |
| 104 | ✗ | case 5: // X-ECEF [m] | |
| 105 | ✗ | return e_position().x(); | |
| 106 | ✗ | case 6: // Y-ECEF [m] | |
| 107 | ✗ | return e_position().y(); | |
| 108 | ✗ | case 7: // Z-ECEF [m] | |
| 109 | ✗ | return e_position().z(); | |
| 110 | ✗ | case 8: // X-ECEF StDev [m] | |
| 111 | ✗ | if (auto stDev = e_positionStdev()) { return stDev->get().x(); } | |
| 112 | ✗ | break; | |
| 113 | ✗ | case 9: // Y-ECEF StDev [m] | |
| 114 | ✗ | if (auto stDev = e_positionStdev()) { return stDev->get().y(); } | |
| 115 | ✗ | break; | |
| 116 | ✗ | case 10: // Z-ECEF StDev [m] | |
| 117 | ✗ | if (auto stDev = e_positionStdev()) { return stDev->get().z(); } | |
| 118 | ✗ | break; | |
| 119 | ✗ | case 11: // XY-ECEF StDev [m] | |
| 120 | ✗ | if (e_CovarianceMatrix().has_value()) { return (*e_CovarianceMatrix())(States::PosX, States::PosY); } | |
| 121 | ✗ | break; | |
| 122 | ✗ | case 12: // XZ-ECEF StDev [m] | |
| 123 | ✗ | if (e_CovarianceMatrix().has_value()) { return (*e_CovarianceMatrix())(States::PosX, States::PosZ); } | |
| 124 | ✗ | break; | |
| 125 | ✗ | case 13: // YZ-ECEF StDev [m] | |
| 126 | ✗ | if (e_CovarianceMatrix().has_value()) { return (*e_CovarianceMatrix())(States::PosY, States::PosZ); } | |
| 127 | ✗ | break; | |
| 128 | ✗ | case 14: // North StDev [m] | |
| 129 | ✗ | if (auto stDev = n_positionStdev()) { return stDev->get().x(); } | |
| 130 | ✗ | break; | |
| 131 | ✗ | case 15: // East StDev [m] | |
| 132 | ✗ | if (auto stDev = n_positionStdev()) { return stDev->get().y(); } | |
| 133 | ✗ | break; | |
| 134 | ✗ | case 16: // Down StDev [m] | |
| 135 | ✗ | if (auto stDev = n_positionStdev()) { return stDev->get().z(); } | |
| 136 | ✗ | break; | |
| 137 | ✗ | case 17: // NE StDev [m] | |
| 138 | ✗ | if (n_CovarianceMatrix().has_value()) { return (*n_CovarianceMatrix())(States::PosX, States::PosY); } | |
| 139 | ✗ | break; | |
| 140 | ✗ | case 18: // ND StDev [m] | |
| 141 | ✗ | if (n_CovarianceMatrix().has_value()) { return (*n_CovarianceMatrix())(States::PosX, States::PosZ); } | |
| 142 | ✗ | break; | |
| 143 | ✗ | case 19: // ED StDev [m] | |
| 144 | ✗ | if (n_CovarianceMatrix().has_value()) { return (*n_CovarianceMatrix())(States::PosY, States::PosZ); } | |
| 145 | ✗ | break; | |
| 146 | ✗ | default: | |
| 147 | ✗ | return std::nullopt; | |
| 148 | } | ||
| 149 | ✗ | return std::nullopt; | |
| 150 | } | ||
| 151 | |||
| 152 | /// @brief Set the value at the index | ||
| 153 | /// @param idx Index corresponding to data descriptor order | ||
| 154 | /// @param value Value to set | ||
| 155 | /// @return True if the value was updated | ||
| 156 | ✗ | [[nodiscard]] bool setValueAt(size_t idx, double value) override | |
| 157 | { | ||
| 158 | ✗ | INS_ASSERT(idx < GetStaticDescriptorCount()); | |
| 159 | ✗ | switch (idx) | |
| 160 | { | ||
| 161 | ✗ | case 0: // Latitude [deg] | |
| 162 | ✗ | _lla_position(0) = deg2rad(value); | |
| 163 | ✗ | _e_position = trafo::lla2ecef_WGS84(_lla_position); | |
| 164 | ✗ | break; | |
| 165 | ✗ | case 1: // Longitude [deg] | |
| 166 | ✗ | _lla_position(1) = deg2rad(value); | |
| 167 | ✗ | _e_position = trafo::lla2ecef_WGS84(_lla_position); | |
| 168 | ✗ | break; | |
| 169 | ✗ | case 2: // Altitude [m] | |
| 170 | ✗ | _lla_position(2) = value; | |
| 171 | ✗ | _e_position = trafo::lla2ecef_WGS84(_lla_position); | |
| 172 | ✗ | break; | |
| 173 | ✗ | case 3: // North/South [m] | |
| 174 | ✗ | _e_position += e_Quat_n() * Eigen::Vector3d(value, 0.0, 0.0); | |
| 175 | ✗ | _e_position = trafo::lla2ecef_WGS84(_lla_position); | |
| 176 | ✗ | break; | |
| 177 | ✗ | case 4: // East/West [m] | |
| 178 | ✗ | _e_position += e_Quat_n() * Eigen::Vector3d(0.0, value, 0.0); | |
| 179 | ✗ | _e_position = trafo::lla2ecef_WGS84(_lla_position); | |
| 180 | ✗ | break; | |
| 181 | ✗ | case 5: // X-ECEF [m] | |
| 182 | ✗ | _e_position(0) = value; | |
| 183 | ✗ | _lla_position = trafo::ecef2lla_WGS84(_e_position); | |
| 184 | ✗ | break; | |
| 185 | ✗ | case 6: // Y-ECEF [m] | |
| 186 | ✗ | _e_position(1) = value; | |
| 187 | ✗ | _lla_position = trafo::ecef2lla_WGS84(_e_position); | |
| 188 | ✗ | break; | |
| 189 | ✗ | case 7: // Z-ECEF [m] | |
| 190 | ✗ | _e_position(2) = value; | |
| 191 | ✗ | _lla_position = trafo::ecef2lla_WGS84(_e_position); | |
| 192 | ✗ | break; | |
| 193 | ✗ | case 8: // X-ECEF StDev [m] | |
| 194 | case 9: // Y-ECEF StDev [m] | ||
| 195 | case 10: // Z-ECEF StDev [m] | ||
| 196 | case 11: // XY-ECEF StDev [m] | ||
| 197 | case 12: // XZ-ECEF StDev [m] | ||
| 198 | case 13: // YZ-ECEF StDev [m] | ||
| 199 | case 14: // North StDev [m] | ||
| 200 | case 15: // East StDev [m] | ||
| 201 | case 16: // Down StDev [m] | ||
| 202 | case 17: // NE StDev [m] | ||
| 203 | case 18: // ND StDev [m] | ||
| 204 | case 19: // ED StDev [m] | ||
| 205 | default: | ||
| 206 | ✗ | return false; | |
| 207 | } | ||
| 208 | |||
| 209 | ✗ | return true; | |
| 210 | } | ||
| 211 | |||
| 212 | /* -------------------------------------------------------------------------------------------------------- */ | ||
| 213 | /* Rotation Quaternions */ | ||
| 214 | /* -------------------------------------------------------------------------------------------------------- */ | ||
| 215 | |||
| 216 | /// @brief Returns the Quaternion from navigation to Earth-fixed frame | ||
| 217 | /// @return The Quaternion for the rotation from navigation to earth coordinates | ||
| 218 | 1234999 | [[nodiscard]] Eigen::Quaterniond e_Quat_n() const | |
| 219 | { | ||
| 220 | 1234999 | return trafo::e_Quat_n(latitude(), longitude()); | |
| 221 | } | ||
| 222 | |||
| 223 | /// @brief Returns the Quaternion from Earth-fixed frame to navigation | ||
| 224 | /// @return The Quaternion for the rotation from earth navigation coordinates | ||
| 225 | 435736 | [[nodiscard]] Eigen::Quaterniond n_Quat_e() const | |
| 226 | { | ||
| 227 |
2/4✓ Branch 1 taken 435724 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 435725 times.
✗ Branch 5 not taken.
|
435736 | return e_Quat_n().conjugate(); |
| 228 | } | ||
| 229 | |||
| 230 | /* -------------------------------------------------------------------------------------------------------- */ | ||
| 231 | /* Position */ | ||
| 232 | /* -------------------------------------------------------------------------------------------------------- */ | ||
| 233 | |||
| 234 | /// @brief States | ||
| 235 | struct States | ||
| 236 | { | ||
| 237 | /// @brief Constructor | ||
| 238 | States() = delete; | ||
| 239 | |||
| 240 | /// @brief State Keys | ||
| 241 | enum StateKeys : uint8_t | ||
| 242 | { | ||
| 243 | PosX, ///< Position ECEF_X [m] | ||
| 244 | PosY, ///< Position ECEF_Y [m] | ||
| 245 | PosZ, ///< Position ECEF_Z [m] | ||
| 246 | States_COUNT, ///< Count | ||
| 247 | }; | ||
| 248 | /// @brief All position keys | ||
| 249 | inline static const std::vector<StateKeys> Pos = { PosX, PosY, PosZ }; | ||
| 250 | }; | ||
| 251 | |||
| 252 | /// Returns the latitude 𝜙, longitude λ and altitude (height above ground) in [rad, rad, m] | ||
| 253 | 3449940 | [[nodiscard]] const Eigen::Vector3d& lla_position() const { return _lla_position; } | |
| 254 | |||
| 255 | /// Returns the latitude 𝜙 in [rad] | ||
| 256 | 1499426 | [[nodiscard]] const double& latitude() const { return lla_position()(0); } | |
| 257 | |||
| 258 | /// Returns the longitude λ in [rad] | ||
| 259 | 1499516 | [[nodiscard]] const double& longitude() const { return lla_position()(1); } | |
| 260 | |||
| 261 | /// Returns the altitude (height above ground) in [m] | ||
| 262 | 51057 | [[nodiscard]] const double& altitude() const { return lla_position()(2); } | |
| 263 | |||
| 264 | /// Returns the coordinates in [m] | ||
| 265 | 94083 | [[nodiscard]] const Eigen::Vector3d& e_position() const { return _e_position; } | |
| 266 | |||
| 267 | /// Returns the standard deviation of the position in ECEF frame coordinates in [m] | ||
| 268 | 2648 | [[nodiscard]] std::optional<std::reference_wrapper<const Eigen::Vector3d>> e_positionStdev() const { return _e_positionStdev; } | |
| 269 | |||
| 270 | /// Returns the standard deviation of the position in local navigation frame coordinates in [m] | ||
| 271 | 55 | [[nodiscard]] std::optional<std::reference_wrapper<const Eigen::Vector3d>> n_positionStdev() const { return _n_positionStdev; } | |
| 272 | |||
| 273 | /// Returns the Covariance matrix in ECEF frame | ||
| 274 | ✗ | [[nodiscard]] std::optional<std::reference_wrapper<const KeyedMatrixXd<States::StateKeys, States::StateKeys>>> e_CovarianceMatrix() const { return _e_covarianceMatrix; } | |
| 275 | |||
| 276 | /// Returns the Covariance matrix in local navigation frame | ||
| 277 | ✗ | [[nodiscard]] std::optional<std::reference_wrapper<const KeyedMatrixXd<States::StateKeys, States::StateKeys>>> n_CovarianceMatrix() const { return _n_covarianceMatrix; } | |
| 278 | |||
| 279 | // ########################################################################################################### | ||
| 280 | // Setter | ||
| 281 | // ########################################################################################################### | ||
| 282 | |||
| 283 | /// @brief Set the Position in coordinates | ||
| 284 | /// @param[in] e_position New Position in ECEF coordinates | ||
| 285 | template<typename Derived> | ||
| 286 | 69293 | void setPosition_e(const Eigen::MatrixBase<Derived>& e_position) | |
| 287 | { | ||
| 288 | 69293 | _e_position = e_position; | |
| 289 |
1/2✓ Branch 1 taken 55109 times.
✗ Branch 2 not taken.
|
69293 | _lla_position = trafo::ecef2lla_WGS84(e_position); |
| 290 | 69293 | } | |
| 291 | |||
| 292 | /// @brief Set the Position lla object | ||
| 293 | /// @param[in] lla_position New Position in LatLonAlt coordinates | ||
| 294 | template<typename Derived> | ||
| 295 | 244313 | void setPosition_lla(const Eigen::MatrixBase<Derived>& lla_position) | |
| 296 | { | ||
| 297 |
1/2✓ Branch 1 taken 180889 times.
✗ Branch 2 not taken.
|
244313 | _e_position = trafo::lla2ecef_WGS84(lla_position); |
| 298 | 244291 | _lla_position = lla_position; | |
| 299 | 244308 | } | |
| 300 | |||
| 301 | /// @brief Set the Position in ECEF coordinates and its standard deviation | ||
| 302 | /// @param[in] e_position New Position in ECEF coordinates [m] | ||
| 303 | /// @param[in] e_positionCovarianceMatrix Covariance matrix of position in ECEF coordinates [m] | ||
| 304 | template<typename Derived, typename Derived2> | ||
| 305 | 81272 | void setPositionAndStdDev_e(const Eigen::MatrixBase<Derived>& e_position, const Eigen::MatrixBase<Derived2>& e_positionCovarianceMatrix) | |
| 306 | { | ||
| 307 | 81272 | setPosition_e(e_position); | |
| 308 |
3/6✓ Branch 1 taken 40906 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40906 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 40906 times.
✗ Branch 8 not taken.
|
81272 | _e_positionStdev = e_positionCovarianceMatrix.diagonal().cwiseSqrt(); |
| 309 |
7/14✓ Branch 1 taken 40906 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40906 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 40906 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 40906 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 40906 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 40906 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 40906 times.
✗ Branch 20 not taken.
|
81272 | _n_positionStdev = (n_Quat_e() * e_positionCovarianceMatrix * e_Quat_n()).diagonal().cwiseSqrt(); |
| 310 | 81272 | } | |
| 311 | |||
| 312 | /// @brief Set the Position in LLA coordinates and its standard deviation | ||
| 313 | /// @param[in] lla_position New Position in LatLonAlt coordinates | ||
| 314 | /// @param[in] n_positionCovarianceMatrix Covariance matrix of Position in NED coordinates [m] | ||
| 315 | template<typename Derived, typename Derived2> | ||
| 316 | 89867 | void setPositionAndStdDev_lla(const Eigen::MatrixBase<Derived>& lla_position, const Eigen::MatrixBase<Derived2>& n_positionCovarianceMatrix) | |
| 317 | { | ||
| 318 | 89867 | setPosition_lla(lla_position); | |
| 319 |
3/6✓ Branch 1 taken 89852 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 89855 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 89867 times.
✗ Branch 8 not taken.
|
89867 | _n_positionStdev = n_positionCovarianceMatrix.diagonal().cwiseSqrt(); |
| 320 |
7/14✓ Branch 1 taken 89862 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 89864 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 89864 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 89867 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 89855 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 89848 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 89864 times.
✗ Branch 20 not taken.
|
89867 | _e_positionStdev = (e_Quat_n() * n_positionCovarianceMatrix * n_Quat_e()).diagonal().cwiseSqrt(); |
| 321 | 89864 | } | |
| 322 | |||
| 323 | /// @brief Set the Covariance matrix in ECEF coordinates | ||
| 324 | /// @param[in] e_covarianceMatrix 3x3 Pos Error variance | ||
| 325 | /// @attention Position has to be set before calling this | ||
| 326 | template<typename Derived> | ||
| 327 | 538 | void setPosCovarianceMatrix_e(const Eigen::MatrixBase<Derived>& e_covarianceMatrix) | |
| 328 | { | ||
| 329 |
1/2✓ Branch 1 taken 538 times.
✗ Branch 2 not taken.
|
538 | INS_ASSERT_USER_ERROR(e_covarianceMatrix.rows() == 3, "This function needs a 3x3 matrix as input"); |
| 330 |
1/2✓ Branch 1 taken 538 times.
✗ Branch 2 not taken.
|
538 | INS_ASSERT_USER_ERROR(e_covarianceMatrix.cols() == 3, "This function needs a 3x3 matrix as input"); |
| 331 | |||
| 332 |
1/2✓ Branch 1 taken 538 times.
✗ Branch 2 not taken.
|
538 | _e_covarianceMatrix = KeyedMatrixXd<States::StateKeys, |
| 333 | States::StateKeys>(e_covarianceMatrix, | ||
| 334 | States::Pos); | ||
| 335 |
6/12✓ Branch 1 taken 538 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 538 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 538 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 538 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 538 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 538 times.
✗ Branch 17 not taken.
|
1076 | _n_covarianceMatrix = KeyedMatrixXd<States::StateKeys, |
| 336 | States::StateKeys>(n_Quat_e() | ||
| 337 | 538 | * (*_e_covarianceMatrix)(States::Pos, States::Pos) | |
| 338 | * e_Quat_n(), | ||
| 339 | States::Pos); | ||
| 340 | 538 | } | |
| 341 | |||
| 342 | /// @brief Set the Covariance matrix in ECEF coordinates | ||
| 343 | /// @param[in] n_covarianceMatrix 3x3 Pos Error variance | ||
| 344 | /// @attention Position has to be set before calling this | ||
| 345 | template<typename Derived> | ||
| 346 | 1885 | void setPosCovarianceMatrix_n(const Eigen::MatrixBase<Derived>& n_covarianceMatrix) | |
| 347 | { | ||
| 348 |
1/2✓ Branch 1 taken 1885 times.
✗ Branch 2 not taken.
|
1885 | INS_ASSERT_USER_ERROR(n_covarianceMatrix.rows() == 3, "This function needs a 3x3 matrix as input"); |
| 349 |
1/2✓ Branch 1 taken 1885 times.
✗ Branch 2 not taken.
|
1885 | INS_ASSERT_USER_ERROR(n_covarianceMatrix.cols() == 3, "This function needs a 3x3 matrix as input"); |
| 350 | |||
| 351 |
1/2✓ Branch 1 taken 1885 times.
✗ Branch 2 not taken.
|
1885 | _n_covarianceMatrix = KeyedMatrixXd<States::StateKeys, |
| 352 | States::StateKeys>(n_covarianceMatrix, | ||
| 353 | States::Pos); | ||
| 354 |
6/12✓ Branch 1 taken 1885 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1885 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1885 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1885 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1885 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1885 times.
✗ Branch 17 not taken.
|
3770 | _e_covarianceMatrix = KeyedMatrixXd<States::StateKeys, |
| 355 | States::StateKeys>(e_Quat_n() | ||
| 356 | 1885 | * (*_n_covarianceMatrix)(States::Pos, States::Pos) | |
| 357 | * n_Quat_e(), | ||
| 358 | States::Pos); | ||
| 359 | 1885 | } | |
| 360 | |||
| 361 | /* -------------------------------------------------------------------------------------------------------- */ | ||
| 362 | /* Member variables */ | ||
| 363 | /* -------------------------------------------------------------------------------------------------------- */ | ||
| 364 | |||
| 365 | private: | ||
| 366 | /// Position in ECEF coordinates [m] | ||
| 367 | Eigen::Vector3d _e_position{ std::nan(""), std::nan(""), std::nan("") }; | ||
| 368 | /// Position in LatLonAlt coordinates [rad, rad, m] | ||
| 369 | Eigen::Vector3d _lla_position{ std::nan(""), std::nan(""), std::nan("") }; | ||
| 370 | |||
| 371 | /// Standard deviation of Position in ECEF coordinates [m] | ||
| 372 | std::optional<Eigen::Vector3d> _e_positionStdev; | ||
| 373 | /// Standard deviation of Position in local navigation frame coordinates [m] | ||
| 374 | std::optional<Eigen::Vector3d> _n_positionStdev; | ||
| 375 | |||
| 376 | /// Covariance matrix in ECEF coordinates | ||
| 377 | std::optional<KeyedMatrixXd<States::StateKeys, States::StateKeys>> _e_covarianceMatrix; | ||
| 378 | |||
| 379 | /// Covariance matrix in local navigation coordinates | ||
| 380 | std::optional<KeyedMatrixXd<States::StateKeys, States::StateKeys>> _n_covarianceMatrix; | ||
| 381 | }; | ||
| 382 | |||
| 383 | } // namespace NAV | ||
| 384 |