INSTINCT Code Coverage Report

Directory: src/
File: NodeData/State/PosVelAtt.hpp
Date: 2025-06-02 15:19:59
Exec Total Coverage
Lines: 103 157 65.6%
Functions: 28 31 90.3%
Branches: 87 283 30.7%
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 PosVelAtt.hpp
10 /// @brief Position, Velocity and Attitude Storage Class
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2020-08-21
13
14 #pragma once
15
16 #include "Navigation/Transformations/CoordinateFrames.hpp"
17
18 #include "Navigation/Transformations/Units.hpp"
19 #include "NodeData/State/PosVel.hpp"
20
21 namespace NAV
22 {
23 /// Position, Velocity and Attitude Storage Class
24 class PosVelAtt : public PosVel
25 {
26 public:
27 /// @brief Returns the type of the data class
28 /// @return The data type
29 317530 [[nodiscard]] static std::string type()
30 {
31
1/2
✓ Branch 1 taken 317529 times.
✗ Branch 2 not taken.
 635059 return "PosVelAtt";
32 }
33
34 /// @brief Returns the type of the data class
35 /// @return The data type
36 ✗ [[nodiscard]] std::string getType() const override { return type(); }
37
38 /// @brief Returns the parent types of the data class
39 /// @return The parent data types
40 224 [[nodiscard]] static std::vector<std::string> parentTypes()
41 {
42 224 auto parent = PosVel::parentTypes();
43
2/4
✓ Branch 1 taken 224 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 224 times.
✗ Branch 5 not taken.
 224 parent.push_back(PosVel::type());
44 224 return parent;
45 ✗ }
46
47 /// @brief Returns a vector of data descriptors
48 581346 [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
49 {
50 581346 auto desc = PosVel::GetStaticDataDescriptors();
51
1/2
✓ Branch 2 taken 581346 times.
✗ Branch 3 not taken.
 581346 desc.reserve(GetStaticDescriptorCount());
52
1/2
✓ Branch 1 taken 581346 times.
✗ Branch 2 not taken.
 581346 desc.emplace_back("Roll [deg]");
53
1/2
✓ Branch 1 taken 581346 times.
✗ Branch 2 not taken.
 581346 desc.emplace_back("Pitch [deg]");
54
1/2
✓ Branch 1 taken 581346 times.
✗ Branch 2 not taken.
 581346 desc.emplace_back("Yaw [deg]");
55
1/2
✓ Branch 1 taken 581346 times.
✗ Branch 2 not taken.
 581346 desc.emplace_back("Roll StdDev [deg]");
56
1/2
✓ Branch 1 taken 581346 times.
✗ Branch 2 not taken.
 581346 desc.emplace_back("Pitch StdDev [deg]");
57
1/2
✓ Branch 1 taken 581346 times.
✗ Branch 2 not taken.
 581346 desc.emplace_back("Yaw StdDev [deg]");
58
1/2
✓ Branch 1 taken 581346 times.
✗ Branch 2 not taken.
 581346 desc.emplace_back("n_Quat_b x");
59
1/2
✓ Branch 1 taken 581346 times.
✗ Branch 2 not taken.
 581346 desc.emplace_back("n_Quat_b y");
60
1/2
✓ Branch 1 taken 581346 times.
✗ Branch 2 not taken.
 581346 desc.emplace_back("n_Quat_b z");
61
1/2
✓ Branch 1 taken 581346 times.
✗ Branch 2 not taken.
 581346 desc.emplace_back("n_Quat_b w");
62
1/2
✓ Branch 1 taken 581346 times.
✗ Branch 2 not taken.
 581346 desc.emplace_back("n_Quat_b x StdDev");
63
1/2
✓ Branch 1 taken 581346 times.
✗ Branch 2 not taken.
 581346 desc.emplace_back("n_Quat_b y StdDev");
64
1/2
✓ Branch 1 taken 581346 times.
✗ Branch 2 not taken.
 581346 desc.emplace_back("n_Quat_b z StdDev");
65
1/2
✓ Branch 1 taken 581346 times.
✗ Branch 2 not taken.
 581346 desc.emplace_back("n_Quat_b w StdDev");
66 581346 return desc;
67 ✗ }
68
69 /// @brief Get the amount of descriptors
70 1743990 [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return PosVel::GetStaticDescriptorCount() + 14; }
71
72 /// @brief Returns a vector of data descriptors
73 ✗ [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); }
74
75 /// @brief Get the amount of descriptors
76 145332 [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); }
77
78 /// @brief Get the value at the index
79 /// @param idx Index corresponding to data descriptor order
80 /// @return Value if in the observation
81 290658 [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
82 {
83
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 290658 times.
 290658 INS_ASSERT(idx < GetStaticDescriptorCount());
84
2/2
✓ Branch 1 taken 145329 times.
✓ Branch 2 taken 145329 times.
 290658 if (idx < PosVel::GetStaticDescriptorCount()) { return PosVel::getValueAt(idx); }
85
3/15
✓ Branch 0 taken 48443 times.
✓ Branch 1 taken 48443 times.
✓ Branch 2 taken 48443 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ 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.
 145329 switch (idx)
86 {
87 48443 case PosVel::GetStaticDescriptorCount() + 0: // Roll [deg]
88
3/6
✓ Branch 1 taken 48443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48443 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 48443 times.
✗ Branch 8 not taken.
 48443 if (_n_Quat_b.norm() != 0) { return rad2deg(rollPitchYaw().x()); }
89 ✗ break;
90 48443 case PosVel::GetStaticDescriptorCount() + 1: // Pitch [deg]
91
3/6
✓ Branch 1 taken 48443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48443 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 48443 times.
✗ Branch 8 not taken.
 48443 if (_n_Quat_b.norm() != 0) { return rad2deg(rollPitchYaw().y()); }
92 ✗ break;
93 48443 case PosVel::GetStaticDescriptorCount() + 2: // Yaw [deg]
94
3/6
✓ Branch 1 taken 48443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48443 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 48443 times.
✗ Branch 8 not taken.
 48443 if (_n_Quat_b.norm() != 0) { return rad2deg(rollPitchYaw().z()); }
95 ✗ break;
96 ✗ case PosVel::GetStaticDescriptorCount() + 3: // Roll StdDev [deg]
97 ✗ if (_n_covarianceMatrix && _n_covarianceMatrix->hasRows(Keys::Att<Keys::MotionModelKey>))
98 {
99 ✗ auto covRPY = trafo::covQuat2RPY((*_n_covarianceMatrix)(Keys::Att<Keys::MotionModelKey>, Keys::Att<Keys::MotionModelKey>), n_Quat_b());
100 ✗ return rad2deg(std::sqrt(covRPY(0, 0)));
101 }
102 ✗ break;
103 ✗ case PosVel::GetStaticDescriptorCount() + 4: // Pitch StdDev [deg]
104 ✗ if (_n_covarianceMatrix && _n_covarianceMatrix->hasRows(Keys::Att<Keys::MotionModelKey>))
105 {
106 ✗ auto covRPY = trafo::covQuat2RPY((*_n_covarianceMatrix)(Keys::Att<Keys::MotionModelKey>, Keys::Att<Keys::MotionModelKey>), n_Quat_b());
107 ✗ return rad2deg(std::sqrt(covRPY(1, 1)));
108 }
109 ✗ break;
110 ✗ case PosVel::GetStaticDescriptorCount() + 5: // Yaw StdDev [deg]
111 ✗ if (_n_covarianceMatrix && _n_covarianceMatrix->hasRows(Keys::Att<Keys::MotionModelKey>))
112 {
113 ✗ auto covRPY = trafo::covQuat2RPY((*_n_covarianceMatrix)(Keys::Att<Keys::MotionModelKey>, Keys::Att<Keys::MotionModelKey>), n_Quat_b());
114 ✗ return rad2deg(std::sqrt(covRPY(2, 2)));
115 }
116 ✗ break;
117 ✗ case PosVel::GetStaticDescriptorCount() + 6: // n_Quat_b x
118 ✗ if (_n_Quat_b.norm() != 0) { return _n_Quat_b.x(); }
119 ✗ break;
120 ✗ case PosVel::GetStaticDescriptorCount() + 7: // n_Quat_b y
121 ✗ if (_n_Quat_b.norm() != 0) { return _n_Quat_b.y(); }
122 ✗ break;
123 ✗ case PosVel::GetStaticDescriptorCount() + 8: // n_Quat_b z
124 ✗ if (_n_Quat_b.norm() != 0) { return _n_Quat_b.z(); }
125 ✗ break;
126 ✗ case PosVel::GetStaticDescriptorCount() + 9: // n_Quat_b w
127 ✗ if (_n_Quat_b.norm() != 0) { return _n_Quat_b.w(); }
128 ✗ break;
129 ✗ case PosVel::GetStaticDescriptorCount() + 10: // n_Quat_b x StdDev
130 ✗ if (_n_covarianceMatrix && _n_covarianceMatrix->hasRow(Keys::AttQ1)) { return std::sqrt((*_n_covarianceMatrix)(Keys::AttQ1, Keys::AttQ1)); }
131 ✗ break;
132 ✗ case PosVel::GetStaticDescriptorCount() + 11: // n_Quat_b y StdDev
133 ✗ if (_n_covarianceMatrix && _n_covarianceMatrix->hasRow(Keys::AttQ2)) { return std::sqrt((*_n_covarianceMatrix)(Keys::AttQ2, Keys::AttQ2)); }
134 ✗ break;
135 ✗ case PosVel::GetStaticDescriptorCount() + 12: // n_Quat_b z StdDev
136 ✗ if (_n_covarianceMatrix && _n_covarianceMatrix->hasRow(Keys::AttQ3)) { return std::sqrt((*_n_covarianceMatrix)(Keys::AttQ3, Keys::AttQ3)); }
137 ✗ break;
138 ✗ case PosVel::GetStaticDescriptorCount() + 13: // n_Quat_b w StdDev
139 ✗ if (_n_covarianceMatrix && _n_covarianceMatrix->hasRow(Keys::AttQ0)) { return std::sqrt((*_n_covarianceMatrix)(Keys::AttQ0, Keys::AttQ0)); }
140 ✗ break;
141 ✗ default:
142 ✗ return std::nullopt;
143 }
144 ✗ return std::nullopt;
145 }
146
147 /// @brief Set the value at the index
148 /// @param idx Index corresponding to data descriptor order
149 /// @param value Value to set
150 /// @return True if the value was updated
151 ✗ [[nodiscard]] bool setValueAt(size_t idx, double value) override
152 {
153 ✗ INS_ASSERT(idx < GetStaticDescriptorCount());
154 ✗ if (idx < PosVel::GetStaticDescriptorCount()) { return PosVel::setValueAt(idx, value); }
155 // switch (idx)
156 // {
157 // case PosVel::GetStaticDescriptorCount() + 0: // Roll [deg]
158 // case PosVel::GetStaticDescriptorCount() + 1: // Pitch [deg]
159 // case PosVel::GetStaticDescriptorCount() + 2: // Yaw [deg]
160 // case PosVel::GetStaticDescriptorCount() + 3: // Quaternion::w
161 // case PosVel::GetStaticDescriptorCount() + 4: // Quaternion::x
162 // case PosVel::GetStaticDescriptorCount() + 5: // Quaternion::y
163 // case PosVel::GetStaticDescriptorCount() + 6: // Quaternion::z
164 // default:
165 // }
166
167 ✗ return false;
168 }
169
170 /* -------------------------------------------------------------------------------------------------------- */
171 /* Rotation Quaternions */
172 /* -------------------------------------------------------------------------------------------------------- */
173
174 /// @brief Returns the Quaternion from body to navigation frame (NED)
175 /// @return The Quaternion for the rotation from body to navigation coordinates
176 482928 [[nodiscard]] const Eigen::Quaterniond& n_Quat_b() const
177 {
178 482928 return _n_Quat_b;
179 }
180
181 /// @brief Returns the Quaternion from navigation to body frame (NED)
182 /// @return The Quaternion for the rotation from navigation to body coordinates
183 2 [[nodiscard]] Eigen::Quaterniond b_Quat_n() const
184 {
185 2 return n_Quat_b().conjugate();
186 }
187
188 /// @brief Returns the Quaternion from body to Earth-fixed frame
189 /// @return The Quaternion for the rotation from body to earth coordinates
190 58025 [[nodiscard]] const Eigen::Quaterniond& e_Quat_b() const
191 {
192 58025 return _e_Quat_b;
193 }
194
195 /// @brief Returns the Quaternion from Earth-fixed to body frame
196 /// @return The Quaternion for the rotation from earth to body coordinates
197 2 [[nodiscard]] Eigen::Quaterniond b_Quat_e() const
198 {
199 2 return e_Quat_b().conjugate();
200 }
201
202 /// @brief Returns the Roll, Pitch and Yaw angles in [rad]
203 /// @return [roll, pitch, yaw]^T
204 353810 [[nodiscard]] Eigen::Vector3d rollPitchYaw() const
205 {
206 353810 return trafo::quat2eulerZYX(n_Quat_b());
207 }
208
209 /// Returns the standard deviation of the Quaternion body to earth frame (x, y, z, w)
210 [[nodiscard]] std::optional<Eigen::Vector4d> e_QuatStdev() const
211 {
212 if (_e_covarianceMatrix && _e_covarianceMatrix->hasRows(Keys::Att<Keys::MotionModelKey>))
213 {
214 return (*_e_covarianceMatrix)(Keys::Att<Keys::MotionModelKey>, Keys::Att<Keys::MotionModelKey>).diagonal().cwiseSqrt();
215 }
216 return std::nullopt;
217 }
218
219 /// Returns the standard deviation of the Quaternion body to navigation frame (roll, pitch, yaw) (x, y, z, w)
220 1 [[nodiscard]] std::optional<Eigen::Vector4d> n_QuatStdev() const
221 {
222
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 if (_n_covarianceMatrix && _e_covarianceMatrix->hasRows(Keys::Att<Keys::MotionModelKey>))
223 {
224
4/8
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 return (*_n_covarianceMatrix)(Keys::Att<Keys::MotionModelKey>, Keys::Att<Keys::MotionModelKey>).diagonal().cwiseSqrt();
225 }
226 ✗ return std::nullopt;
227 }
228
229 // ###########################################################################################################
230 // Setter
231 // ###########################################################################################################
232
233 /// @brief Set the Quaternion from body to earth frame
234 /// @param[in] e_Quat_b Quaternion from body to earth frame
235 template<typename Derived>
236 52866 void setAttitude_e_Quat_b(const Eigen::QuaternionBase<Derived>& e_Quat_b)
237 {
238 52866 _e_Quat_b = e_Quat_b;
239
2/4
✓ Branch 1 taken 52866 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52866 times.
✗ Branch 5 not taken.
 52866 _n_Quat_b = n_Quat_e() * e_Quat_b;
240 52866 }
241
242 /// @brief Set the Quaternion from body to navigation frame
243 /// @param[in] n_Quat_b Quaternion from body to navigation frame
244 template<typename Derived>
245 327225 void setAttitude_n_Quat_b(const Eigen::QuaternionBase<Derived>& n_Quat_b)
246 {
247
2/4
✓ Branch 1 taken 327224 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 327223 times.
✗ Branch 5 not taken.
 327225 _e_Quat_b = e_Quat_n() * n_Quat_b;
248 327220 _n_Quat_b = n_Quat_b;
249 327223 }
250
251 /// @brief Set the position, velocity and attitude
252 /// @param[in] e_position New Position in ECEF coordinates
253 /// @param[in] e_velocity The new velocity in the earth frame
254 /// @param[in] e_Quat_b Quaternion from body to earth frame
255 template<typename DerivedP, typename DerivedV, typename DerivedA>
256 25794 void setPosVelAtt_e(const Eigen::MatrixBase<DerivedP>& e_position, const Eigen::MatrixBase<DerivedV>& e_velocity, const Eigen::QuaternionBase<DerivedA>& e_Quat_b)
257 {
258 25794 setPosition_e(e_position);
259 25794 setVelocity_e(e_velocity);
260 25794 setAttitude_e_Quat_b(e_Quat_b);
261 25794 }
262
263 /// @brief Set the position, velocity and attitude
264 /// @param[in] lla_position New Position in LatLonAlt coordinates [rad, rad, m]
265 /// @param[in] n_velocity The new velocity in the NED frame [m/s, m/s, m/s]
266 /// @param[in] n_Quat_b Quaternion from body to navigation frame
267 template<typename DerivedP, typename DerivedV, typename DerivedA>
268 98442 void setPosVelAtt_n(const Eigen::MatrixBase<DerivedP>& lla_position, const Eigen::MatrixBase<DerivedV>& n_velocity, const Eigen::QuaternionBase<DerivedA>& n_Quat_b)
269 {
270 98442 setPosition_lla(lla_position);
271 98442 setVelocity_n(n_velocity);
272 98442 setAttitude_n_Quat_b(n_Quat_b);
273 98442 }
274
275 /// @brief Set the position, velocity, attitude 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_Quat_b Quaternion from body to earth frame
279 /// @param[in] e_covarianceMatrix 10x10 PosVelAtt Error variance
280 template<typename DerivedP, typename DerivedV, typename DerivedA, typename Derived>
281 79920 void setPosVelAttAndCov_e(const Eigen::MatrixBase<DerivedP>& e_position, const Eigen::MatrixBase<DerivedV>& e_velocity,
282 const Eigen::QuaternionBase<DerivedA>& e_Quat_b, const Eigen::MatrixBase<Derived>& e_covarianceMatrix)
283 {
284 79920 setPosition_e(e_position);
285 79920 setVelocity_e(e_velocity);
286 79920 setAttitude_e_Quat_b(e_Quat_b);
287 79920 setPosVelAttCovarianceMatrix_e(e_covarianceMatrix);
288 79920 }
289
290 /// @brief Set the position, velocity, attitude and the covariance matrix
291 /// @param[in] lla_position New Position in LatLonAlt coordinates [rad, rad, m]
292 /// @param[in] n_velocity The new velocity in the NED frame [m/s, m/s, m/s]
293 /// @param[in] n_Quat_b Quaternion from body to navigation frame
294 /// @param[in] n_covarianceMatrix 10x10 PosVelAtt Error variance
295 template<typename DerivedP, typename DerivedV, typename DerivedA, typename Derived>
296 149475 void setPosVelAttAndCov_n(const Eigen::MatrixBase<DerivedP>& lla_position, const Eigen::MatrixBase<DerivedV>& n_velocity,
297 const Eigen::QuaternionBase<DerivedA>& n_Quat_b, const Eigen::MatrixBase<Derived>& n_covarianceMatrix)
298 {
299 149475 setPosition_lla(lla_position);
300 149475 setVelocity_n(n_velocity);
301 149475 setAttitude_n_Quat_b(n_Quat_b);
302 149475 setPosVelAttCovarianceMatrix_n(n_covarianceMatrix);
303 149475 }
304
305 /// @brief Set the Covariance matrix in ECEF coordinates
306 /// @param[in] e_covarianceMatrix 10x10 PosVelAtt Error variance
307 /// @attention Position has to be set before calling this
308 template<typename Derived>
309 79920 void setPosVelAttCovarianceMatrix_e(const Eigen::MatrixBase<Derived>& e_covarianceMatrix)
310 {
311
1/2
✓ Branch 1 taken 39960 times.
✗ Branch 2 not taken.
 79920 INS_ASSERT_USER_ERROR(e_covarianceMatrix.rows() == 10, "This function needs a 10x10 matrix as input");
312
1/2
✓ Branch 1 taken 39960 times.
✗ Branch 2 not taken.
 79920 INS_ASSERT_USER_ERROR(e_covarianceMatrix.cols() == 10, "This function needs a 10x10 matrix as input");
313
314
1/2
✓ Branch 2 taken 39960 times.
✗ Branch 3 not taken.
 79920 _e_covarianceMatrix = KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>(
315 e_covarianceMatrix, Keys::PosVelAtt<Keys::MotionModelKey>);
316
317
1/2
✓ Branch 1 taken 39960 times.
✗ Branch 2 not taken.
 79920 Eigen::Quaterniond n_q_e = n_Quat_e();
318
2/4
✓ Branch 1 taken 39960 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39960 times.
✗ Branch 5 not taken.
 79920 Eigen::Matrix<double, 10, 10> J = Eigen::Matrix<double, 10, 10>::Zero();
319
3/6
✓ Branch 1 taken 39960 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39960 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39960 times.
✗ Branch 8 not taken.
 79920 J.block<3, 3>(0, 0) = n_q_e.toRotationMatrix();
320
3/6
✓ Branch 1 taken 39960 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39960 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39960 times.
✗ Branch 8 not taken.
 79920 J.block<3, 3>(3, 3) = n_q_e.toRotationMatrix();
321
3/6
✓ Branch 1 taken 39960 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39960 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39960 times.
✗ Branch 8 not taken.
 79920 J.block<4, 4>(6, 6) = trafo::covQuat2QuatJacobian(n_q_e);
322
323
3/6
✓ Branch 2 taken 39960 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14184 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 14184 times.
✗ Branch 9 not taken.
 159840 _n_covarianceMatrix = KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>(
324
3/6
✓ Branch 1 taken 39960 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25776 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25776 times.
✗ Branch 8 not taken.
 79920 J * e_covarianceMatrix * J.transpose(), Keys::PosVelAtt<Keys::MotionModelKey>);
325 79920 }
326
327 /// @brief Set the Covariance matrix in NED coordinates
328 /// @param[in] n_covarianceMatrix 10x10 PosVelAtt Error variance
329 /// @attention Position has to be set before calling this
330 template<typename Derived>
331 149475 void setPosVelAttCovarianceMatrix_n(const Eigen::MatrixBase<Derived>& n_covarianceMatrix)
332 {
333
1/2
✓ Branch 1 taken 86849 times.
✗ Branch 2 not taken.
 149475 INS_ASSERT_USER_ERROR(n_covarianceMatrix.rows() == 10, "This function needs a 10x10 matrix as input");
334
1/2
✓ Branch 1 taken 86849 times.
✗ Branch 2 not taken.
 149475 INS_ASSERT_USER_ERROR(n_covarianceMatrix.cols() == 10, "This function needs a 10x10 matrix as input");
335
336
1/2
✓ Branch 2 taken 86849 times.
✗ Branch 3 not taken.
 149475 _n_covarianceMatrix = KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>(
337 n_covarianceMatrix, Keys::PosVelAtt<Keys::MotionModelKey>);
338
339
1/2
✓ Branch 1 taken 86849 times.
✗ Branch 2 not taken.
 149475 Eigen::Quaterniond e_q_n = e_Quat_n();
340
2/4
✓ Branch 1 taken 86849 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 86849 times.
✗ Branch 5 not taken.
 149475 Eigen::Matrix<double, 10, 10> J = Eigen::Matrix<double, 10, 10>::Zero();
341
3/6
✓ Branch 1 taken 86849 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 86849 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 86849 times.
✗ Branch 8 not taken.
 149475 J.block<3, 3>(0, 0) = e_q_n.toRotationMatrix();
342
3/6
✓ Branch 1 taken 86849 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 86849 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 86849 times.
✗ Branch 8 not taken.
 149475 J.block<3, 3>(3, 3) = e_q_n.toRotationMatrix();
343
3/6
✓ Branch 1 taken 86849 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 86849 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 86849 times.
✗ Branch 8 not taken.
 149475 J.block<4, 4>(6, 6) = trafo::covQuat2QuatJacobian(e_q_n);
344
345
3/6
✓ Branch 2 taken 86849 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 62626 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 62626 times.
✗ Branch 9 not taken.
 298950 _e_covarianceMatrix = KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>(
346
3/6
✓ Branch 1 taken 86849 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24223 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24223 times.
✗ Branch 8 not taken.
 149475 J * n_covarianceMatrix * J.transpose(), Keys::PosVelAtt<Keys::MotionModelKey>);
347 149475 }
348
349 /* -------------------------------------------------------------------------------------------------------- */
350 /* Member variables */
351 /* -------------------------------------------------------------------------------------------------------- */
352
353 private:
354 /// Quaternion body to earth frame
355 Eigen::Quaterniond _e_Quat_b{ 0, 0, 0, 0 };
356 /// Quaternion body to navigation frame (roll, pitch, yaw)
357 Eigen::Quaterniond _n_Quat_b{ 0, 0, 0, 0 };
358 };
359
360 } // namespace NAV
361