INSTINCT Code Coverage Report

Directory: src/
File: NodeData/State/PosVel.hpp
Date: 2025-07-19 10:51:51
Exec Total Coverage
Lines: 90 181 49.7%
Functions: 22 33 66.7%
Branches: 90 336 26.8%
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 310640 [[nodiscard]] static std::string type()
28 {
29
1/2
✓ Branch 1 taken 310640 times.
✗ Branch 2 not taken.
 621280 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 798 [[nodiscard]] static std::vector<std::string> parentTypes()
39 {
40 798 auto parent = Pos::parentTypes();
41
2/4
✓ Branch 1 taken 798 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 798 times.
✗ Branch 5 not taken.
 798 parent.push_back(Pos::type());
42 798 return parent;
43 ✗ }
44
45 /// @brief Returns a vector of data descriptors
46 624633 [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
47 {
48 624633 auto desc = Pos::GetStaticDataDescriptors();
49
1/2
✓ Branch 2 taken 624633 times.
✗ Branch 3 not taken.
 624633 desc.reserve(GetStaticDescriptorCount());
50
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("Velocity norm [m/s]");
51
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("X velocity ECEF [m/s]");
52
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("Y velocity ECEF [m/s]");
53
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("Z velocity ECEF [m/s]");
54
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("North velocity [m/s]");
55
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("East velocity [m/s]");
56
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("Down velocity [m/s]");
57
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("X velocity ECEF StDev [m/s]");
58
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("Y velocity ECEF StDev [m/s]");
59
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("Z velocity ECEF StDev [m/s]");
60
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("XY velocity StDev [m]");
61
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("XZ velocity StDev [m]");
62
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("YZ velocity StDev [m]");
63
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("North velocity StDev [m/s]");
64
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("East velocity StDev [m/s]");
65
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("Down velocity StDev [m/s]");
66
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("NE velocity StDev [m]");
67
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("ND velocity StDev [m]");
68
1/2
✓ Branch 1 taken 624633 times.
✗ Branch 2 not taken.
 624633 desc.emplace_back("ED velocity StDev [m]");
69 624633 return desc;
70 ✗ }
71
72 /// @brief Get the amount of descriptors
73 3323313 [[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 264183 [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
85 {
86
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 264183 times.
 264183 INS_ASSERT(idx < GetStaticDescriptorCount());
87
2/2
✓ Branch 1 taken 204756 times.
✓ Branch 2 taken 59427 times.
 264183 if (idx < Pos::GetStaticDescriptorCount()) { return Pos::getValueAt(idx); }
88
3/20
✗ Branch 0 not taken.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 19809 times.
✓ Branch 5 taken 19809 times.
✓ Branch 6 taken 19809 times.
✗ 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.
 59427 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 19809 case Pos::GetStaticDescriptorCount() + 4: // North velocity [m/s]
99
1/2
✓ Branch 2 taken 19809 times.
✗ Branch 3 not taken.
 19809 return n_velocity().x();
100 19809 case Pos::GetStaticDescriptorCount() + 5: // East velocity [m/s]
101
1/2
✓ Branch 2 taken 19809 times.
✗ Branch 3 not taken.
 19809 return n_velocity().y();
102 19809 case Pos::GetStaticDescriptorCount() + 6: // Down velocity [m/s]
103
1/2
✓ Branch 2 taken 19809 times.
✗ Branch 3 not taken.
 19809 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 47847 [[nodiscard]] const Eigen::Vector3d& e_velocity() const { return _e_velocity; }
209
210 /// Returns the velocity in [m/s], in navigation coordinates
211 183420 [[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 73895 void setVelocity_e(const Eigen::MatrixBase<Derived>& e_velocity)
241 {
242 73895 _e_velocity = e_velocity;
243
2/4
✓ Branch 1 taken 61007 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61007 times.
✗ Branch 5 not taken.
 73895 _n_velocity = n_Quat_e() * e_velocity;
244 73895 }
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 190324 void setVelocity_n(const Eigen::MatrixBase<Derived>& n_velocity)
250 {
251
2/4
✓ Branch 1 taken 153218 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 153209 times.
✗ Branch 5 not taken.
 190324 _e_velocity = e_Quat_n() * n_velocity;
252 190304 _n_velocity = n_velocity;
253 190324 }
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 3682 void setPosVelAndCov_e(const Eigen::MatrixBase<DerivedP>& e_position, const Eigen::MatrixBase<DerivedV>& e_velocity,
281 const Eigen::MatrixBase<Derived>& e_covarianceMatrix)
282 {
283 3682 setPosition_e(e_position);
284 3682 setVelocity_e(e_velocity);
285 3682 setPosVelCovarianceMatrix_e(e_covarianceMatrix);
286 3682 }
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 1475 void setPosVelAndCov_n(const Eigen::MatrixBase<DerivedP>& lla_position, const Eigen::MatrixBase<DerivedV>& n_velocity,
294 const Eigen::MatrixBase<Derived>& n_covarianceMatrix)
295 {
296 1475 setPosition_lla(lla_position);
297 1475 setVelocity_n(n_velocity);
298 1475 setPosVelCovarianceMatrix_n(n_covarianceMatrix);
299 1475 }
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 3684 void setPosVelCovarianceMatrix_e(const Eigen::MatrixBase<Derived>& e_covarianceMatrix)
306 {
307
1/2
✓ Branch 1 taken 2138 times.
✗ Branch 2 not taken.
 3684 INS_ASSERT_USER_ERROR(e_covarianceMatrix.rows() == 6, "This function needs a 6x6 matrix as input");
308
1/2
✓ Branch 1 taken 2138 times.
✗ Branch 2 not taken.
 3684 INS_ASSERT_USER_ERROR(e_covarianceMatrix.cols() == 6, "This function needs a 6x6 matrix as input");
309
310
1/2
✓ Branch 2 taken 2138 times.
✗ Branch 3 not taken.
 3684 _e_covarianceMatrix = KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>(
311 e_covarianceMatrix, Keys::PosVel<Keys::MotionModelKey>);
312
313
1/2
✓ Branch 1 taken 2138 times.
✗ Branch 2 not taken.
 3684 Eigen::Quaterniond n_q_e = n_Quat_e();
314
2/4
✓ Branch 1 taken 2138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2138 times.
✗ Branch 5 not taken.
 3684 Eigen::Matrix<double, 6, 6> J = Eigen::Matrix<double, 6, 6>::Zero();
315
3/6
✓ Branch 1 taken 2138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2138 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2138 times.
✗ Branch 8 not taken.
 3684 J.block<3, 3>(0, 0) = n_q_e.toRotationMatrix();
316
3/6
✓ Branch 1 taken 2138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2138 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2138 times.
✗ Branch 8 not taken.
 3684 J.block<3, 3>(3, 3) = n_q_e.toRotationMatrix();
317
318
3/6
✓ Branch 2 taken 2138 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 945 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 945 times.
✗ Branch 9 not taken.
 7368 _n_covarianceMatrix = KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>(
319
3/6
✓ Branch 1 taken 2138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1193 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1193 times.
✗ Branch 8 not taken.
 3684 J * e_covarianceMatrix * J.transpose(), Keys::PosVel<Keys::MotionModelKey>);
320 3684 }
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 2413 void setPosVelCovarianceMatrix_n(const Eigen::MatrixBase<Derived>& n_covarianceMatrix)
327 {
328
1/2
✓ Branch 1 taken 2415 times.
✗ Branch 2 not taken.
 2413 INS_ASSERT_USER_ERROR(n_covarianceMatrix.rows() == 6, "This function needs a 6x6 matrix as input");
329
1/2
✓ Branch 1 taken 2415 times.
✗ Branch 2 not taken.
 2415 INS_ASSERT_USER_ERROR(n_covarianceMatrix.cols() == 6, "This function needs a 6x6 matrix as input");
330
331
1/2
✓ Branch 2 taken 2421 times.
✗ Branch 3 not taken.
 2415 _n_covarianceMatrix = KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>(
332 n_covarianceMatrix, Keys::PosVel<Keys::MotionModelKey>);
333
334
1/2
✓ Branch 1 taken 2423 times.
✗ Branch 2 not taken.
 2420 Eigen::Quaterniond e_q_n = e_Quat_n();
335
2/4
✓ Branch 1 taken 2413 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2424 times.
✗ Branch 5 not taken.
 2423 Eigen::Matrix<double, 6, 6> J = Eigen::Matrix<double, 6, 6>::Zero();
336
3/6
✓ Branch 1 taken 2415 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2412 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2416 times.
✗ Branch 8 not taken.
 2424 J.block<3, 3>(0, 0) = e_q_n.toRotationMatrix();
337
3/6
✓ Branch 1 taken 2421 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2413 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2424 times.
✗ Branch 8 not taken.
 2416 J.block<3, 3>(3, 3) = e_q_n.toRotationMatrix();
338
339
1/2
✓ Branch 2 taken 2425 times.
✗ Branch 3 not taken.
 4836 _e_covarianceMatrix = KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>(
340
3/6
✓ Branch 1 taken 2412 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2409 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2412 times.
✗ Branch 8 not taken.
 2418 J * n_covarianceMatrix * J.transpose(), Keys::PosVel<Keys::MotionModelKey>);
341 2417 }
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