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