INSTINCT Code Coverage Report

Directory: src/
File: Navigation/INS/InertialIntegrator.cpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 142 291 48.8%
Functions: 19 23 82.6%
Branches: 191 598 31.9%
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 "util/Logger.hpp"
18
19 #include "Navigation/Ellipsoid/Ellipsoid.hpp"
20 #include "Navigation/INS/Functions.hpp"
21 #include "Navigation/INS/LocalNavFrame/Mechanization.hpp"
22 #include "Navigation/INS/EcefFrame/Mechanization.hpp"
23 #include "Navigation/Math/Math.hpp"
24
25 namespace NAV
26 {
27
28 54 void InertialIntegrator::reset()
29 {
30 54 _measurements.clear();
31 54 _states.clear();
32
2/4
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
 54 p_lastBiasAcceleration = Eigen::Vector3d::Zero();
33
2/4
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
 54 p_lastBiasAngularRate = Eigen::Vector3d::Zero();
34 54 setBufferSizes();
35 54 }
36
37 128399 bool InertialIntegrator::hasInitialPosition() const
38 {
39 128399 return !_states.empty();
40 }
41
42 18 void InertialIntegrator::setInitialState(const PosVelAtt& state)
43 {
44 18 _states.clear();
45 18 _states.push_back(state);
46 18 }
47
48 ✗ void InertialIntegrator::setState(const PosVelAtt& state)
49 {
50 ✗ _states.push_back(state);
51 ✗ }
52
53 17 void InertialIntegrator::setTotalSensorBiases(const Eigen::Vector3d& p_biasAcceleration, const Eigen::Vector3d& p_biasAngularRate)
54 {
55 17 p_lastBiasAcceleration = p_biasAcceleration;
56 17 p_lastBiasAngularRate = p_biasAngularRate;
57
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 17 times.
 17 if (!_measurements.empty())
58 {
59 ✗ _measurements.back().p_biasAcceleration = p_lastBiasAcceleration;
60 ✗ _measurements.back().p_biasAngularRate = p_lastBiasAngularRate;
61 }
62 17 }
63
64 26813 void InertialIntegrator::applySensorBiasesIncrements(const Eigen::Vector3d& p_deltaBiasAcceleration, const Eigen::Vector3d& p_deltaBiasAngularRate)
65 {
66 26813 p_lastBiasAcceleration += p_deltaBiasAcceleration;
67 26813 p_lastBiasAngularRate += p_deltaBiasAngularRate;
68
1/2
✓ Branch 1 taken 26813 times.
✗ Branch 2 not taken.
 26813 if (!_measurements.empty())
69 {
70 26813 _measurements.back().p_biasAcceleration = p_lastBiasAcceleration;
71 26813 _measurements.back().p_biasAngularRate = p_lastBiasAngularRate;
72 }
73 26813 }
74
75 25517 void InertialIntegrator::applyStateErrors_n(const Eigen::Vector3d& lla_positionError, const Eigen::Vector3d& n_velocityError, const Eigen::Vector3d& n_attitudeError_b)
76 {
77
1/2
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
 25517 if (!_states.empty())
78 {
79
4/8
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 25517 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 25517 times.
✗ Branch 12 not taken.
 25517 _states.back().setPosition_lla(_states.back().lla_position() - lla_positionError);
80
4/8
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25517 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 25517 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 25517 times.
✗ Branch 12 not taken.
 25517 _states.back().setVelocity_n(_states.back().n_velocity() - n_velocityError);
81 // Attitude correction, see Titterton and Weston (2004), p. 407 eq. 13.15
82
7/14
✓ Branch 1 taken 25517 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 25517 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 25517 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 25517 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 25517 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 25517 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 25517 times.
✗ Branch 21 not taken.
 25517 Eigen::Matrix3d n_Dcm_b = (Eigen::Matrix3d::Identity() - math::skewSymmetricMatrix(n_attitudeError_b)) * _states.back().n_Quat_b().toRotationMatrix();
83
4/8
✓ 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.
 25517 _states.back().setAttitude_n_Quat_b(Eigen::Quaterniond(n_Dcm_b).normalized());
84
85 // TODO: Test this out again
86 // Attitude correction, see Titterton and Weston (2004), p. 407 eq. 13.16
87 // Eigen::Quaterniond n_Quat_b = posVelAtt->n_Quat_b()
88 // * (Eigen::AngleAxisd(attError(0), Eigen::Vector3d::UnitX())
89 // * Eigen::AngleAxisd(attError(1), Eigen::Vector3d::UnitY())
90 // * Eigen::AngleAxisd(attError(2), Eigen::Vector3d::UnitZ()))
91 // .normalized();
92 // posVelAttCorrected->setAttitude_n_Quat_b(n_Quat_b.normalized());
93
94 // Eigen::Vector3d attError = pvaError->n_attitudeError();
95 // const Eigen::Quaterniond& n_Quat_b = posVelAtt->n_Quat_b();
96 // 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()),
97 // n_Quat_b.x() + 0.5 * (-attError(0) * n_Quat_b.w() + attError(1) * n_Quat_b.z() - attError(2) * n_Quat_b.y()),
98 // n_Quat_b.y() + 0.5 * (-attError(0) * n_Quat_b.z() - attError(1) * n_Quat_b.w() + attError(2) * n_Quat_b.x()),
99 // n_Quat_b.z() + 0.5 * (+attError(0) * n_Quat_b.y() - attError(1) * n_Quat_b.x() - attError(2) * n_Quat_b.w()) };
100 // posVelAttCorrected->setAttitude_n_Quat_b(n_Quat_b_c.normalized());
101 }
102 25517 }
103
104 1296 void InertialIntegrator::applyStateErrors_e(const Eigen::Vector3d& e_positionError, const Eigen::Vector3d& e_velocityError, const Eigen::Vector3d& e_attitudeError_b)
105 {
106
1/2
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
 1296 if (!_states.empty())
107 {
108
4/8
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1296 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1296 times.
✗ Branch 12 not taken.
 1296 _states.back().setPosition_e(_states.back().e_position() - e_positionError);
109
4/8
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1296 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1296 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1296 times.
✗ Branch 12 not taken.
 1296 _states.back().setVelocity_e(_states.back().e_velocity() - e_velocityError);
110 // Attitude correction, see Titterton and Weston (2004), p. 407 eq. 13.15
111
7/14
✓ Branch 1 taken 1296 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1296 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1296 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1296 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1296 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1296 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1296 times.
✗ Branch 21 not taken.
 1296 Eigen::Matrix3d e_Dcm_b = (Eigen::Matrix3d::Identity() - math::skewSymmetricMatrix(e_attitudeError_b)) * _states.back().e_Quat_b().toRotationMatrix();
112
4/8
✓ 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.
 1296 _states.back().setAttitude_e_Quat_b(Eigen::Quaterniond(e_Dcm_b).normalized());
113 }
114 1296 }
115
116 99995 const ScrollingBuffer<InertialIntegrator::Measurement>& InertialIntegrator::getMeasurements() const
117 {
118 99995 return _measurements;
119 }
120
121 266834 std::optional<std::reference_wrapper<const PosVelAtt>> InertialIntegrator::getLatestState() const
122 {
123
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 266834 times.
 266834 if (_states.empty()) { return {}; }
124
1/2
✓ Branch 1 taken 266834 times.
✗ Branch 2 not taken.
 266834 return _states.back();
125 }
126
127 52589 const Eigen::Vector3d& InertialIntegrator::p_getLastAccelerationBias() const
128 {
129 52589 return p_lastBiasAcceleration;
130 }
131
132 52589 const Eigen::Vector3d& InertialIntegrator::p_getLastAngularRateBias() const
133 {
134 52589 return p_lastBiasAngularRate;
135 }
136
137 216624 InertialIntegrator::IntegrationFrame InertialIntegrator::getIntegrationFrame() const
138 {
139 216624 return _integrationFrame;
140 }
141
142 49997 std::optional<Eigen::Vector3d> InertialIntegrator::p_calcCurrentAcceleration() const
143 {
144
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 49997 times.
 49997 if (_measurements.empty()) { return {}; }
145
146
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 _measurements.back().p_acceleration + _measurements.back().p_biasAcceleration;
147 }
148
149 26813 std::optional<Eigen::Vector3d> InertialIntegrator::p_calcCurrentAngularRate() const
150 {
151
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 26813 times.
 26813 if (_measurements.empty()) { return {}; }
152
153
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 _measurements.back().p_angularRate + _measurements.back().p_biasAngularRate;
154 }
155
156 49998 std::shared_ptr<PosVelAtt> InertialIntegrator::calcInertialSolution(const InsTime& obsTime, const Eigen::Vector3d& p_acceleration,
157 const Eigen::Vector3d& p_angularRate, const ImuPos& imuPos)
158 {
159
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().insTime) { return nullptr; }
160
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 49998 times.
 49998 if (_states.back().insTime.empty()) { _states.back().insTime = obsTime; }
161
162
5/10
✓ Branch 1 taken 49998 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49998 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 49998 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 49998 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 49998 times.
✗ Branch 15 not taken.
 99996 _measurements.push_back(Measurement{ .dt = static_cast<double>((obsTime - _states.back().insTime).count()),
163 .p_acceleration = p_acceleration,
164 .p_angularRate = p_angularRate,
165
1/2
✓ Branch 1 taken 49998 times.
✗ Branch 2 not taken.
 49998 .p_biasAcceleration = p_lastBiasAcceleration,
166
1/2
✓ Branch 1 taken 49998 times.
✗ Branch 2 not taken.
 49998 .p_biasAngularRate = p_lastBiasAngularRate });
167
168 49998 return calcInertialSolutionFromMeasurementBuffer(imuPos);
169 }
170
171 786 std::shared_ptr<PosVelAtt> InertialIntegrator::calcInertialSolutionDelta(const InsTime& obsTime, const double& dt,
172 const Eigen::Vector3d& p_deltaVelocity, const Eigen::Vector3d& p_deltaTheta,
173 const ImuPos& imuPos)
174 {
175
5/10
✓ Branch 1 taken 786 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 786 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 786 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 786 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 786 times.
 786 if (!hasInitialPosition() || obsTime < _states.back().insTime) { return nullptr; }
176
2/6
✓ Branch 1 taken 786 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 786 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 786 if (_states.back().insTime.empty()) { _states.back().insTime = obsTime; }
177
178
2/4
✓ Branch 1 taken 786 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 786 times.
✗ Branch 5 not taken.
 786 double dTimeLastState = static_cast<double>((obsTime - _states.back().insTime).count());
179
180
2/4
✓ Branch 1 taken 786 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 786 times.
✗ Branch 5 not taken.
 786 Eigen::Vector3d p_acceleration = Eigen::Vector3d::Zero();
181
2/4
✓ Branch 1 taken 786 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 786 times.
✗ Branch 5 not taken.
 786 Eigen::Vector3d p_angularRate = Eigen::Vector3d::Zero();
182
183
1/2
✓ Branch 0 taken 786 times.
✗ Branch 1 not taken.
 786 if (dt > 0.0) // dt given by sensor (should never be 0 or negative, but check here just in case)
184 {
185
2/4
✓ Branch 1 taken 786 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 786 times.
✗ Branch 5 not taken.
 786 p_acceleration = p_deltaVelocity / dt;
186
2/4
✓ Branch 1 taken 786 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 786 times.
✗ Branch 5 not taken.
 786 p_angularRate = p_deltaTheta / dt;
187 }
188 ✗ else if (std::abs(dTimeLastState) > 0.0) // Time difference between messages (differs from dt if message lost)
189 {
190 // Negative values of dTimeLastState should not happen, but algorithm can work with it to propagate backwards
191 ✗ p_acceleration = p_deltaVelocity / dTimeLastState;
192 ✗ p_angularRate = p_deltaTheta / dTimeLastState;
193 }
194
195
3/6
✓ Branch 1 taken 786 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 786 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 786 times.
✗ Branch 8 not taken.
 1572 _measurements.push_back(Measurement{ .dt = dTimeLastState,
196 .p_acceleration = p_acceleration,
197 .p_angularRate = p_angularRate,
198
1/2
✓ Branch 1 taken 786 times.
✗ Branch 2 not taken.
 786 .p_biasAcceleration = p_lastBiasAcceleration,
199
1/2
✓ Branch 1 taken 786 times.
✗ Branch 2 not taken.
 786 .p_biasAngularRate = p_lastBiasAngularRate });
200
201
1/2
✓ Branch 1 taken 786 times.
✗ Branch 2 not taken.
 786 return calcInertialSolutionFromMeasurementBuffer(imuPos);
202 }
203
204 50784 std::shared_ptr<PosVelAtt> InertialIntegrator::calcInertialSolutionFromMeasurementBuffer(const ImuPos& imuPos)
205 {
206 LOG_DATA("New measurement: dt = {:.5f}, p_accel [{}], p_angRate [{}], p_biasAccel [{}], p_biasAngRate [{}]", _measurements.back().dt,
207 _measurements.back().p_acceleration.transpose(), _measurements.back().p_angularRate.transpose(),
208 _measurements.back().p_biasAcceleration.transpose(), _measurements.back().p_biasAngularRate.transpose());
209
210
3/4
✓ Branch 1 taken 50784 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 50782 times.
 50784 if (std::abs(_measurements.back().dt) < 1e-8) // e.g. Initial state at 0.0s, first measurement at 0.0s --> Send out initial state
211 {
212
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 return std::make_shared<PosVelAtt>(_states.back());
213 }
214
215
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 50766 times.
 50782 if (_measurements.size() == 1) // e.g. Initial state at 0.0s, first measurement at 0.1s -> Assuming constant acceleration and angular rate
216 {
217
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 16 _measurements.push_back(_measurements.back());
218 }
219
220
1/2
✓ Branch 1 taken 50782 times.
✗ Branch 2 not taken.
 50782 const auto& posVelAtt__t1 = _states.back();
221
222
1/2
✓ Branch 1 taken 50782 times.
✗ Branch 2 not taken.
 50782 Eigen::Matrix<double, 10, 1> y;
223
2/3
✓ Branch 0 taken 37894 times.
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
 50782 switch (_integrationFrame)
224 {
225 37894 case IntegrationFrame::NED:
226 // 0 1 2 3 4 5 6 7 8 9
227 // [w, x, y, z, v_N, v_E, v_D, 𝜙, λ, h]^T
228
7/14
✓ Branch 2 taken 37894 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 37894 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 37894 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 37894 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 37894 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 37894 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 37894 times.
✗ Branch 24 not taken.
 37894 y.segment<4>(0) = Eigen::Vector4d{ posVelAtt__t1.n_Quat_b().w(), posVelAtt__t1.n_Quat_b().x(), posVelAtt__t1.n_Quat_b().y(), posVelAtt__t1.n_Quat_b().z() };
229
2/4
✓ Branch 2 taken 37894 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37894 times.
✗ Branch 6 not taken.
 37894 y.segment<3>(4) = posVelAtt__t1.n_velocity();
230
2/4
✓ Branch 2 taken 37894 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37894 times.
✗ Branch 6 not taken.
 37894 y.segment<3>(7) = posVelAtt__t1.lla_position();
231 37894 break;
232 12888 case IntegrationFrame::ECEF:
233 // 0 1 2 3 4 5 6 7 8 9
234 // [w, x, y, z, v_x, v_y, v_z, x, y, z]^T
235
7/14
✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 12888 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 12888 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 12888 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 12888 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 12888 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 12888 times.
✗ Branch 24 not taken.
 12888 y.segment<4>(0) = Eigen::Vector4d{ posVelAtt__t1.e_Quat_b().w(), posVelAtt__t1.e_Quat_b().x(), posVelAtt__t1.e_Quat_b().y(), posVelAtt__t1.e_Quat_b().z() };
236
2/4
✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
 12888 y.segment<3>(4) = posVelAtt__t1.e_velocity();
237
2/4
✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
 12888 y.segment<3>(7) = posVelAtt__t1.e_position();
238 12888 break;
239 }
240
241
1/2
✓ Branch 1 taken 50782 times.
✗ Branch 2 not taken.
 50782 double dt = _measurements.back().dt;
242
243
1/2
✓ Branch 1 taken 50782 times.
✗ Branch 2 not taken.
 50782 if (_measurements.size() == 2)
244 {
245
4/8
✓ Branch 1 taken 50782 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50782 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50782 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 50782 times.
✗ Branch 12 not taken.
 50782 Eigen::Vector3d b_accel__t1 = imuPos.b_quatAccel_p() * (_measurements.front().p_acceleration + _measurements.front().p_biasAcceleration);
246
4/8
✓ Branch 1 taken 50782 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50782 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50782 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 50782 times.
✗ Branch 12 not taken.
 50782 Eigen::Vector3d b_gyro__t1 = imuPos.b_quatGyro_p() * (_measurements.front().p_angularRate + _measurements.front().p_biasAngularRate);
247
4/8
✓ Branch 1 taken 50782 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50782 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50782 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 50782 times.
✗ Branch 12 not taken.
 50782 Eigen::Vector3d b_accel__t0 = imuPos.b_quatAccel_p() * (_measurements.back().p_acceleration + _measurements.back().p_biasAcceleration);
248
4/8
✓ Branch 1 taken 50782 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50782 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50782 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 50782 times.
✗ Branch 12 not taken.
 50782 Eigen::Vector3d b_gyro__t0 = imuPos.b_quatGyro_p() * (_measurements.back().p_angularRate + _measurements.back().p_biasAngularRate);
249
250 LOG_DATA("[{:.1f}] p_accel__t0 [{:+.10f} {:+.10f} {:+.10f}]; p_accel__t1 [{:+.10f} {:+.10f} {:+.10f}]",
251 dt, b_accel__t0.x(), b_accel__t0.y(), b_accel__t0.z(), b_accel__t1.x(), b_accel__t1.y(), b_accel__t1.z());
252 LOG_DATA("[{:.1f}] p_gyro__t0 [{:+.10f} {:+.10f} {:+.10f}]; p_gyro__t1 [{:+.10f} {:+.10f} {:+.10f}]",
253 dt, b_gyro__t0.x(), b_gyro__t0.y(), b_gyro__t0.z(), b_gyro__t1.x(), b_gyro__t1.y(), b_gyro__t1.z());
254
255
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 50782 times.
 50782 if (_integrationAlgorithm == IntegrationAlgorithm::SingleStepRungeKutta1)
256 {
257 ✗ std::array<Eigen::Vector<double, 6>, 1> z;
258 ✗ z[0] << b_accel__t1, b_gyro__t1;
259 ✗ switch (_integrationFrame)
260 {
261 ✗ case IntegrationFrame::NED:
262 ✗ y = RungeKutta1(y, z, dt, n_calcPosVelAttDerivative<double>, _posVelAttDerivativeConstants);
263 ✗ break;
264 ✗ case IntegrationFrame::ECEF:
265 ✗ y = RungeKutta1(y, z, dt, e_calcPosVelAttDerivative<double>, _posVelAttDerivativeConstants);
266 ✗ break;
267 }
268 }
269
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 50782 times.
 50782 else if (_integrationAlgorithm == IntegrationAlgorithm::SingleStepRungeKutta2)
270 {
271 ✗ std::array<Eigen::Vector<double, 6>, 2> z;
272 ✗ z[0] << b_accel__t1, b_gyro__t1;
273 ✗ z[1] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 0.5);
274 ✗ switch (_integrationFrame)
275 {
276 ✗ case IntegrationFrame::NED:
277 ✗ y = RungeKutta2(y, z, dt, n_calcPosVelAttDerivative<double>, _posVelAttDerivativeConstants);
278 ✗ break;
279 ✗ case IntegrationFrame::ECEF:
280 ✗ y = RungeKutta2(y, z, dt, e_calcPosVelAttDerivative<double>, _posVelAttDerivativeConstants);
281 ✗ break;
282 }
283 }
284
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 50782 times.
 50782 else if (_integrationAlgorithm == IntegrationAlgorithm::SingleStepHeun2)
285 {
286 ✗ std::array<Eigen::Vector<double, 6>, 2> z;
287 ✗ z[0] << b_accel__t1, b_gyro__t1;
288 ✗ z[1] << b_accel__t0, b_gyro__t0;
289 ✗ switch (_integrationFrame)
290 {
291 ✗ case IntegrationFrame::NED:
292 ✗ y = Heun2(y, z, dt, n_calcPosVelAttDerivative<double>, _posVelAttDerivativeConstants);
293 ✗ break;
294 ✗ case IntegrationFrame::ECEF:
295 ✗ y = Heun2(y, z, dt, e_calcPosVelAttDerivative<double>, _posVelAttDerivativeConstants);
296 ✗ break;
297 }
298 }
299
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 50782 times.
 50782 else if (_integrationAlgorithm == IntegrationAlgorithm::SingleStepRungeKutta3
300 ✗ || _integrationAlgorithm == IntegrationAlgorithm::MultiStepRK3)
301 {
302
1/2
✓ Branch 1 taken 50782 times.
✗ Branch 2 not taken.
 50782 std::array<Eigen::Vector<double, 6>, 3> z;
303
2/4
✓ Branch 2 taken 50782 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50782 times.
✗ Branch 6 not taken.
 50782 z[0] << b_accel__t1, b_gyro__t1;
304
4/8
✓ Branch 2 taken 50782 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50782 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50782 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50782 times.
✗ Branch 12 not taken.
 50782 z[1] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 0.5);
305
2/4
✓ Branch 2 taken 50782 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50782 times.
✗ Branch 6 not taken.
 50782 z[2] << b_accel__t0, b_gyro__t0;
306
2/3
✓ Branch 0 taken 37894 times.
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
 50782 switch (_integrationFrame)
307 {
308 37894 case IntegrationFrame::NED:
309
1/2
✓ Branch 1 taken 37894 times.
✗ Branch 2 not taken.
 37894 y = RungeKutta3(y, z, dt, n_calcPosVelAttDerivative<double>, _posVelAttDerivativeConstants);
310 37894 break;
311 12888 case IntegrationFrame::ECEF:
312
1/2
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
 12888 y = RungeKutta3(y, z, dt, e_calcPosVelAttDerivative<double>, _posVelAttDerivativeConstants);
313 12888 break;
314 }
315 50782 }
316 ✗ else if (_integrationAlgorithm == IntegrationAlgorithm::SingleStepHeun3)
317 {
318 ✗ std::array<Eigen::Vector<double, 6>, 3> z;
319 ✗ z[0] << b_accel__t1, b_gyro__t1;
320 ✗ z[1] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 1.0 / 3.0);
321 ✗ z[2] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 2.0 / 3.0);
322 ✗ switch (_integrationFrame)
323 {
324 ✗ case IntegrationFrame::NED:
325 ✗ y = RungeKutta3(y, z, dt, n_calcPosVelAttDerivative<double>, _posVelAttDerivativeConstants);
326 ✗ break;
327 ✗ case IntegrationFrame::ECEF:
328 ✗ y = RungeKutta3(y, z, dt, e_calcPosVelAttDerivative<double>, _posVelAttDerivativeConstants);
329 ✗ break;
330 }
331 }
332 ✗ else if (_integrationAlgorithm == IntegrationAlgorithm::SingleStepRungeKutta4
333 ✗ || _integrationAlgorithm == IntegrationAlgorithm::MultiStepRK4)
334 {
335 ✗ std::array<Eigen::Vector<double, 6>, 4> z;
336 ✗ z[0] << b_accel__t1, b_gyro__t1;
337 ✗ z[1] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 0.5);
338 ✗ z[2] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 0.5);
339 ✗ z[3] << b_accel__t0, b_gyro__t0;
340 ✗ switch (_integrationFrame)
341 {
342 ✗ case IntegrationFrame::NED:
343 ✗ y = RungeKutta4(y, z, dt, n_calcPosVelAttDerivative<double>, _posVelAttDerivativeConstants);
344 ✗ break;
345 ✗ case IntegrationFrame::ECEF:
346 ✗ y = RungeKutta4(y, z, dt, e_calcPosVelAttDerivative<double>, _posVelAttDerivativeConstants);
347 ✗ break;
348 }
349 }
350 }
351 else // if (_measurements.size() == 3)
352 {
353 ✗ if (_integrationAlgorithm == IntegrationAlgorithm::MultiStepRK3)
354 {
355 ✗ LOG_ERROR("Not implemented yet"); // TODO
356 }
357 ✗ else if (_integrationAlgorithm == IntegrationAlgorithm::MultiStepRK4)
358 {
359 ✗ LOG_ERROR("Not implemented yet"); // TODO
360 }
361 }
362
363
1/2
✓ Branch 1 taken 50782 times.
✗ Branch 2 not taken.
 50782 auto posVelAtt__t0 = std::make_shared<PosVelAtt>();
364
2/4
✓ Branch 2 taken 50782 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50782 times.
✗ Branch 6 not taken.
 50782 posVelAtt__t0->insTime = posVelAtt__t1.insTime + std::chrono::duration<double>(dt);
365
2/3
✓ Branch 0 taken 37894 times.
✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
 50782 switch (_integrationFrame)
366 {
367 37894 case IntegrationFrame::NED:
368
9/18
✓ Branch 2 taken 37894 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37894 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 37894 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 37894 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 37894 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 37894 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 37894 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 37894 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 37894 times.
✗ Branch 27 not taken.
 37894 posVelAtt__t0->setState_n(y.segment<3>(7), y.segment<3>(4), Eigen::Quaterniond{ y(0), y(1), y(2), y(3) }.normalized());
369 37894 break;
370 12888 case IntegrationFrame::ECEF:
371
9/18
✓ 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.
✓ Branch 17 taken 12888 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 12888 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 12888 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 12888 times.
✗ Branch 27 not taken.
 12888 posVelAtt__t0->setState_e(y.segment<3>(7), y.segment<3>(4), Eigen::Quaterniond{ y(0), y(1), y(2), y(3) }.normalized());
372 12888 break;
373 }
374
375 LOG_DATA("posVelAtt__t0->e_position() = {}", posVelAtt__t0->e_position().transpose());
376 LOG_DATA("posVelAtt__t0->lla_position() - posVelAtt__t1->lla_position() = {} [m]",
377 calcGeographicalDistance(posVelAtt__t0->latitude(), posVelAtt__t0->longitude(), posVelAtt__t1.latitude(), posVelAtt__t1.longitude()));
378 LOG_DATA("posVelAtt__t0->n_velocity() = {}", posVelAtt__t0->n_velocity().transpose());
379 LOG_DATA("posVelAtt__t0->e_velocity() = {}", posVelAtt__t0->e_velocity().transpose());
380 LOG_DATA("posVelAtt__t0->n_Quat_b() = {}", posVelAtt__t0->n_Quat_b());
381 LOG_DATA("posVelAtt__t0->e_Quat_b() = {}", posVelAtt__t0->e_Quat_b());
382
383
1/2
✓ Branch 2 taken 50782 times.
✗ Branch 3 not taken.
 50782 _states.push_back(*posVelAtt__t0);
384 50782 return posVelAtt__t0;
385 50782 }
386
387 73 void InertialIntegrator::setBufferSizes()
388 {
389
1/4
✓ Branch 0 taken 73 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 73 switch (_integrationAlgorithm)
390 {
391 73 case IntegrationAlgorithm::SingleStepRungeKutta1:
392 case IntegrationAlgorithm::SingleStepRungeKutta2:
393 case IntegrationAlgorithm::SingleStepHeun2:
394 case IntegrationAlgorithm::SingleStepRungeKutta3:
395 case IntegrationAlgorithm::SingleStepHeun3:
396 case IntegrationAlgorithm::SingleStepRungeKutta4:
397 73 _measurements.resize(2);
398 73 _states.resize(1);
399 73 break;
400 ✗ case IntegrationAlgorithm::MultiStepRK3:
401 case IntegrationAlgorithm::MultiStepRK4:
402 ✗ _measurements.resize(3);
403 ✗ _states.resize(2);
404 ✗ break;
405 ✗ case IntegrationAlgorithm::COUNT:
406 ✗ break;
407 }
408 73 }
409
410 ✗ bool InertialIntegratorGui(const char* label, InertialIntegrator& integrator, float width)
411 {
412 ✗ bool changed = false;
413
414 ✗ ImGui::SetNextItemWidth(width * gui::NodeEditorApplication::windowFontRatio());
415 ✗ if (auto integrationFrame = static_cast<int>(integrator._integrationFrame);
416 ✗ ImGui::Combo(fmt::format("Integration Frame##{}", label).c_str(), &integrationFrame, "ECEF\0NED\0\0"))
417 {
418 ✗ integrator._integrationFrame = static_cast<decltype(integrator._integrationFrame)>(integrationFrame);
419 ✗ LOG_DEBUG("Integration Frame changed to {}", integrator._integrationFrame == InertialIntegrator::IntegrationFrame::NED ? "NED" : "ECEF");
420 ✗ changed = true;
421 }
422
423 ✗ ImGui::SetNextItemWidth(width * gui::NodeEditorApplication::windowFontRatio());
424 ✗ if (ImGui::BeginCombo(fmt::format("Integration Algorithm##{}", label).c_str(), to_string(integrator._integrationAlgorithm)))
425 {
426 ✗ for (size_t i = 0; i < static_cast<size_t>(InertialIntegrator::IntegrationAlgorithm::MultiStepRK3); i++) // TODO: InertialIntegrator::IntegrationAlgorithm::COUNT
427 {
428 ✗ const bool is_selected = (static_cast<size_t>(integrator._integrationAlgorithm) == i);
429 ✗ if (ImGui::Selectable(to_string(static_cast<InertialIntegrator::IntegrationAlgorithm>(i)), is_selected))
430 {
431 ✗ integrator._integrationAlgorithm = static_cast<InertialIntegrator::IntegrationAlgorithm>(i);
432 ✗ integrator.setBufferSizes();
433 ✗ LOG_DEBUG("Integration Algorithm Attitude changed to {}", fmt::underlying(integrator._integrationAlgorithm));
434 ✗ changed = true;
435 }
436
437 // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
438 ✗ if (is_selected)
439 {
440 ✗ ImGui::SetItemDefaultFocus();
441 }
442 }
443
444 ✗ ImGui::EndCombo();
445 }
446
447 ✗ ImGui::SetNextItemOpen(false, ImGuiCond_FirstUseEver);
448 ✗ if (ImGui::TreeNode(fmt::format("Compensation models##{}", label).c_str()))
449 {
450 ✗ ImGui::TextUnformatted("Acceleration compensation");
451 {
452 ✗ ImGui::Indent();
453 ✗ ImGui::SetNextItemWidth(230 * gui::NodeEditorApplication::windowFontRatio());
454 ✗ if (ComboGravitationModel(fmt::format("Gravitation Model##{}", label).c_str(), integrator._posVelAttDerivativeConstants.gravitationModel))
455 {
456 ✗ LOG_DEBUG("Gravity Model changed to {}", NAV::to_string(integrator._posVelAttDerivativeConstants.gravitationModel));
457 ✗ changed = true;
458 }
459 ✗ if (ImGui::Checkbox(fmt::format("Coriolis acceleration ##{}", label).c_str(), &integrator._posVelAttDerivativeConstants.coriolisAccelerationCompensationEnabled))
460 {
461 ✗ LOG_DEBUG("coriolisAccelerationCompensationEnabled changed to {}", integrator._posVelAttDerivativeConstants.coriolisAccelerationCompensationEnabled);
462 ✗ changed = true;
463 }
464 ✗ if (ImGui::Checkbox(fmt::format("Centrifugal acceleration##{}", label).c_str(), &integrator._posVelAttDerivativeConstants.centrifgalAccelerationCompensationEnabled))
465 {
466 ✗ LOG_DEBUG("centrifgalAccelerationCompensationEnabled changed to {}", integrator._posVelAttDerivativeConstants.centrifgalAccelerationCompensationEnabled);
467 ✗ changed = true;
468 }
469 ✗ ImGui::Unindent();
470 }
471 ✗ ImGui::TextUnformatted("Angular rate compensation");
472 {
473 ✗ ImGui::Indent();
474 ✗ if (ImGui::Checkbox(fmt::format("Earth rotation rate##{}", label).c_str(), &integrator._posVelAttDerivativeConstants.angularRateEarthRotationCompensationEnabled))
475 {
476 ✗ LOG_DEBUG("angularRateEarthRotationCompensationEnabled changed to {}", integrator._posVelAttDerivativeConstants.angularRateEarthRotationCompensationEnabled);
477 ✗ changed = true;
478 }
479 ✗ if (integrator._integrationFrame == InertialIntegrator::IntegrationFrame::NED)
480 {
481 ✗ if (ImGui::Checkbox(fmt::format("Transport rate##{}", label).c_str(), &integrator._posVelAttDerivativeConstants.angularRateTransportRateCompensationEnabled))
482 {
483 ✗ LOG_DEBUG("angularRateTransportRateCompensationEnabled changed to {}", integrator._posVelAttDerivativeConstants.angularRateTransportRateCompensationEnabled);
484 ✗ changed = true;
485 }
486 }
487 ✗ ImGui::Unindent();
488 }
489
490 ✗ ImGui::TreePop();
491 }
492
493 ✗ return changed;
494 }
495
496 ✗ void to_json(json& j, const InertialIntegrator& data)
497 {
498 ✗ j = json{
499 ✗ { "integrationFrame", data._integrationFrame },
500 ✗ { "integrationAlgorithm", data._integrationAlgorithm },
501 ✗ { "posVelAttDerivativeConstants", data._posVelAttDerivativeConstants },
502 ✗ };
503 ✗ }
504
505 19 void from_json(const json& j, InertialIntegrator& data)
506 {
507
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 19 if (j.contains("integrationFrame")) { j.at("integrationFrame").get_to(data._integrationFrame); }
508
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 19 if (j.contains("integrationAlgorithm"))
509 {
510 19 j.at("integrationAlgorithm").get_to(data._integrationAlgorithm);
511 19 data.setBufferSizes();
512 }
513
1/2
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
 19 if (j.contains("posVelAttDerivativeConstants")) { j.at("posVelAttDerivativeConstants").get_to(data._posVelAttDerivativeConstants); }
514 19 }
515
516 ✗ const char* to_string(InertialIntegrator::IntegrationAlgorithm algorithm)
517 {
518 ✗ switch (algorithm)
519 {
520 ✗ case InertialIntegrator::IntegrationAlgorithm::SingleStepRungeKutta1:
521 ✗ return "Runge-Kutta 1st order (explicit) / (Forward) Euler method";
522 ✗ case InertialIntegrator::IntegrationAlgorithm::SingleStepRungeKutta2:
523 ✗ return "Runge-Kutta 2nd order (explicit)";
524 ✗ case InertialIntegrator::IntegrationAlgorithm::SingleStepHeun2:
525 ✗ return "Heun's method (2nd order) (explicit)";
526 ✗ case InertialIntegrator::IntegrationAlgorithm::SingleStepRungeKutta3:
527 ✗ return "Runge-Kutta 3rd order (explicit) / Simpson's rule";
528 ✗ case InertialIntegrator::IntegrationAlgorithm::SingleStepHeun3:
529 ✗ return "Heun's method (3nd order) (explicit)";
530 ✗ case InertialIntegrator::IntegrationAlgorithm::SingleStepRungeKutta4:
531 ✗ return "Runge-Kutta 4th order (explicit)";
532 ✗ case InertialIntegrator::IntegrationAlgorithm::MultiStepRK3:
533 ✗ return "Runge-Kutta 3rd order (explicit) / Simpson's rule (multistep)";
534 ✗ case InertialIntegrator::IntegrationAlgorithm::MultiStepRK4:
535 ✗ return "Runge-Kutta 4th order (explicit) (multistep)";
536 ✗ case InertialIntegrator::IntegrationAlgorithm::COUNT:
537 ✗ return "";
538 }
539 ✗ return "";
540 }
541
542 } // namespace NAV
543