INSTINCT Code Coverage Report

Directory: src/
File: NodeData/State/PosVel.hpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 78 173 45.1%
Functions: 29 38 76.3%
Branches: 91 348 26.1%
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 PosVel.hpp
10 /// @brief Position, Velocity and Attitude Storage Class
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2021-10-27
13
14 #pragma once
15
16 #include "NodeData/State/Pos.hpp"
17 #include <Eigen/src/Core/Matrix.h>
18
19 namespace NAV
20 {
21 /// Position, Velocity and Attitude Storage Class
22 class PosVel : public Pos
23 {
24 public:
25 /// @brief Returns the type of the data class
26 /// @return The data type
27 1676 [[nodiscard]] static std::string type()
28 {
29
1/2
✓ Branch 1 taken 1676 times.
✗ Branch 2 not taken.
 3352 return "PosVel";
30 }
31
32 /// @brief Returns the type of the data class
33 /// @return The data type
34 ✗ [[nodiscard]] std::string getType() const override { return type(); }
35
36 /// @brief Returns the parent types of the data class
37 /// @return The parent data types
38 672 [[nodiscard]] static std::vector<std::string> parentTypes()
39 {
40 672 auto parent = Pos::parentTypes();
41
2/4
✓ Branch 1 taken 672 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 672 times.
✗ Branch 5 not taken.
 672 parent.push_back(Pos::type());
42 672 return parent;
43 ✗ }
44
45 /// @brief Returns a vector of data descriptors
46 581382 [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
47 {
48 581382 auto desc = Pos::GetStaticDataDescriptors();
49
1/2
✓ Branch 2 taken 581382 times.
✗ Branch 3 not taken.
 581382 desc.reserve(GetStaticDescriptorCount());
50
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("Velocity norm [m/s]");
51
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("X velocity ECEF [m/s]");
52
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("Y velocity ECEF [m/s]");
53
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("Z velocity ECEF [m/s]");
54
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("North velocity [m/s]");
55
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("East velocity [m/s]");
56
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("Down velocity [m/s]");
57
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("X velocity ECEF StDev [m/s]");
58
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("Y velocity ECEF StDev [m/s]");
59
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("Z velocity ECEF StDev [m/s]");
60
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("XY velocity StDev [m]");
61
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("XZ velocity StDev [m]");
62
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("YZ velocity StDev [m]");
63
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("North velocity StDev [m/s]");
64
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("East velocity StDev [m/s]");
65
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("Down velocity StDev [m/s]");
66
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("NE velocity StDev [m]");
67
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("ND velocity StDev [m]");
68
1/2
✓ Branch 1 taken 581382 times.
✗ Branch 2 not taken.
 581382 desc.emplace_back("ED velocity StDev [m]");
69 581382 return desc;
70 ✗ }
71
72 /// @brief Get the amount of descriptors
73 2761440 [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return Pos::GetStaticDescriptorCount() + 19; }
74
75 /// @brief Returns a vector of data descriptors
76 ✗ [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); }
77
78 /// @brief Get the amount of descriptors
79 ✗ [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); }
80
81 /// @brief Get the value at the index
82 /// @param idx Index corresponding to data descriptor order
83 /// @return Value if in the observation
84 145332 [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
85 {
86
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 145332 times.
 145332 INS_ASSERT(idx < GetStaticDescriptorCount());
87
1/2
✓ Branch 1 taken 145332 times.
✗ Branch 2 not taken.
 145332 if (idx < Pos::GetStaticDescriptorCount()) { return Pos::getValueAt(idx); }
88 ✗ switch (idx)
89 {
90 ✗ case Pos::GetStaticDescriptorCount() + 0: // Velocity norm [m/s]
91 ✗ return e_velocity().norm();
92 ✗ case Pos::GetStaticDescriptorCount() + 1: // X velocity ECEF [m/s]
93 ✗ return e_velocity().x();
94 ✗ case Pos::GetStaticDescriptorCount() + 2: // Y velocity ECEF [m/s]
95 ✗ return e_velocity().y();
96 ✗ case Pos::GetStaticDescriptorCount() + 3: // Z velocity ECEF [m/s]
97 ✗ return e_velocity().z();
98 ✗ case Pos::GetStaticDescriptorCount() + 4: // North velocity [m/s]
99 ✗ return n_velocity().x();
100 ✗ case Pos::GetStaticDescriptorCount() + 5: // East velocity [m/s]
101 ✗ return n_velocity().y();
102 ✗ case Pos::GetStaticDescriptorCount() + 6: // Down velocity [m/s]
103 ✗ return n_velocity().z();
104 ✗ case Pos::GetStaticDescriptorCount() + 7: // X velocity ECEF StDev [m/s]
105 ✗ if (auto stDev = e_velocityStdev()) { return stDev->get().x(); }
106 ✗ break;
107 ✗ case Pos::GetStaticDescriptorCount() + 8: // Y velocity ECEF StDev [m/s]
108 ✗ if (auto stDev = e_velocityStdev()) { return stDev->get().y(); }
109 ✗ break;
110 ✗ case Pos::GetStaticDescriptorCount() + 9: // Z velocity ECEF StDev [m/s]
111 ✗ if (auto stDev = e_velocityStdev()) { return stDev->get().z(); }
112 ✗ break;
113 ✗ case Pos::GetStaticDescriptorCount() + 10: // XY velocity StDev [m]
114 ✗ if (e_CovarianceMatrix().has_value() && (*e_CovarianceMatrix()).get().hasAnyCols(States::Vel)) { return (*e_CovarianceMatrix())(States::VelX, States::VelY); }
115 ✗ break;
116 ✗ case Pos::GetStaticDescriptorCount() + 11: // XZ velocity StDev [m]
117 ✗ if (e_CovarianceMatrix().has_value() && (*e_CovarianceMatrix()).get().hasAnyCols(States::Vel)) { return (*e_CovarianceMatrix())(States::VelX, States::VelZ); }
118 ✗ break;
119 ✗ case Pos::GetStaticDescriptorCount() + 12: // YZ velocity StDev [m]
120 ✗ if (e_CovarianceMatrix().has_value() && (*e_CovarianceMatrix()).get().hasAnyCols(States::Vel)) { return (*e_CovarianceMatrix())(States::VelY, States::VelZ); }
121 ✗ break;
122 ✗ case Pos::GetStaticDescriptorCount() + 13: // North velocity StDev [m/s]
123 ✗ if (auto stDev = n_velocityStdev()) { return stDev->get().x(); }
124 ✗ break;
125 ✗ case Pos::GetStaticDescriptorCount() + 14: // East velocity StDev [m/s]
126 ✗ if (auto stDev = n_velocityStdev()) { return stDev->get().y(); }
127 ✗ break;
128 ✗ case Pos::GetStaticDescriptorCount() + 15: // Down velocity StDev [m/s]
129 ✗ if (auto stDev = n_velocityStdev()) { return stDev->get().z(); }
130 ✗ break;
131 ✗ case Pos::GetStaticDescriptorCount() + 16: // NE velocity StDev [m]
132 ✗ if (n_CovarianceMatrix().has_value() && (*n_CovarianceMatrix()).get().hasAnyCols(States::Vel)) { return (*n_CovarianceMatrix())(States::VelX, States::VelY); }
133 ✗ break;
134 ✗ case Pos::GetStaticDescriptorCount() + 17: // ND velocity StDev [m]
135 ✗ if (n_CovarianceMatrix().has_value() && (*n_CovarianceMatrix()).get().hasAnyCols(States::Vel)) { return (*n_CovarianceMatrix())(States::VelX, States::VelZ); }
136 ✗ break;
137 ✗ case Pos::GetStaticDescriptorCount() + 18: // ED velocity StDev [m]
138 ✗ if (n_CovarianceMatrix().has_value() && (*n_CovarianceMatrix()).get().hasAnyCols(States::Vel)) { return (*n_CovarianceMatrix())(States::VelY, States::VelZ); }
139 ✗ break;
140 ✗ default:
141 ✗ return std::nullopt;
142 }
143 ✗ return std::nullopt;
144 }
145
146 /// @brief Set the value at the index
147 /// @param idx Index corresponding to data descriptor order
148 /// @param value Value to set
149 /// @return True if the value was updated
150 ✗ [[nodiscard]] bool setValueAt(size_t idx, double value) override
151 {
152 ✗ INS_ASSERT(idx < GetStaticDescriptorCount());
153 ✗ if (idx < Pos::GetStaticDescriptorCount()) { return Pos::setValueAt(idx, value); }
154 ✗ switch (idx)
155 {
156 ✗ case Pos::GetStaticDescriptorCount() + 0: // Velocity norm [m/s]
157 ✗ _e_velocity = value * _e_velocity.normalized();
158 ✗ _n_velocity = value * _n_velocity.normalized();
159 ✗ break;
160 ✗ case Pos::GetStaticDescriptorCount() + 1: // X velocity ECEF [m/s]
161 ✗ _e_velocity(0) = value;
162 ✗ _n_velocity = n_Quat_e() * _e_velocity;
163 ✗ break;
164 ✗ case Pos::GetStaticDescriptorCount() + 2: // Y velocity ECEF [m/s]
165 ✗ _e_velocity(1) = value;
166 ✗ _n_velocity = n_Quat_e() * _e_velocity;
167 ✗ break;
168 ✗ case Pos::GetStaticDescriptorCount() + 3: // Z velocity ECEF [m/s]
169 ✗ _e_velocity(2) = value;
170 ✗ _n_velocity = n_Quat_e() * _e_velocity;
171 ✗ break;
172 ✗ case Pos::GetStaticDescriptorCount() + 4: // North velocity [m/s]
173 ✗ _n_velocity(0) = value;
174 ✗ _e_velocity = e_Quat_n() * _n_velocity;
175 ✗ break;
176 ✗ case Pos::GetStaticDescriptorCount() + 5: // East velocity [m/s]
177 ✗ _n_velocity(1) = value;
178 ✗ _e_velocity = e_Quat_n() * _n_velocity;
179 ✗ break;
180 ✗ case Pos::GetStaticDescriptorCount() + 6: // Down velocity [m/s]
181 ✗ _n_velocity(2) = value;
182 ✗ _e_velocity = e_Quat_n() * _n_velocity;
183 ✗ break;
184 ✗ case Pos::GetStaticDescriptorCount() + 7: // X velocity ECEF StDev [m/s]
185 case Pos::GetStaticDescriptorCount() + 8: // Y velocity ECEF StDev [m/s]
186 case Pos::GetStaticDescriptorCount() + 9: // Z velocity ECEF StDev [m/s]
187 case Pos::GetStaticDescriptorCount() + 10: // XY velocity StDev [m]
188 case Pos::GetStaticDescriptorCount() + 11: // XZ velocity StDev [m]
189 case Pos::GetStaticDescriptorCount() + 12: // YZ velocity StDev [m]
190 case Pos::GetStaticDescriptorCount() + 13: // North velocity StDev [m/s]
191 case Pos::GetStaticDescriptorCount() + 14: // East velocity StDev [m/s]
192 case Pos::GetStaticDescriptorCount() + 15: // Down velocity StDev [m/s]
193 case Pos::GetStaticDescriptorCount() + 16: // NE velocity StDev [m]
194 case Pos::GetStaticDescriptorCount() + 17: // ND velocity StDev [m]
195 case Pos::GetStaticDescriptorCount() + 18: // ED velocity StDev [m]
196 default:
197 ✗ return false;
198 }
199
200 ✗ return true;
201 }
202
203 /* -------------------------------------------------------------------------------------------------------- */
204 /* Velocity */
205 /* -------------------------------------------------------------------------------------------------------- */
206
207 /// @brief States
208 struct States
209 {
210 /// @brief Constructor
211 States() = delete;
212
213 /// @brief State Keys
214 enum StateKeys : uint8_t
215 {
216 PosX, ///< Position ECEF_X [m]
217 PosY, ///< Position ECEF_Y [m]
218 PosZ, ///< Position ECEF_Z [m]
219 VelX, ///< Velocity ECEF_X [m/s]
220 VelY, ///< Velocity ECEF_Y [m/s]
221 VelZ, ///< Velocity ECEF_Z [m/s]
222 States_COUNT, ///< Count
223 };
224 /// @brief All position keys
225 inline static const std::vector<StateKeys> Pos = { PosX, PosY, PosZ };
226 /// @brief All velocity keys
227 inline static const std::vector<StateKeys> Vel = { States::VelX, States::VelY, States::VelZ };
228 /// @brief Vector with all position and velocity state keys
229 inline static const std::vector<StateKeys> PosVel = { States::PosX, States::PosY, States::PosZ,
230 States::VelX, States::VelY, States::VelZ };
231 };
232
233 /// Returns the velocity in [m/s], in earth coordinates
234 64613 [[nodiscard]] const Eigen::Vector3d& e_velocity() const { return _e_velocity; }
235
236 /// Returns the velocity in [m/s], in navigation coordinates
237 239215 [[nodiscard]] const Eigen::Vector3d& n_velocity() const { return _n_velocity; }
238
239 /// Returns the standard deviation of the velocity in [m/s], in earth coordinates
240 2644 [[nodiscard]] std::optional<std::reference_wrapper<const Eigen::Vector3d>> e_velocityStdev() const { return _e_velocityStdev; }
241
242 /// Returns the standard deviation of the velocity in [m/s], in navigation coordinates
243 51 [[nodiscard]] std::optional<std::reference_wrapper<const Eigen::Vector3d>> n_velocityStdev() const { return _n_velocityStdev; }
244
245 /// Returns the Covariance matrix in ECEF frame
246 103121 [[nodiscard]] std::optional<std::reference_wrapper<const KeyedMatrixXd<States::StateKeys, States::StateKeys>>> e_CovarianceMatrix() const { return _e_covarianceMatrix; }
247
248 /// Returns the Covariance matrix in local navigation frame
249 ✗ [[nodiscard]] std::optional<std::reference_wrapper<const KeyedMatrixXd<States::StateKeys, States::StateKeys>>> n_CovarianceMatrix() const { return _n_covarianceMatrix; }
250
251 // ###########################################################################################################
252 // Setter
253 // ###########################################################################################################
254
255 /// @brief Set the Velocity in the earth frame
256 /// @param[in] e_velocity The new velocity in the earth frame
257 template<typename Derived>
258 69295 void setVelocity_e(const Eigen::MatrixBase<Derived>& e_velocity)
259 {
260 69295 _e_velocity = e_velocity;
261
2/4
✓ Branch 1 taken 55111 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 55111 times.
✗ Branch 5 not taken.
 69295 _n_velocity = n_Quat_e() * e_velocity;
262 69295 }
263
264 /// @brief Set the Velocity in the NED frame
265 /// @param[in] n_velocity The new velocity in the NED frame
266 template<typename Derived>
267 243769 void setVelocity_n(const Eigen::MatrixBase<Derived>& n_velocity)
268 {
269
2/4
✓ Branch 1 taken 180359 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 180350 times.
✗ Branch 5 not taken.
 243769 _e_velocity = e_Quat_n() * n_velocity;
270 243747 _n_velocity = n_velocity;
271 243766 }
272
273 /// @brief Set the Velocity in ECEF coordinates and its standard deviation
274 /// @param[in] e_velocity New Velocity in ECEF coordinates [m/s]
275 /// @param[in] e_velocityCovarianceMatrix Covariance matrix of Velocity in earth coordinates [m/s]
276 template<typename Derived, typename Derived2>
277 81272 void setVelocityAndStdDev_e(const Eigen::MatrixBase<Derived>& e_velocity, const Eigen::MatrixBase<Derived2>& e_velocityCovarianceMatrix)
278 {
279 81272 setVelocity_e(e_velocity);
280
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_velocityStdev = e_velocityCovarianceMatrix.diagonal().cwiseSqrt();
281
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_velocityStdev = (n_Quat_e() * e_velocityCovarianceMatrix * e_Quat_n()).diagonal().cwiseSqrt();
282 81272 }
283
284 /// @brief Set the Velocity in NED coordinates and its standard deviation
285 /// @param[in] n_velocity New Velocity in NED coordinates [m/s]
286 /// @param[in] n_velocityCovarianceMatrix Covariance matrix of Velocity in navigation coordinates [m/s]
287 template<typename Derived, typename Derived2>
288 89323 void setVelocityAndStdDev_n(const Eigen::MatrixBase<Derived>& n_velocity, const Eigen::MatrixBase<Derived2>& n_velocityCovarianceMatrix)
289 {
290 89323 setVelocity_n(n_velocity);
291
3/6
✓ Branch 1 taken 89317 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 89311 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 89318 times.
✗ Branch 8 not taken.
 89326 _n_velocityStdev = n_velocityCovarianceMatrix.diagonal().cwiseSqrt();
292
7/14
✓ Branch 1 taken 89327 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 89330 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 89328 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 89328 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 89318 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 89324 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 89322 times.
✗ Branch 20 not taken.
 89318 _e_velocityStdev = (e_Quat_n() * n_velocityCovarianceMatrix * n_Quat_e()).diagonal().cwiseSqrt();
293 89322 }
294
295 /// @brief Set the Covariance matrix in ECEF coordinates
296 /// @param[in] e_covarianceMatrix 6x6 PosVel Error variance
297 /// @attention Position has to be set before calling this
298 template<typename Derived>
299 80734 void setPosVelCovarianceMatrix_e(const Eigen::MatrixBase<Derived>& e_covarianceMatrix)
300 {
301
1/2
✓ Branch 1 taken 40368 times.
✗ Branch 2 not taken.
 80734 INS_ASSERT_USER_ERROR(e_covarianceMatrix.rows() == 6, "This function needs a 6x6 matrix as input");
302
1/2
✓ Branch 1 taken 40368 times.
✗ Branch 2 not taken.
 80734 INS_ASSERT_USER_ERROR(e_covarianceMatrix.cols() == 6, "This function needs a 6x6 matrix as input");
303
304
1/2
✓ Branch 1 taken 40368 times.
✗ Branch 2 not taken.
 80734 _e_covarianceMatrix = KeyedMatrixXd<States::StateKeys,
305 States::StateKeys>(e_covarianceMatrix,
306 States::PosVel);
307
1/2
✓ Branch 1 taken 40368 times.
✗ Branch 2 not taken.
 80734 _n_covarianceMatrix = _e_covarianceMatrix;
308
309
3/6
✓ Branch 1 taken 40368 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40368 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 40368 times.
✗ Branch 8 not taken.
 80734 Eigen::Quaterniond n_Quat_e = trafo::n_Quat_e(latitude(), longitude());
310
3/6
✓ Branch 1 taken 40368 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40368 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 40368 times.
✗ Branch 8 not taken.
 80734 Eigen::Quaterniond e_Quat_n = trafo::e_Quat_n(latitude(), longitude());
311
312
1/2
✓ Branch 3 taken 40368 times.
✗ Branch 4 not taken.
 80734 (*_n_covarianceMatrix)(all, all).setZero();
313
5/10
✓ Branch 2 taken 40368 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 40368 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 40368 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 40368 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 40368 times.
✗ Branch 16 not taken.
 80734 (*_n_covarianceMatrix)(States::Pos, States::Pos) = n_Quat_e * (*_e_covarianceMatrix)(States::Pos, States::Pos) * e_Quat_n;
314
5/10
✓ Branch 2 taken 40368 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 40368 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 40368 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 40368 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 40368 times.
✗ Branch 16 not taken.
 80734 (*_n_covarianceMatrix)(States::Vel, States::Vel) = n_Quat_e * (*_e_covarianceMatrix)(States::Vel, States::Vel) * e_Quat_n;
315 80734 }
316
317 /// @brief Set the Covariance matrix in NED coordinates
318 /// @param[in] n_covarianceMatrix 6x6 PosVel Error variance
319 /// @attention Position has to be set before calling this
320 template<typename Derived>
321 62965 void setPosVelCovarianceMatrix_n(const Eigen::MatrixBase<Derived>& n_covarianceMatrix)
322 {
323
1/2
✓ Branch 1 taken 62967 times.
✗ Branch 2 not taken.
 62965 INS_ASSERT_USER_ERROR(n_covarianceMatrix.rows() == 6, "This function needs a 6x6 matrix as input");
324
1/2
✓ Branch 1 taken 62962 times.
✗ Branch 2 not taken.
 62967 INS_ASSERT_USER_ERROR(n_covarianceMatrix.cols() == 6, "This function needs a 6x6 matrix as input");
325
326
1/2
✓ Branch 1 taken 62975 times.
✗ Branch 2 not taken.
 62962 _n_covarianceMatrix = KeyedMatrixXd<States::StateKeys,
327 States::StateKeys>(n_covarianceMatrix,
328 States::PosVel);
329
1/2
✓ Branch 1 taken 62976 times.
✗ Branch 2 not taken.
 62964 _e_covarianceMatrix = _n_covarianceMatrix;
330
331
3/6
✓ Branch 1 taken 62963 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62968 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 62976 times.
✗ Branch 8 not taken.
 62976 Eigen::Quaterniond n_Quat_e = trafo::n_Quat_e(latitude(), longitude());
332
3/6
✓ Branch 1 taken 62961 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62964 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 62972 times.
✗ Branch 8 not taken.
 62976 Eigen::Quaterniond e_Quat_n = trafo::e_Quat_n(latitude(), longitude());
333
334
1/2
✓ Branch 3 taken 62962 times.
✗ Branch 4 not taken.
 62972 (*_e_covarianceMatrix)(all, all).setZero();
335
5/10
✓ Branch 2 taken 62973 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 62974 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 62974 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 62973 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 62970 times.
✗ Branch 16 not taken.
 62962 (*_e_covarianceMatrix)(States::Pos, States::Pos) = e_Quat_n * (*_n_covarianceMatrix)(States::Pos, States::Pos) * n_Quat_e;
336
5/10
✓ Branch 2 taken 62973 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 62972 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 62970 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 62972 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 62974 times.
✗ Branch 16 not taken.
 62973 (*_e_covarianceMatrix)(States::Vel, States::Vel) = e_Quat_n * (*_n_covarianceMatrix)(States::Vel, States::Vel) * n_Quat_e;
337 62970 }
338
339 /* -------------------------------------------------------------------------------------------------------- */
340 /* Member variables */
341 /* -------------------------------------------------------------------------------------------------------- */
342
343 private:
344 /// Velocity in earth coordinates [m/s]
345 Eigen::Vector3d _e_velocity{ std::nan(""), std::nan(""), std::nan("") };
346 /// Velocity in navigation coordinates [m/s]
347 Eigen::Vector3d _n_velocity{ std::nan(""), std::nan(""), std::nan("") };
348
349 /// Standard deviation of Velocity in earth coordinates [m/s]
350 std::optional<Eigen::Vector3d> _e_velocityStdev;
351 /// Standard deviation of Velocity in navigation coordinates [m/s]
352 std::optional<Eigen::Vector3d> _n_velocityStdev;
353
354 /// Covariance matrix in ECEF coordinates
355 std::optional<KeyedMatrixXd<States::StateKeys, States::StateKeys>> _e_covarianceMatrix;
356
357 /// Covariance matrix in local navigation coordinates
358 std::optional<KeyedMatrixXd<States::StateKeys, States::StateKeys>> _n_covarianceMatrix;
359 };
360
361 } // namespace NAV
362