INSTINCT Code Coverage Report

Directory: src/
File: Navigation/INS/InertialIntegrator.cpp
Date: 2025-06-02 15:19:59
Exec Total Coverage
Lines: 133 260 51.2%
Functions: 20 29 69.0%
Branches: 145 432 33.6%
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 InertialIntegrator.cpp
10 /// @brief Inertial Measurement Integrator
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2023-12-09
13
14 #include "InertialIntegrator.hpp"
15
16 #include "internal/gui/NodeEditorApplication.hpp"
17 #include "internal/gui/widgets/HelpMarker.hpp"
18
19 #include "Navigation/Ellipsoid/Ellipsoid.hpp"
20 #include "Navigation/INS/Functions.hpp"
21 #include "Navigation/Math/Math.hpp"
22
23 namespace NAV
24 {
25
26 ✗ InertialIntegrator::InertialIntegrator(IntegrationFrame integrationFrame)
27 ✗ : _integrationFrame(integrationFrame), _lockIntegrationFrame(true) {}
28
29 51 void InertialIntegrator::reset()
30 {
31 51 _states.clear();
32 51 _p_lastBiasAcceleration.setZero();
33 51 _p_lastBiasAngularRate.setZero();
34 51 setBufferSizes();
35 51 }
36
37 126826 bool InertialIntegrator::hasInitialPosition() const
38 {
39 126826 return !_states.empty();
40 }
41
42 17 void InertialIntegrator::setInitialState(const PosVelAtt& state, const char* nameId)
43 {
44 17 _states.clear();
45 17 addState(state, nameId);
46 17 }
47
48 17 void InertialIntegrator::addState(const PosVelAtt& state, [[maybe_unused]] const char* nameId)
49 {
50
16/32
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 17 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 17 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 17 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 17 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 17 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 17 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 17 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 17 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 17 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 17 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 17 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 17 times.
✗ Branch 47 not taken.
 85 _states.push_back(State{
51 .epoch = state.insTime,
52
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 9 times.
 17 .position = _integrationFrame == IntegrationFrame::ECEF ? state.e_position() : state.lla_position(),
53
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 9 times.
 17 .velocity = _integrationFrame == IntegrationFrame::ECEF ? state.e_velocity() : state.n_velocity(),
54
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 9 times.
 17 .attitude = _integrationFrame == IntegrationFrame::ECEF ? state.e_Quat_b() : state.n_Quat_b(),
55 .m = Measurement{
56 .averagedMeasurement = false,
57 .dt = 0.0,
58
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 .p_acceleration = Eigen::Vector3d::Zero(),
59
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 .p_angularRate = Eigen::Vector3d::Zero(),
60 } });
61 LOG_DATA("{}: Adding state for [{}]. Now has {} states", nameId, state.insTime.toYMDHMS(GPST), _states.size());
62 17 }
63
64 17 void InertialIntegrator::setTotalSensorBiases(const Eigen::Vector3d& p_biasAcceleration, const Eigen::Vector3d& p_biasAngularRate)
65 {
66 17 _p_lastBiasAcceleration = p_biasAcceleration;
67 17 _p_lastBiasAngularRate = p_biasAngularRate;
68 17 }
69
70 26813 void InertialIntegrator::applySensorBiasesIncrements(const Eigen::Vector3d& p_deltaBiasAcceleration, const Eigen::Vector3d& p_deltaBiasAngularRate)
71 {
72 26813 _p_lastBiasAcceleration += p_deltaBiasAcceleration;
73 26813 _p_lastBiasAngularRate += p_deltaBiasAngularRate;
74 26813 }
75
76 25517 void InertialIntegrator::applyStateErrors_n(const Eigen::Vector3d& lla_positionError, const Eigen::Vector3d& n_velocityError, const Eigen::Vector3d& n_attitudeError_b)
77 {
78
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 25517 times.
 25517 INS_ASSERT_USER_ERROR(_integrationFrame == IntegrationFrame::NED, "You can only apply errors to the selected integration frame");
79
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 25517 times.
✗ Branch 4 not taken.
 25517 INS_ASSERT_USER_ERROR(!_states.empty(), "You can only apply errors if the states vector is not empty");
80
81
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 _states.back().position -= lla_positionError;
82
2/4
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
 25517 _states.back().velocity -= n_velocityError;
83 // Attitude correction, see Titterton and Weston (2004), p. 407 eq. 13.15
84
7/14
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25517 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 25517 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 25517 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 25517 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 25517 times.
✗ Branch 20 not taken.
 25517 Eigen::Matrix3d n_Dcm_b = (Eigen::Matrix3d::Identity() - math::skewSymmetricMatrix(n_attitudeError_b)) * _states.back().attitude.toRotationMatrix();
85
3/6
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25517 times.
✗ Branch 8 not taken.
 25517 _states.back().attitude = Eigen::Quaterniond(n_Dcm_b).normalized();
86
87 // TODO: Test this out again
88 // Attitude correction, see Titterton and Weston (2004), p. 407 eq. 13.16
89 // Eigen::Quaterniond n_Quat_b = posVelAtt->n_Quat_b()
90 // * (Eigen::AngleAxisd(attError(0), Eigen::Vector3d::UnitX())
91 // * Eigen::AngleAxisd(attError(1), Eigen::Vector3d::UnitY())
92 // * Eigen::AngleAxisd(attError(2), Eigen::Vector3d::UnitZ()))
93 // .normalized();
94 // posVelAttCorrected->setAttitude_n_Quat_b(n_Quat_b.normalized());
95
96 // Eigen::Vector3d attError = pvaError->n_attitudeError();
97 // const Eigen::Quaterniond& n_Quat_b = posVelAtt->n_Quat_b();
98 // Eigen::Quaterniond n_Quat_b_c{ n_Quat_b.w() + 0.5 * (+attError(0) * n_Quat_b.x() + attError(1) * n_Quat_b.y() + attError(2) * n_Quat_b.z()),
99 // n_Quat_b.x() + 0.5 * (-attError(0) * n_Quat_b.w() + attError(1) * n_Quat_b.z() - attError(2) * n_Quat_b.y()),
100 // n_Quat_b.y() + 0.5 * (-attError(0) * n_Quat_b.z() - attError(1) * n_Quat_b.w() + attError(2) * n_Quat_b.x()),
101 // n_Quat_b.z() + 0.5 * (+attError(0) * n_Quat_b.y() - attError(1) * n_Quat_b.x() - attError(2) * n_Quat_b.w()) };
102 // posVelAttCorrected->setAttitude_n_Quat_b(n_Quat_b_c.normalized());
103 25517 }
104
105 1296 void InertialIntegrator::applyStateErrors_e(const Eigen::Vector3d& e_positionError, const Eigen::Vector3d& e_velocityError, const Eigen::Vector3d& e_attitudeError_b)
106 {
107
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1296 times.
 1296 INS_ASSERT_USER_ERROR(_integrationFrame == IntegrationFrame::ECEF, "You can only apply errors to the selected integration frame");
108
2/4
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1296 times.
✗ Branch 4 not taken.
 1296 INS_ASSERT_USER_ERROR(!_states.empty(), "You can only apply errors if the states vector is not empty");
109
110
2/4
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
 1296 _states.back().position -= e_positionError;
111
2/4
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
 1296 _states.back().velocity -= e_velocityError;
112 // Attitude correction, see Titterton and Weston (2004), p. 407 eq. 13.15
113
7/14
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1296 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1296 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1296 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1296 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1296 times.
✗ Branch 20 not taken.
 1296 Eigen::Matrix3d e_Dcm_b = (Eigen::Matrix3d::Identity() - math::skewSymmetricMatrix(e_attitudeError_b)) * _states.back().attitude.toRotationMatrix();
114
3/6
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1296 times.
✗ Branch 8 not taken.
 1296 _states.back().attitude = Eigen::Quaterniond(e_Dcm_b).normalized();
115 1296 }
116
117 543633 std::optional<std::reference_wrapper<const InertialIntegrator::State>> InertialIntegrator::getLatestState() const
118 {
119
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 543633 times.
 543633 if (_states.empty()) { return {}; }
120
1/2
✓ Branch 1 taken 543633 times.
✗ Branch 2 not taken.
 543633 return _states.back();
121 }
122
123 52589 const Eigen::Vector3d& InertialIntegrator::p_getLastAccelerationBias() const
124 {
125 52589 return _p_lastBiasAcceleration;
126 }
127
128 52589 const Eigen::Vector3d& InertialIntegrator::p_getLastAngularRateBias() const
129 {
130 52589 return _p_lastBiasAngularRate;
131 }
132
133 270250 InertialIntegrator::IntegrationFrame InertialIntegrator::getIntegrationFrame() const
134 {
135 270250 return _integrationFrame;
136 }
137
138 ✗ const PosVelAttDerivativeConstants& InertialIntegrator::getConstants() const
139 {
140 ✗ return _posVelAttDerivativeConstants;
141 }
142
143 ✗ bool InertialIntegrator::areAccelerationsAveragedMeasurements() const
144 {
145 ✗ return _accelerationsAreAveragedMeasurements;
146 }
147
148 49997 std::optional<Eigen::Vector3d> InertialIntegrator::p_calcCurrentAcceleration() const
149 {
150
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 49997 times.
 49997 if (_states.empty()) { return {}; }
151
152
4/8
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 49997 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 49997 times.
✗ Branch 11 not taken.
 49997 return _states.back().m.p_acceleration + _states.back().p_biasAcceleration;
153 }
154
155 26813 std::optional<Eigen::Vector3d> InertialIntegrator::p_calcCurrentAngularRate() const
156 {
157
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 26813 times.
 26813 if (_states.empty()) { return {}; }
158
159
4/8
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26813 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26813 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26813 times.
✗ Branch 11 not taken.
 26813 return _states.back().m.p_angularRate + _states.back().p_biasAngularRate;
160 }
161
162 49998 void InertialIntegrator::addMeasurement(const InsTime& epoch, double dt, const Eigen::Vector3d& p_acceleration, const Eigen::Vector3d& p_angularRate, [[maybe_unused]] const char* nameId)
163 {
164 LOG_DATA("{}: Adding measurement [{}]. Last state at [{}]", nameId, epoch.toYMDHMS(GPST), _states.back().epoch.toYMDHMS(GPST));
165
1/2
✓ Branch 1 taken 49998 times.
✗ Branch 2 not taken.
 49998 INS_ASSERT_USER_ERROR(!_states.empty(), "You need to add an initial state first");
166
167
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 49997 times.
 49998 if (_states.back().epoch == epoch)
168 {
169 LOG_DATA("{}: Updating existing state", nameId);
170 1 _states.back().m.averagedMeasurement = _accelerationsAreAveragedMeasurements;
171 1 _states.back().m.dt = dt;
172 1 _states.back().m.p_acceleration = p_acceleration;
173 1 _states.back().m.p_angularRate = p_angularRate;
174 1 _states.back().p_biasAcceleration = _p_lastBiasAcceleration;
175 1 _states.back().p_biasAngularRate = _p_lastBiasAngularRate;
176 1 return;
177 }
178
179 LOG_DATA("{}: Adding as new state", nameId);
180
8/16
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 49997 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 49997 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 49997 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 49997 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 49997 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 49997 times.
✗ Branch 23 not taken.
 99994 _states.push_back(State{
181 .epoch = epoch,
182
2/4
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
 49997 .position = _states.back().position,
183
2/4
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
 49997 .velocity = _states.back().velocity,
184
2/4
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
 49997 .attitude = _states.back().attitude,
185 .m = Measurement{
186
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 .averagedMeasurement = _accelerationsAreAveragedMeasurements,
187 .dt = dt,
188 .p_acceleration = p_acceleration,
189 .p_angularRate = p_angularRate,
190 },
191
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 .p_biasAcceleration = _p_lastBiasAcceleration,
192
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 .p_biasAngularRate = _p_lastBiasAngularRate,
193 });
194 }
195
196 ✗ void InertialIntegrator::addDeltaMeasurement(const InsTime& epoch, double dt, double deltaTime, const Eigen::Vector3d& p_deltaVelocity, const Eigen::Vector3d& p_deltaTheta, const char* nameId)
197 {
198 LOG_DATA("{}: Adding delta measurement for [{}]", nameId, epoch.toYMDHMS(GPST));
199 ✗ Eigen::Vector3d p_acceleration = Eigen::Vector3d::Zero();
200 ✗ Eigen::Vector3d p_angularRate = Eigen::Vector3d::Zero();
201
202 ✗ if (deltaTime > 0.0) // dt given by sensor (should never be 0 or negative, but check here just in case)
203 {
204 ✗ p_acceleration = p_deltaVelocity / deltaTime;
205 ✗ p_angularRate = p_deltaTheta / deltaTime;
206 }
207 ✗ else if (std::abs(dt) > 0.0) // Time difference between messages (differs from dt if message lost)
208 {
209 // Negative values of dTimeLastState should not happen, but algorithm can work with it to propagate backwards
210 ✗ p_acceleration = p_deltaVelocity / dt;
211 ✗ p_angularRate = p_deltaTheta / dt;
212 }
213
214 ✗ addMeasurement(epoch, dt, p_acceleration, p_angularRate, nameId);
215 ✗ _states.back().m.averagedMeasurement = true;
216 ✗ }
217
218 1 std::shared_ptr<PosVelAtt> InertialIntegrator::lastStateAsPosVelAtt() const
219 {
220
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 INS_ASSERT_USER_ERROR(!_states.empty(), "You need to add an initial state first");
221
222 1 auto posVelAtt = std::make_shared<PosVelAtt>();
223
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 posVelAtt->insTime = _states.back().epoch;
224
1/3
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 1 switch (_integrationFrame)
225 {
226 1 case IntegrationFrame::NED:
227
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 posVelAtt->setPosVelAtt_n(_states.back().position, _states.back().velocity, _states.back().attitude);
228 1 break;
229 ✗ case IntegrationFrame::ECEF:
230 ✗ posVelAtt->setPosVelAtt_e(_states.back().position, _states.back().velocity, _states.back().attitude);
231 ✗ break;
232 }
233
234 1 return posVelAtt;
235 ✗ }
236
237 49998 std::shared_ptr<PosVelAtt> InertialIntegrator::calcInertialSolution(const InsTime& obsTime,
238 const Eigen::Vector3d& p_acceleration, const Eigen::Vector3d& p_angularRate,
239 const ImuPos& imuPos, const char* nameId)
240 {
241
3/6
✓ Branch 1 taken 49998 times.
✗ Branch 2 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 49998 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 49998 times.
 49998 if (!hasInitialPosition() || obsTime < _states.back().epoch) { return nullptr; }
242
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 49998 times.
 49998 if (_states.back().epoch.empty()) { _states.back().epoch = obsTime; }
243
244
2/4
✓ Branch 1 taken 49998 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49998 times.
✗ Branch 5 not taken.
 49998 auto dt = static_cast<double>((obsTime - _states.back().epoch).count());
245 49998 addMeasurement(obsTime, dt, p_acceleration, p_angularRate, nameId);
246
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 49997 times.
 49998 if (std::abs(dt) < 1e-8)
247 {
248 LOG_DATA("{}: Returning state [{}], as no time difference between measurement.", nameId, _states.back().epoch.toYMDHMS(GPST));
249 1 return lastStateAsPosVelAtt();
250 }
251
252 49997 return calcInertialSolutionFromMeasurementBuffer(imuPos, nameId);
253 }
254
255 ✗ std::shared_ptr<PosVelAtt> InertialIntegrator::calcInertialSolutionDelta(const InsTime& obsTime, double deltaTime,
256 const Eigen::Vector3d& p_deltaVelocity, const Eigen::Vector3d& p_deltaTheta,
257 const ImuPos& imuPos, const char* nameId)
258 {
259 ✗ if (!hasInitialPosition() || obsTime < _states.back().epoch) { return nullptr; }
260 ✗ if (_states.back().epoch.empty()) { _states.back().epoch = obsTime; }
261
262 ✗ auto dt = static_cast<double>((obsTime - _states.back().epoch).count());
263 ✗ addDeltaMeasurement(obsTime, dt, deltaTime, p_deltaVelocity, p_deltaTheta, nameId);
264 ✗ if (std::abs(dt) < 1e-8)
265 {
266 LOG_DATA("{}: Returning state [{}], as no time difference between measurement.", nameId, _states.back().epoch.toYMDHMS(GPST));
267 ✗ return lastStateAsPosVelAtt();
268 }
269
270 ✗ return calcInertialSolutionFromMeasurementBuffer(imuPos, nameId);
271 }
272
273 49997 std::shared_ptr<PosVelAtt> InertialIntegrator::calcInertialSolutionFromMeasurementBuffer(const ImuPos& imuPos, const char* nameId)
274 {
275
2/4
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 49997 times.
✗ Branch 4 not taken.
 49997 INS_ASSERT_USER_ERROR(!_states.empty(), "You need to add an initial state first");
276
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 INS_ASSERT_USER_ERROR(_states.size() >= 2, "You need to add an initial state and at least one measurement first");
277
278 LOG_DATA("{}: Calculating inertial solution from buffer. States t0 = [{}], t1 = [{}]",
279 nameId, _states.back().epoch.toYMDHMS(GPST), _states.at(_states.size() - 2).epoch.toYMDHMS(GPST));
280
281
3/6
✓ Branch 2 taken 49997 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 49997 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 49997 times.
✗ Branch 9 not taken.
 49997 auto y = calcInertialSolution(imuPos, _states.back(), _states.at(_states.size() - 2), nameId);
282
283
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 auto posVelAtt = std::make_shared<PosVelAtt>();
284
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 49997 posVelAtt->insTime = _states.back().epoch;
285
2/3
✓ Branch 0 taken 37109 times.
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
 49997 switch (_integrationFrame)
286 {
287 37109 case IntegrationFrame::NED:
288
5/10
✓ Branch 2 taken 37109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37109 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 37109 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 37109 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 37109 times.
✗ Branch 15 not taken.
 37109 posVelAtt->setPosVelAtt_n(y.segment<3>(0), y.segment<3>(3), Eigen::Quaterniond(y.segment<4>(6)));
289 37109 break;
290 12888 case IntegrationFrame::ECEF:
291
5/10
✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12888 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 12888 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 12888 times.
✗ Branch 15 not taken.
 12888 posVelAtt->setPosVelAtt_e(y.segment<3>(0), y.segment<3>(3), Eigen::Quaterniond(y.segment<4>(6)));
292 12888 break;
293 }
294
295
2/3
✓ Branch 0 taken 37109 times.
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
 49997 switch (_integrationFrame)
296 {
297 37109 case IntegrationFrame::NED:
298
2/4
✓ Branch 3 taken 37109 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 37109 times.
✗ Branch 7 not taken.
 37109 _states.back().position = posVelAtt->lla_position();
299
2/4
✓ Branch 3 taken 37109 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 37109 times.
✗ Branch 7 not taken.
 37109 _states.back().velocity = posVelAtt->n_velocity();
300
2/4
✓ Branch 3 taken 37109 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 37109 times.
✗ Branch 7 not taken.
 37109 _states.back().attitude = posVelAtt->n_Quat_b();
301 37109 break;
302 12888 case IntegrationFrame::ECEF:
303
2/4
✓ Branch 3 taken 12888 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12888 times.
✗ Branch 7 not taken.
 12888 _states.back().position = posVelAtt->e_position();
304
2/4
✓ Branch 3 taken 12888 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12888 times.
✗ Branch 7 not taken.
 12888 _states.back().velocity = posVelAtt->e_velocity();
305
2/4
✓ Branch 3 taken 12888 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12888 times.
✗ Branch 7 not taken.
 12888 _states.back().attitude = posVelAtt->e_Quat_b();
306 12888 break;
307 }
308
309 99994 return posVelAtt;
310 ✗ }
311
312 69 void InertialIntegrator::setBufferSizes()
313 {
314
1/4
✓ Branch 0 taken 69 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 69 switch (_integrationAlgorithm)
315 {
316 69 case IntegrationAlgorithm::SingleStepRungeKutta1:
317 case IntegrationAlgorithm::SingleStepRungeKutta2:
318 case IntegrationAlgorithm::SingleStepHeun2:
319 case IntegrationAlgorithm::SingleStepRungeKutta3:
320 case IntegrationAlgorithm::SingleStepHeun3:
321 case IntegrationAlgorithm::SingleStepRungeKutta4:
322 69 _states.resize(2);
323 69 break;
324 ✗ case IntegrationAlgorithm::MultiStepRK3:
325 case IntegrationAlgorithm::MultiStepRK4:
326 ✗ _states.resize(3);
327 ✗ break;
328 ✗ case IntegrationAlgorithm::COUNT:
329 ✗ break;
330 }
331 69 }
332
333 ✗ bool InertialIntegratorGui(const char* label, InertialIntegrator& integrator, bool& preferAccelerationOverDeltaMeasurements, float width)
334 {
335 ✗ bool changed = false;
336
337 ✗ if (integrator._lockIntegrationFrame) { ImGui::BeginDisabled(); }
338 ✗ ImGui::SetNextItemWidth(width * gui::NodeEditorApplication::windowFontRatio());
339 ✗ if (auto integrationFrame = static_cast<int>(integrator._integrationFrame);
340 ✗ ImGui::Combo(fmt::format("Integration Frame##{}", label).c_str(), &integrationFrame, "ECEF\0NED\0\0"))
341 {
342 ✗ integrator._integrationFrame = static_cast<decltype(integrator._integrationFrame)>(integrationFrame);
343 ✗ LOG_DEBUG("Integration Frame changed to {}", integrator._integrationFrame == InertialIntegrator::IntegrationFrame::NED ? "NED" : "ECEF");
344 ✗ changed = true;
345 }
346 ✗ if (integrator._lockIntegrationFrame) { ImGui::EndDisabled(); }
347
348 ✗ ImGui::SetNextItemWidth(width * gui::NodeEditorApplication::windowFontRatio());
349 ✗ if (ImGui::BeginCombo(fmt::format("Integration Algorithm##{}", label).c_str(), to_string(integrator._integrationAlgorithm)))
350 {
351 ✗ for (size_t i = 0; i < static_cast<size_t>(InertialIntegrator::IntegrationAlgorithm::MultiStepRK3); i++) // TODO: InertialIntegrator::IntegrationAlgorithm::COUNT
352 {
353 ✗ const bool is_selected = (static_cast<size_t>(integrator._integrationAlgorithm) == i);
354 ✗ if (ImGui::Selectable(to_string(static_cast<InertialIntegrator::IntegrationAlgorithm>(i)), is_selected))
355 {
356 ✗ integrator._integrationAlgorithm = static_cast<InertialIntegrator::IntegrationAlgorithm>(i);
357 ✗ integrator.setBufferSizes();
358 ✗ LOG_DEBUG("Integration Algorithm Attitude changed to {}", fmt::underlying(integrator._integrationAlgorithm));
359 ✗ changed = true;
360 }
361
362 // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
363 ✗ if (is_selected)
364 {
365 ✗ ImGui::SetItemDefaultFocus();
366 }
367 }
368
369 ✗ ImGui::EndCombo();
370 }
371
372 ✗ changed |= ImGui::Checkbox(fmt::format("Prefer raw measurements over delta##{}", label).c_str(), &preferAccelerationOverDeltaMeasurements);
373
374 ✗ if (preferAccelerationOverDeltaMeasurements)
375 {
376 ✗ changed |= ImGui::Checkbox(fmt::format("Accumulated Acceleration##{}", label).c_str(), &integrator._accelerationsAreAveragedMeasurements);
377 ✗ ImGui::SameLine();
378 ✗ gui::widgets::HelpMarker("Some IMUs operate at higher rates than their output rate. (e.g. internal rate 800Hz and output rate is 100Hz)\n"
379 "- Such IMUs often output averaged accelerations between the current and last output.\n"
380 "- If the connected IMU (e.g. a VectorNavSensor node) is such as sensor, please check this.\n"
381 "- If delta measurements are used, this is automatically true");
382 }
383
384 ✗ ImGui::SetNextItemOpen(false, ImGuiCond_FirstUseEver);
385 ✗ if (ImGui::TreeNode(fmt::format("Compensation models##{}", label).c_str()))
386 {
387 ✗ ImGui::TextUnformatted("Acceleration compensation");
388 {
389 ✗ ImGui::Indent();
390 ✗ ImGui::SetNextItemWidth(230 * gui::NodeEditorApplication::windowFontRatio());
391 ✗ if (ComboGravitationModel(fmt::format("Gravitation Model##{}", label).c_str(), integrator._posVelAttDerivativeConstants.gravitationModel))
392 {
393 ✗ LOG_DEBUG("Gravity Model changed to {}", NAV::to_string(integrator._posVelAttDerivativeConstants.gravitationModel));
394 ✗ changed = true;
395 }
396 ✗ if (ImGui::Checkbox(fmt::format("Coriolis acceleration ##{}", label).c_str(), &integrator._posVelAttDerivativeConstants.coriolisAccelerationCompensationEnabled))
397 {
398 ✗ LOG_DEBUG("coriolisAccelerationCompensationEnabled changed to {}", integrator._posVelAttDerivativeConstants.coriolisAccelerationCompensationEnabled);
399 ✗ changed = true;
400 }
401 ✗ if (ImGui::Checkbox(fmt::format("Centrifugal acceleration##{}", label).c_str(), &integrator._posVelAttDerivativeConstants.centrifgalAccelerationCompensationEnabled))
402 {
403 ✗ LOG_DEBUG("centrifgalAccelerationCompensationEnabled changed to {}", integrator._posVelAttDerivativeConstants.centrifgalAccelerationCompensationEnabled);
404 ✗ changed = true;
405 }
406 ✗ ImGui::Unindent();
407 }
408 ✗ ImGui::TextUnformatted("Angular rate compensation");
409 {
410 ✗ ImGui::Indent();
411 ✗ if (ImGui::Checkbox(fmt::format("Earth rotation rate##{}", label).c_str(), &integrator._posVelAttDerivativeConstants.angularRateEarthRotationCompensationEnabled))
412 {
413 ✗ LOG_DEBUG("angularRateEarthRotationCompensationEnabled changed to {}", integrator._posVelAttDerivativeConstants.angularRateEarthRotationCompensationEnabled);
414 ✗ changed = true;
415 }
416 ✗ if (integrator._integrationFrame == InertialIntegrator::IntegrationFrame::NED)
417 {
418 ✗ if (ImGui::Checkbox(fmt::format("Transport rate##{}", label).c_str(), &integrator._posVelAttDerivativeConstants.angularRateTransportRateCompensationEnabled))
419 {
420 ✗ LOG_DEBUG("angularRateTransportRateCompensationEnabled changed to {}", integrator._posVelAttDerivativeConstants.angularRateTransportRateCompensationEnabled);
421 ✗ changed = true;
422 }
423 }
424 ✗ ImGui::Unindent();
425 }
426
427 ✗ ImGui::TreePop();
428 }
429
430 ✗ return changed;
431 }
432
433 ✗ void to_json(json& j, const InertialIntegrator& data)
434 {
435 ✗ j = json{
436 ✗ { "integrationFrame", data._integrationFrame },
437 ✗ { "lockIntegrationFrame", data._lockIntegrationFrame },
438 ✗ { "integrationAlgorithm", data._integrationAlgorithm },
439 ✗ { "accelerationsAreAveragedMeasurements", data._accelerationsAreAveragedMeasurements },
440 ✗ { "posVelAttDerivativeConstants", data._posVelAttDerivativeConstants },
441 ✗ };
442 ✗ }
443
444 18 void from_json(const json& j, InertialIntegrator& data)
445 {
446
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 if (j.contains("integrationFrame")) { j.at("integrationFrame").get_to(data._integrationFrame); }
447
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 if (j.contains("lockIntegrationFrame")) { j.at("lockIntegrationFrame").get_to(data._lockIntegrationFrame); }
448
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 if (j.contains("integrationAlgorithm"))
449 {
450 18 j.at("integrationAlgorithm").get_to(data._integrationAlgorithm);
451 18 data.setBufferSizes();
452 }
453
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 if (j.contains("accelerationsAreAveragedMeasurements")) { j.at("accelerationsAreAveragedMeasurements").get_to(data._accelerationsAreAveragedMeasurements); }
454
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 if (j.contains("posVelAttDerivativeConstants")) { j.at("posVelAttDerivativeConstants").get_to(data._posVelAttDerivativeConstants); }
455 18 }
456
457 ✗ const char* to_string(InertialIntegrator::IntegrationAlgorithm algorithm)
458 {
459 ✗ switch (algorithm)
460 {
461 ✗ case InertialIntegrator::IntegrationAlgorithm::SingleStepRungeKutta1:
462 ✗ return "Runge-Kutta 1st order (explicit) / (Forward) Euler method";
463 ✗ case InertialIntegrator::IntegrationAlgorithm::SingleStepRungeKutta2:
464 ✗ return "Runge-Kutta 2nd order (explicit)";
465 ✗ case InertialIntegrator::IntegrationAlgorithm::SingleStepHeun2:
466 ✗ return "Heun's method (2nd order) (explicit)";
467 ✗ case InertialIntegrator::IntegrationAlgorithm::SingleStepRungeKutta3:
468 ✗ return "Runge-Kutta 3rd order (explicit) / Simpson's rule";
469 ✗ case InertialIntegrator::IntegrationAlgorithm::SingleStepHeun3:
470 ✗ return "Heun's method (3nd order) (explicit)";
471 ✗ case InertialIntegrator::IntegrationAlgorithm::SingleStepRungeKutta4:
472 ✗ return "Runge-Kutta 4th order (explicit)";
473 ✗ case InertialIntegrator::IntegrationAlgorithm::MultiStepRK3:
474 ✗ return "Runge-Kutta 3rd order (explicit) / Simpson's rule (multistep)";
475 ✗ case InertialIntegrator::IntegrationAlgorithm::MultiStepRK4:
476 ✗ return "Runge-Kutta 4th order (explicit) (multistep)";
477 ✗ case InertialIntegrator::IntegrationAlgorithm::COUNT:
478 ✗ return "";
479 }
480 ✗ return "";
481 }
482
483 ✗ const char* to_string(InertialIntegrator::IntegrationFrame frame)
484 {
485 ✗ switch (frame)
486 {
487 ✗ case InertialIntegrator::IntegrationFrame::ECEF:
488 ✗ return "ECEF";
489 ✗ case InertialIntegrator::IntegrationFrame::NED:
490 ✗ return "NED";
491 }
492 ✗ return "";
493 }
494
495 } // namespace NAV
496