INSTINCT Code Coverage Report

Directory: src/
File: NodeData/State/PosVel.hpp
Date: 2025-06-02 15:19:59
Exec Total Coverage
Lines: 83 177 46.9%
Functions: 20 31 64.5%
Branches: 84 336 25.0%
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 and Velocity 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 and Velocity 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 310158 [[nodiscard]] static std::string type()
28 {
29
1/2
✓ Branch 1 taken 310158 times.
✗ Branch 2 not taken.
 620316 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 581354 [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
47 {
48 581354 auto desc = Pos::GetStaticDataDescriptors();
49
1/2
✓ Branch 2 taken 581354 times.
✗ Branch 3 not taken.
 581354 desc.reserve(GetStaticDescriptorCount());
50
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("Velocity norm [m/s]");
51
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("X velocity ECEF [m/s]");
52
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("Y velocity ECEF [m/s]");
53
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("Z velocity ECEF [m/s]");
54
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("North velocity [m/s]");
55
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("East velocity [m/s]");
56
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("Down velocity [m/s]");
57
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("X velocity ECEF StDev [m/s]");
58
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("Y velocity ECEF StDev [m/s]");
59
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("Z velocity ECEF StDev [m/s]");
60
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("XY velocity StDev [m]");
61
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("XZ velocity StDev [m]");
62
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("YZ velocity StDev [m]");
63
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("North velocity StDev [m/s]");
64
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("East velocity StDev [m/s]");
65
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("Down velocity StDev [m/s]");
66
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("NE velocity StDev [m]");
67
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("ND velocity StDev [m]");
68
1/2
✓ Branch 1 taken 581354 times.
✗ Branch 2 not taken.
 581354 desc.emplace_back("ED velocity StDev [m]");
69 581354 return desc;
70 ✗ }
71
72 /// @brief Get the amount of descriptors
73 2761339 [[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 145329 [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
85 {
86
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 145329 times.
 145329 INS_ASSERT(idx < GetStaticDescriptorCount());
87
1/2
✓ Branch 1 taken 145329 times.
✗ Branch 2 not taken.
 145329 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->x(); }
106 ✗ break;
107 ✗ case Pos::GetStaticDescriptorCount() + 8: // Y velocity ECEF StDev [m/s]
108 ✗ if (auto stDev = e_velocityStdev()) { return stDev->y(); }
109 ✗ break;
110 ✗ case Pos::GetStaticDescriptorCount() + 9: // Z velocity ECEF StDev [m/s]
111 ✗ if (auto stDev = e_velocityStdev()) { return stDev->z(); }
112 ✗ break;
113 ✗ case Pos::GetStaticDescriptorCount() + 10: // XY velocity StDev [m]
114 ✗ if (_e_covarianceMatrix && _e_covarianceMatrix->hasRows(Keys::Vel<Keys::MotionModelKey>)) { return std::sqrt(std::abs((*_e_covarianceMatrix)(Keys::VelX, Keys::VelY))); }
115 ✗ break;
116 ✗ case Pos::GetStaticDescriptorCount() + 11: // XZ velocity StDev [m]
117 ✗ if (_e_covarianceMatrix && _e_covarianceMatrix->hasRows(Keys::Vel<Keys::MotionModelKey>)) { return std::sqrt(std::abs((*_e_covarianceMatrix)(Keys::VelX, Keys::VelZ))); }
118 ✗ break;
119 ✗ case Pos::GetStaticDescriptorCount() + 12: // YZ velocity StDev [m]
120 ✗ if (_e_covarianceMatrix && _e_covarianceMatrix->hasRows(Keys::Vel<Keys::MotionModelKey>)) { return std::sqrt(std::abs((*_e_covarianceMatrix)(Keys::VelY, Keys::VelZ))); }
121 ✗ break;
122 ✗ case Pos::GetStaticDescriptorCount() + 13: // North velocity StDev [m/s]
123 ✗ if (auto stDev = n_velocityStdev()) { return stDev->x(); }
124 ✗ break;
125 ✗ case Pos::GetStaticDescriptorCount() + 14: // East velocity StDev [m/s]
126 ✗ if (auto stDev = n_velocityStdev()) { return stDev->y(); }
127 ✗ break;
128 ✗ case Pos::GetStaticDescriptorCount() + 15: // Down velocity StDev [m/s]
129 ✗ if (auto stDev = n_velocityStdev()) { return stDev->z(); }
130 ✗ break;
131 ✗ case Pos::GetStaticDescriptorCount() + 16: // NE velocity StDev [m]
132 ✗ if (_n_covarianceMatrix && _n_covarianceMatrix->hasRows(Keys::Vel<Keys::MotionModelKey>)) { return std::sqrt(std::abs((*_n_covarianceMatrix)(Keys::VelX, Keys::VelY))); }
133 ✗ break;
134 ✗ case Pos::GetStaticDescriptorCount() + 17: // ND velocity StDev [m]
135 ✗ if (_n_covarianceMatrix && _n_covarianceMatrix->hasRows(Keys::Vel<Keys::MotionModelKey>)) { return std::sqrt(std::abs((*_n_covarianceMatrix)(Keys::VelX, Keys::VelZ))); }
136 ✗ break;
137 ✗ case Pos::GetStaticDescriptorCount() + 18: // ED velocity StDev [m]
138 ✗ if (_n_covarianceMatrix && _n_covarianceMatrix->hasRows(Keys::Vel<Keys::MotionModelKey>)) { return std::sqrt(std::abs((*_n_covarianceMatrix)(Keys::VelY, Keys::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 /// Returns the velocity in [m/s], in earth coordinates
208 47845 [[nodiscard]] const Eigen::Vector3d& e_velocity() const { return _e_velocity; }
209
210 /// Returns the velocity in [m/s], in navigation coordinates
211 123993 [[nodiscard]] const Eigen::Vector3d& n_velocity() const { return _n_velocity; }
212
213 /// Returns the standard deviation of the velocity in [m/s], in earth coordinates
214 52 [[nodiscard]] std::optional<Eigen::Vector3d> e_velocityStdev() const
215 {
216
6/8
✓ Branch 1 taken 52 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 52 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 46 times.
✓ Branch 9 taken 6 times.
✓ Branch 10 taken 46 times.
✓ Branch 11 taken 6 times.
 52 if (_e_covarianceMatrix && _e_covarianceMatrix->hasRows(Keys::Vel<Keys::MotionModelKey>))
217 {
218
4/8
✓ Branch 4 taken 46 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 46 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 46 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 46 times.
✗ Branch 14 not taken.
 46 return (*_e_covarianceMatrix)(Keys::Vel<Keys::MotionModelKey>, Keys::Vel<Keys::MotionModelKey>).diagonal().cwiseSqrt();
219 }
220 6 return std::nullopt;
221 }
222
223 /// Returns the standard deviation of the velocity in [m/s], in navigation coordinates
224 51 [[nodiscard]] std::optional<Eigen::Vector3d> n_velocityStdev() const
225 {
226
6/8
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 51 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 45 times.
✓ Branch 9 taken 6 times.
✓ Branch 10 taken 45 times.
✓ Branch 11 taken 6 times.
 51 if (_n_covarianceMatrix && _e_covarianceMatrix->hasRows(Keys::Vel<Keys::MotionModelKey>))
227 {
228
4/8
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 45 times.
✗ Branch 14 not taken.
 45 return (*_n_covarianceMatrix)(Keys::Vel<Keys::MotionModelKey>, Keys::Vel<Keys::MotionModelKey>).diagonal().cwiseSqrt();
229 }
230 6 return std::nullopt;
231 }
232
233 // ###########################################################################################################
234 // Setter
235 // ###########################################################################################################
236
237 /// @brief Set the Velocity in the earth frame
238 /// @param[in] e_velocity The new velocity in the earth frame
239 template<typename Derived>
240 66650 void setVelocity_e(const Eigen::MatrixBase<Derived>& e_velocity)
241 {
242 66650 _e_velocity = e_velocity;
243
2/4
✓ Branch 1 taken 53762 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 53762 times.
✗ Branch 5 not taken.
 66650 _n_velocity = n_Quat_e() * e_velocity;
244 66650 }
245
246 /// @brief Set the Velocity in the NED frame
247 /// @param[in] n_velocity The new velocity in the NED frame
248 template<typename Derived>
249 189621 void setVelocity_n(const Eigen::MatrixBase<Derived>& n_velocity)
250 {
251
2/4
✓ Branch 1 taken 152515 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 152507 times.
✗ Branch 5 not taken.
 189621 _e_velocity = e_Quat_n() * n_velocity;
252 189604 _n_velocity = n_velocity;
253 189618 }
254
255 /// @brief Set the position and velocity
256 /// @param[in] e_position New Position in ECEF coordinates
257 /// @param[in] e_velocity The new velocity in the earth frame
258 template<typename DerivedP, typename DerivedV>
259 void setPosVel_e(const Eigen::MatrixBase<DerivedP>& e_position, const Eigen::MatrixBase<DerivedV>& e_velocity)
260 {
261 setPosition_e(e_position);
262 setVelocity_e(e_velocity);
263 }
264
265 /// @brief Set the position and velocity
266 /// @param[in] lla_position New Position in LatLonAlt coordinates [rad, rad, m]
267 /// @param[in] n_velocity The new velocity in the NED frame [m/s, m/s, m/s]
268 template<typename DerivedP, typename DerivedV>
269 void setPosVel_n(const Eigen::MatrixBase<DerivedP>& lla_position, const Eigen::MatrixBase<DerivedV>& n_velocity)
270 {
271 setPosition_lla(lla_position);
272 setVelocity_n(n_velocity);
273 }
274
275 /// @brief Set the position, velocity and the covariance matrix
276 /// @param[in] e_position New Position in ECEF coordinates
277 /// @param[in] e_velocity The new velocity in the earth frame
278 /// @param[in] e_covarianceMatrix 6x6 PosVel Error variance
279 template<typename DerivedP, typename DerivedV, typename Derived>
280 1236 void setPosVelAndCov_e(const Eigen::MatrixBase<DerivedP>& e_position, const Eigen::MatrixBase<DerivedV>& e_velocity,
281 const Eigen::MatrixBase<Derived>& e_covarianceMatrix)
282 {
283 1236 setPosition_e(e_position);
284 1236 setVelocity_e(e_velocity);
285 1236 setPosVelCovarianceMatrix_e(e_covarianceMatrix);
286 1236 }
287
288 /// @brief Set the position, velocity and the covariance matrix
289 /// @param[in] lla_position New Position in LatLonAlt coordinates [rad, rad, m]
290 /// @param[in] n_velocity The new velocity in the NED frame [m/s, m/s, m/s]
291 /// @param[in] n_covarianceMatrix 6x6 PosVel Error variance
292 template<typename DerivedP, typename DerivedV, typename Derived>
293 1375 void setPosVelAndCov_n(const Eigen::MatrixBase<DerivedP>& lla_position, const Eigen::MatrixBase<DerivedV>& n_velocity,
294 const Eigen::MatrixBase<Derived>& n_covarianceMatrix)
295 {
296 1375 setPosition_lla(lla_position);
297 1375 setVelocity_n(n_velocity);
298 1375 setPosVelCovarianceMatrix_n(n_covarianceMatrix);
299 1375 }
300
301 /// @brief Set the Covariance matrix in ECEF coordinates
302 /// @param[in] e_covarianceMatrix 6x6 PosVel Error variance
303 /// @attention Position has to be set before calling this
304 template<typename Derived>
305 1238 void setPosVelCovarianceMatrix_e(const Eigen::MatrixBase<Derived>& e_covarianceMatrix)
306 {
307
1/2
✓ Branch 1 taken 895 times.
✗ Branch 2 not taken.
 1238 INS_ASSERT_USER_ERROR(e_covarianceMatrix.rows() == 6, "This function needs a 6x6 matrix as input");
308
1/2
✓ Branch 1 taken 895 times.
✗ Branch 2 not taken.
 1238 INS_ASSERT_USER_ERROR(e_covarianceMatrix.cols() == 6, "This function needs a 6x6 matrix as input");
309
310
1/2
✓ Branch 2 taken 895 times.
✗ Branch 3 not taken.
 1238 _e_covarianceMatrix = KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>(
311 e_covarianceMatrix, Keys::PosVel<Keys::MotionModelKey>);
312
313
1/2
✓ Branch 1 taken 895 times.
✗ Branch 2 not taken.
 1238 Eigen::Quaterniond n_q_e = n_Quat_e();
314
2/4
✓ Branch 1 taken 895 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 895 times.
✗ Branch 5 not taken.
 1238 Eigen::Matrix<double, 6, 6> J = Eigen::Matrix<double, 6, 6>::Zero();
315
3/6
✓ Branch 1 taken 895 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 895 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 895 times.
✗ Branch 8 not taken.
 1238 J.block<3, 3>(0, 0) = n_q_e.toRotationMatrix();
316
3/6
✓ Branch 1 taken 895 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 895 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 895 times.
✗ Branch 8 not taken.
 1238 J.block<3, 3>(3, 3) = n_q_e.toRotationMatrix();
317
318
3/6
✓ Branch 2 taken 895 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 343 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 343 times.
✗ Branch 9 not taken.
 2476 _n_covarianceMatrix = KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>(
319
3/6
✓ Branch 1 taken 895 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 552 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 552 times.
✗ Branch 8 not taken.
 1238 J * e_covarianceMatrix * J.transpose(), Keys::PosVel<Keys::MotionModelKey>);
320 1238 }
321
322 /// @brief Set the Covariance matrix in NED coordinates
323 /// @param[in] n_covarianceMatrix 6x6 PosVel Error variance
324 /// @attention Position has to be set before calling this
325 template<typename Derived>
326 1710 void setPosVelCovarianceMatrix_n(const Eigen::MatrixBase<Derived>& n_covarianceMatrix)
327 {
328
2/2
✓ Branch 1 taken 1709 times.
✓ Branch 2 taken 1 times.
 1710 INS_ASSERT_USER_ERROR(n_covarianceMatrix.rows() == 6, "This function needs a 6x6 matrix as input");
329
1/2
✓ Branch 1 taken 1710 times.
✗ Branch 2 not taken.
 1709 INS_ASSERT_USER_ERROR(n_covarianceMatrix.cols() == 6, "This function needs a 6x6 matrix as input");
330
331
1/2
✓ Branch 2 taken 1722 times.
✗ Branch 3 not taken.
 1710 _n_covarianceMatrix = KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>(
332 n_covarianceMatrix, Keys::PosVel<Keys::MotionModelKey>);
333
334
1/2
✓ Branch 1 taken 1719 times.
✗ Branch 2 not taken.
 1716 Eigen::Quaterniond e_q_n = e_Quat_n();
335
2/4
✓ Branch 1 taken 1695 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1717 times.
✗ Branch 5 not taken.
 1719 Eigen::Matrix<double, 6, 6> J = Eigen::Matrix<double, 6, 6>::Zero();
336
3/6
✓ Branch 1 taken 1709 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1711 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1714 times.
✗ Branch 8 not taken.
 1717 J.block<3, 3>(0, 0) = e_q_n.toRotationMatrix();
337
3/6
✓ Branch 1 taken 1715 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1715 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1718 times.
✗ Branch 8 not taken.
 1714 J.block<3, 3>(3, 3) = e_q_n.toRotationMatrix();
338
339
1/2
✓ Branch 2 taken 1724 times.
✗ Branch 3 not taken.
 3417 _e_covarianceMatrix = KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>(
340
3/6
✓ Branch 1 taken 1701 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1690 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1699 times.
✗ Branch 8 not taken.
 1708 J * n_covarianceMatrix * J.transpose(), Keys::PosVel<Keys::MotionModelKey>);
341 1713 }
342
343 /* -------------------------------------------------------------------------------------------------------- */
344 /* Member variables */
345 /* -------------------------------------------------------------------------------------------------------- */
346
347 private:
348 /// Velocity in earth coordinates [m/s]
349 Eigen::Vector3d _e_velocity{ std::nan(""), std::nan(""), std::nan("") };
350 /// Velocity in navigation coordinates [m/s]
351 Eigen::Vector3d _n_velocity{ std::nan(""), std::nan(""), std::nan("") };
352 };
353
354 } // namespace NAV
355