0.4.1
Loading...
Searching...
No Matches
InertialIntegrator.hpp
Go to the documentation of this file.
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.hpp
10/// @brief Inertial Measurement Integrator
11/// @author T. Topp (topp@ins.uni-stuttgart.de)
12/// @date 2023-12-09
13
14#pragma once
15
16#include <functional>
17#include <optional>
18#include <memory>
19
22
29
31#include "util/Eigen.hpp"
32#include "util/Json.hpp"
33#include "util/Logger.hpp"
34
35namespace NAV
36{
37
38/// @brief Inertial Measurement Integrator
40{
41 public:
42 /// Available Integration Algorithms
43 enum class IntegrationAlgorithm : uint8_t
44 {
45 SingleStepRungeKutta1, ///< Runge-Kutta 1st order (explicit) / (Forward) Euler method
46 SingleStepRungeKutta2, ///< Runge-Kutta 2nd order (explicit) / Explicit midpoint method
47 SingleStepHeun2, ///< Heun's method (2nd order) (explicit)
48 SingleStepRungeKutta3, ///< Runge-Kutta 3rd order (explicit) / Simpson's rule
49 SingleStepHeun3, ///< Heun's method (3nd order) (explicit)
50 SingleStepRungeKutta4, ///< Runge-Kutta 4th order (explicit)
51 MultiStepRK3, ///< Multistep Runge-Kutta 3rd order (explicit) / Simpson's rule (taking 2 old epochs into account)
52 MultiStepRK4, ///< Multistep Runge-Kutta 4th order (explicit) (taking 2 old epochs into account)
53 COUNT, ///< Amount of available integration algorithms
54 };
55
56 /// Available Integration Frames
57 enum class IntegrationFrame : uint8_t
58 {
59 ECEF, ///< Earth-Centered Earth-Fixed frame
60 NED, ///< Local North-East-Down frame
61 };
62
63 /// Inertial Measurement
65 {
66 bool averagedMeasurement = false; ///< Wether the acceleration is averaged over the last epoch
67 double dt = 0.0; ///< Time since previous observation in [s]
68 Eigen::Vector3d p_acceleration; ///< Acceleration in platform frame coordinates in [m/s^2]
69 Eigen::Vector3d p_angularRate; ///< Angular rate in platform frame coordinates in [rad/s]
70 };
71
72 /// @brief Inertial state and measurements
73 template<typename T>
75 {
76 InsTime epoch; ///< Epoch of this state
77
78 Eigen::Vector3<T> position; ///< IMU position (e_pos / lla_pos)
79 Eigen::Vector3<T> velocity; ///< IMU velocity (e_vel / n_vel)
80 Eigen::Quaternion<T> attitude; ///< IMU attitude (e_Quat_b / n_Quat_b)
81
82 Measurement m; ///< Inertial measurement
83
84 Eigen::Vector3<T> p_biasAcceleration = Eigen::Vector3<T>::Zero(); ///< Acceleration bias in platform frame coordinates in [m/s^2]
85 Eigen::Vector3<T> p_biasAngularRate = Eigen::Vector3<T>::Zero(); ///< Angular rate bias in platform frame coordinates in [rad/s]
86
87 Eigen::Vector3<T> scaleFactorAccel = Eigen::Vector3<T>::Ones(); ///< Scale factor of the accelerometer [-]
88 Eigen::Vector3<T> scaleFactorGyro = Eigen::Vector3<T>::Ones(); ///< Scale factor of the gyroscope [-]
89
90 Eigen::Quaternion<T> misalignmentAccel = Eigen::Quaternion<T>::Identity(); ///< Misalignment of the accelerometer sensor axes
91 Eigen::Quaternion<T> misalignmentGyro = Eigen::Quaternion<T>::Identity(); ///< Misalignment of the gyroscope sensor axes
92 };
93
94 /// Inertial state and measurements
96
97 /// @brief Default Constructor
98 InertialIntegrator() = default;
99
100 /// @brief Constructor
101 /// @param integrationFrame Integration frame to lock to
102 explicit InertialIntegrator(IntegrationFrame integrationFrame);
103
104 /// @brief Clears all internal data
105 void reset();
106
107 /// @brief Checks if an initial position is set
108 [[nodiscard]] bool hasInitialPosition() const;
109
110 /// @brief Sets the initial state
111 /// @param[in] state State to set
112 /// @param[in] nameId NameId of the calling node for logging
113 void setInitialState(const PosVelAtt& state, const char* nameId);
114
115 /// @brief Pushes the state to the list of states
116 /// @param[in] state State to set
117 /// @param[in] nameId NameId of the calling node for logging
118 void addState(const PosVelAtt& state, const char* nameId);
119
120 /// @brief Sets the sensor biases total values
121 /// @param[in] p_biasAcceleration Acceleration bias in platform frame coordinates in [m/s^2]
122 /// @param[in] p_biasAngularRate Angular rate bias in platform frame coordinates in [rad/s]
123 void setTotalSensorBiases(const Eigen::Vector3d& p_biasAcceleration, const Eigen::Vector3d& p_biasAngularRate);
124
125 /// @brief Sets the sensor biases increment
126 /// @param[in] p_deltaBiasAcceleration Acceleration bias increment in platform frame coordinates in [m/s^2]
127 /// @param[in] p_deltaBiasAngularRate Angular rate bias increment in platform frame coordinates in [rad/s]
128 void applySensorBiasesIncrements(const Eigen::Vector3d& p_deltaBiasAcceleration, const Eigen::Vector3d& p_deltaBiasAngularRate);
129
130 /// @brief Apply the errors to the latest state
131 /// @param[in] lla_positionError δ𝐩_n = [δ𝜙 δλ δ𝘩] The position error (latitude, longitude, altitude) in [rad, rad, m]
132 /// @param[in] n_velocityError δ𝐯_n The velocity error in n frame coordinates in [m/s]
133 /// @param[in] n_attitudeError_b δ𝛙_nb_n The attitude error in n frame coordinates in [rad]
134 void applyStateErrors_n(const Eigen::Vector3d& lla_positionError, const Eigen::Vector3d& n_velocityError, const Eigen::Vector3d& n_attitudeError_b);
135
136 /// @brief Apply the errors to the latest state
137 /// @param[in] e_positionError δr_e The position error in e frame coordinates in [m]
138 /// @param[in] e_velocityError δ𝐯_e The velocity error in e frame coordinates in [m/s]
139 /// @param[in] e_attitudeError_b δ𝛙_eb_e The attitude error in e frame coordinates in [rad]
140 void applyStateErrors_e(const Eigen::Vector3d& e_positionError, const Eigen::Vector3d& e_velocityError, const Eigen::Vector3d& e_attitudeError_b);
141
142 /// Get the latest state if it exists
143 [[nodiscard]] std::optional<std::reference_wrapper<const State>> getLatestState() const;
144
145 /// @brief Return the last acceleration bias in platform frame coordinates in [m/s^2]
146 [[nodiscard]] const Eigen::Vector3d& p_getLastAccelerationBias() const;
147
148 /// @brief Return the last angular rate bias in platform frame coordinates in [rad/s]
149 [[nodiscard]] const Eigen::Vector3d& p_getLastAngularRateBias() const;
150
151 /// @brief Returns the selected integration frame
152 [[nodiscard]] IntegrationFrame getIntegrationFrame() const;
153
154 /// @brief Returns the selected compensation models
155 [[nodiscard]] const PosVelAttDerivativeConstants& getConstants() const;
156
157 /// Wether the measurements are accumulated values over the last epoch. (always true when using delta measurements, so GUI has no effect)
158 [[nodiscard]] bool areAccelerationsAveragedMeasurements() const;
159
160 /// Calculate the current acceleration, if measurements area available
161 [[nodiscard]] std::optional<Eigen::Vector3d> p_calcCurrentAcceleration() const;
162
163 /// Calculate the current angular rate, if measurements area available
164 [[nodiscard]] std::optional<Eigen::Vector3d> p_calcCurrentAngularRate() const;
165
166 /// @brief Calculates the inertial navigation solution
167 /// @param[in] obsTime Time of the observation
168 /// @param[in] p_acceleration Acceleration in platform frame coordinates in [m/s^2]
169 /// @param[in] p_angularRate Angular rate in platform frame coordinates in [rad/s]
170 /// @param[in] imuPos IMU platform frame position with respect to body frame
171 /// @param[in] nameId NameId of the calling node for logging
172 /// @return The new state at the observation time
173 std::shared_ptr<PosVelAtt> calcInertialSolution(const InsTime& obsTime,
174 const Eigen::Vector3d& p_acceleration, const Eigen::Vector3d& p_angularRate,
175 const ImuPos& imuPos, const char* nameId);
176
177 /// @brief Calculates the inertial navigation solution
178 /// @param[in] obsTime Time of the observation
179 /// @param[in] deltaTime Delta time over which the deltaVelocity and deltaTheta were measured in [s]
180 /// @param[in] p_deltaVelocity Integrated acceleration in platform frame coordinates in [m/s]
181 /// @param[in] p_deltaTheta Integrated angular rate in platform frame coordinates in [rad]
182 /// @param[in] imuPos IMU platform frame position with respect to body frame
183 /// @param[in] nameId NameId of the calling node for logging
184 /// @return The new state at the observation time
185 std::shared_ptr<PosVelAtt> calcInertialSolutionDelta(const InsTime& obsTime, double deltaTime,
186 const Eigen::Vector3d& p_deltaVelocity, const Eigen::Vector3d& p_deltaTheta,
187 const ImuPos& imuPos, const char* nameId);
188
189 // Forward declaring external function
190 friend const char* to_string(InertialIntegrator::IntegrationAlgorithm algorithm);
191 friend const char* to_string(InertialIntegrator::IntegrationFrame frame);
192
193 /// @brief Calculates the inertial solution going from state__t1 to state__t0 given that measurements are available for both states
194 /// @param imuPos IMU mounting position connecting the platform to the body frame
195 /// @param state__t0 State at the epoch to calculate (measurements only)
196 /// @param state__t1 State at the previous epoch (state + measurements)
197 /// @param nameId NameId of the calling node for logging
198 /// @return Position, velocity and attitude from the integration step
199 template<typename T>
200 [[nodiscard]] Eigen::Vector<T, 10> calcInertialSolution(const ImuPos& imuPos,
201 const GenericState<T>& state__t0,
202 const GenericState<T>& state__t1,
203 [[maybe_unused]] const char* nameId) const
204 {
205 // #if LOG_LEVEL <= LOG_LEVEL_DATA
206 // auto printState = [](const GenericState<T>& state, const char* t, const char* nameId) {
207 // LOG_DATA("{} [{}]:\n"
208 // " - {}:\n"
209 // " Position [{}, {}, {}], Velocity [{}, {}, {}], Attitude [{}x, {}y, {}z, {}w]\n"
210 // " dt = {:.5f}, p_accel [{}, {}, {}], p_angRate [{}, {}, {}]\n"
211 // " p_biasAccel [{}, {}, {}], p_biasAngRate [{}, {}, {}]\n"
212 // " p_scaleFacAccel [{}, {}, {}], p_scaleFacAngRate [{}, {}, {}]\n"
213 // " p_misAlignAccel [{}x, {}y, {}z, {}w], p_misAlignAngRate [{}x, {}y, {}z, {}w]",
214 // nameId, t, state.epoch.toYMDHMS(GPST),
215 // state.position(0), state.position(1), state.position(2),
216 // state.velocity(0), state.velocity(1), state.velocity(2),
217 // state.attitude.x(), state.attitude.y(), state.attitude.z(), state.attitude.w(),
218 // state.m.dt, state.m.p_acceleration(0), state.m.p_acceleration(1), state.m.p_acceleration(2),
219 // state.m.p_angularRate(0), state.m.p_angularRate(1), state.m.p_angularRate(2),
220 // state.p_biasAcceleration(0), state.p_biasAcceleration(1), state.p_biasAcceleration(2),
221 // state.p_biasAngularRate(0), state.p_biasAngularRate(1), state.p_biasAngularRate(2),
222 // state.scaleFactorAccel(0), state.scaleFactorAccel(1), state.scaleFactorAccel(2),
223 // state.scaleFactorGyro(0), state.scaleFactorGyro(1), state.scaleFactorGyro(2),
224 // state.misalignmentAccel.x(), state.misalignmentAccel.y(), state.misalignmentAccel.z(), state.misalignmentAccel.w(),
225 // state.misalignmentGyro.x(), state.misalignmentGyro.y(), state.misalignmentGyro.z(), state.misalignmentGyro.w());
226 // };
227 // LOG_DATA("{}: Frame {}, Algorithm {}", nameId, to_string(_integrationFrame), to_string(_integrationAlgorithm));
228 // printState(state__t0, "t0", nameId);
229 // printState(state__t1, "t1", nameId);
230 // #endif
231
232 Eigen::Vector<T, 10> y;
233 // 0 1 2 3 4 5 6 7 8 9
234 // NED [ 𝜙, λ, h, v_N, v_E, v_D, n_q_bx, n_q_by, n_q_bz, n_q_bw]^T
235 // ECEF [ x, y, z, v_x, v_y, v_z, e_q_bx, e_q_by, e_q_bz, e_q_bw]^T
236 y.template segment<3>(0) = state__t1.position;
237 y.template segment<3>(3) = state__t1.velocity;
238 y.template segment<4>(6) = state__t1.attitude.coeffs();
239
240 auto p_accel = [](const GenericState<T>& state) -> Eigen::Vector3<T> {
241 return (state.misalignmentAccel * state.m.p_acceleration.template cast<T>()).cwiseProduct(state.scaleFactorAccel) + state.p_biasAcceleration;
242 };
243 auto p_gyro = [](const GenericState<T>& state) -> Eigen::Vector3<T> {
244 return (state.misalignmentGyro * state.m.p_angularRate.template cast<T>()).cwiseProduct(state.scaleFactorGyro) + state.p_biasAngularRate;
245 };
246
247 // LOG_DATA("{}: imuPos.b_quat_p() [{}]", nameId, imuPos.b_quat_p());
248 Eigen::Vector3<T> b_accel__t0 = imuPos.b_quat_p().cast<T>() * p_accel(state__t0);
249 Eigen::Vector3<T> b_accel__t1 = imuPos.b_quat_p().cast<T>() * p_accel(state__t1);
250 Eigen::Vector3<T> b_gyro__t0 = imuPos.b_quat_p().cast<T>() * p_gyro(state__t0);
251 Eigen::Vector3<T> b_gyro__t1 = imuPos.b_quat_p().cast<T>() * p_gyro(state__t1);
252 // LOG_DATA("{}: b_accel__t0 [{}, {}, {}], b_accel__t1 [{}, {}, {}]", nameId, b_accel__t0(0), b_accel__t0(1), b_accel__t0(2), b_accel__t1(0), b_accel__t1(1), b_accel__t1(2));
253 // LOG_DATA("{}: b_gyro__t0 [{}, {}, {}], b_gyro__t1 [{}, {}, {}]", nameId, b_gyro__t0(0), b_gyro__t0(1), b_gyro__t0(2), b_gyro__t1(0), b_gyro__t1(1), b_gyro__t1(2));
254
255 // LOG_DATA("{}: integrationAlgorithm = {}", nameId, to_string(_integrationAlgorithm));
256 switch (_integrationAlgorithm)
257 {
259 {
260 std::array<Eigen::Vector<T, 6>, 1> z;
261 if (state__t0.m.averagedMeasurement) { z[0] << b_accel__t0, b_gyro__t0; }
262 else { z[0] << b_accel__t1, b_gyro__t1; }
263 // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5));
264
265 switch (_integrationFrame)
266 {
269 break;
272 break;
273 }
274 break;
275 }
277 {
278 std::array<Eigen::Vector<T, 6>, 2> z;
279 if (state__t0.m.averagedMeasurement)
280 {
281 z[0] << b_accel__t0, b_gyro__t0;
282 z[1] << b_accel__t0, b_gyro__t0;
283 }
284 else
285 {
286 z[0] << b_accel__t1, b_gyro__t1;
287 z[1] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 0.5);
288 }
289 // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5));
290 // LOG_DATA("{}: z[1] = {}, {}, {}, {}, {}, {}", nameId, z[1](0), z[1](1), z[1](2), z[1](3), z[1](4), z[1](5));
291 switch (_integrationFrame)
292 {
295 break;
298 break;
299 }
300 break;
301 }
303 {
304 std::array<Eigen::Vector<T, 6>, 2> z;
305 if (state__t0.m.averagedMeasurement)
306 {
307 z[0] << b_accel__t0, b_gyro__t0;
308 z[1] << b_accel__t0, b_gyro__t0;
309 }
310 else
311 {
312 z[0] << b_accel__t1, b_gyro__t1;
313 z[1] << b_accel__t0, b_gyro__t0;
314 }
315 // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5));
316 // LOG_DATA("{}: z[1] = {}, {}, {}, {}, {}, {}", nameId, z[1](0), z[1](1), z[1](2), z[1](3), z[1](4), z[1](5));
317 switch (_integrationFrame)
318 {
321 break;
324 break;
325 }
326 break;
327 }
329 {
330 std::array<Eigen::Vector<T, 6>, 3> z;
331 if (state__t0.m.averagedMeasurement)
332 {
333 z[0] << b_accel__t0, b_gyro__t0;
334 z[1] << b_accel__t0, b_gyro__t0;
335 z[2] << b_accel__t0, b_gyro__t0;
336 }
337 else
338 {
339 z[0] << b_accel__t1, b_gyro__t1;
340 z[1] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 0.5);
341 z[2] << b_accel__t0, b_gyro__t0;
342 }
343 // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5));
344 // LOG_DATA("{}: z[1] = {}, {}, {}, {}, {}, {}", nameId, z[1](0), z[1](1), z[1](2), z[1](3), z[1](4), z[1](5));
345 // LOG_DATA("{}: z[2] = {}, {}, {}, {}, {}, {}", nameId, z[2](0), z[2](1), z[2](2), z[2](3), z[2](4), z[2](5));
346 switch (_integrationFrame)
347 {
350 break;
353 break;
354 }
355 break;
356 }
358 {
359 std::array<Eigen::Vector<T, 6>, 3> z;
360 if (state__t0.m.averagedMeasurement)
361 {
362 z[0] << b_accel__t0, b_gyro__t0;
363 z[1] << b_accel__t0, b_gyro__t0;
364 z[2] << b_accel__t0, b_gyro__t0;
365 }
366 else
367 {
368 z[0] << b_accel__t1, b_gyro__t1;
369 z[1] << math::lerp(b_accel__t1, b_accel__t0, 1.0 / 3.0), math::lerp(b_gyro__t1, b_gyro__t0, 1.0 / 3.0);
370 z[2] << math::lerp(b_accel__t1, b_accel__t0, 2.0 / 3.0), math::lerp(b_gyro__t1, b_gyro__t0, 2.0 / 3.0);
371 }
372 // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5));
373 // LOG_DATA("{}: z[1] = {}, {}, {}, {}, {}, {}", nameId, z[1](0), z[1](1), z[1](2), z[1](3), z[1](4), z[1](5));
374 // LOG_DATA("{}: z[2] = {}, {}, {}, {}, {}, {}", nameId, z[2](0), z[2](1), z[2](2), z[2](3), z[2](4), z[2](5));
375 switch (_integrationFrame)
376 {
379 break;
382 break;
383 }
384 break;
385 }
387 {
388 std::array<Eigen::Vector<T, 6>, 4> z;
389 if (state__t0.m.averagedMeasurement)
390 {
391 z[0] << b_accel__t0, b_gyro__t0;
392 z[1] << b_accel__t0, b_gyro__t0;
393 z[2] << b_accel__t0, b_gyro__t0;
394 z[3] << b_accel__t0, b_gyro__t0;
395 }
396 else
397 {
398 z[0] << b_accel__t1, b_gyro__t1;
399 z[1] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 0.5);
400 z[2] << math::lerp(b_accel__t1, b_accel__t0, 0.5), math::lerp(b_gyro__t1, b_gyro__t0, 0.5);
401 z[3] << b_accel__t0, b_gyro__t0;
402 }
403 // LOG_DATA("{}: z[0] = {}, {}, {}, {}, {}, {}", nameId, z[0](0), z[0](1), z[0](2), z[0](3), z[0](4), z[0](5));
404 // LOG_DATA("{}: z[1] = {}, {}, {}, {}, {}, {}", nameId, z[1](0), z[1](1), z[1](2), z[1](3), z[1](4), z[1](5));
405 // LOG_DATA("{}: z[2] = {}, {}, {}, {}, {}, {}", nameId, z[2](0), z[2](1), z[2](2), z[2](3), z[2](4), z[2](5));
406 // LOG_DATA("{}: z[3] = {}, {}, {}, {}, {}, {}", nameId, z[3](0), z[3](1), z[3](2), z[3](3), z[3](4), z[3](5));
407 switch (_integrationFrame)
408 {
411 break;
414 break;
415 }
416 break;
417 }
419 {
420 LOG_CRITICAL("Not implemented yet");
421 break;
422 }
424 {
425 LOG_CRITICAL("Not implemented yet");
426 break;
427 }
429 {
430 LOG_CRITICAL("Unreachable");
431 break;
432 }
433 }
434
435 y.template segment<4>(6) = Eigen::Quaternion<T>(y.template segment<4>(6)).normalized().coeffs();
436
437 return y;
438 }
439
440 private:
441 /// @brief Adds the measurement to the tracking buffer
442 /// @param[in] epoch Epoch of the measurement
443 /// @param[in] dt Time between observation and last state in [s]
444 /// @param[in] p_acceleration Acceleration in platform frame coordinates in [m/s^2]
445 /// @param[in] p_angularRate Angular rate in platform frame coordinates in [rad/s]
446 /// @param[in] nameId NameId of the calling node for logging
447 void addMeasurement(const InsTime& epoch, double dt,
448 const Eigen::Vector3d& p_acceleration, const Eigen::Vector3d& p_angularRate, const char* nameId);
449
450 /// @brief Adds the measurement to the tracking buffer
451 /// @param[in] epoch Epoch of the measurement
452 /// @param[in] dt Time between observation and last state in [s]
453 /// @param[in] deltaTime Delta time over which the deltaVelocity and deltaTheta were measured in [s]
454 /// @param[in] p_deltaVelocity Integrated acceleration in platform frame coordinates in [m/s]
455 /// @param[in] p_deltaTheta Integrated angular rate in platform frame coordinates in [rad]
456 /// @param[in] nameId NameId of the calling node for logging
457 void addDeltaMeasurement(const InsTime& epoch, double dt, double deltaTime,
458 const Eigen::Vector3d& p_deltaVelocity, const Eigen::Vector3d& p_deltaTheta, const char* nameId);
459
460 /// @brief Returns the last state as PosVelAtt
461 [[nodiscard]] std::shared_ptr<PosVelAtt> lastStateAsPosVelAtt() const;
462
463 /// @brief Calculates the inertial navigation solution
464 /// @param[in] imuPos IMU platform frame position with respect to body frame
465 /// @param[in] nameId NameId of the calling node for logging
466 /// @return The new state at the observation time
467 std::shared_ptr<PosVelAtt> calcInertialSolutionFromMeasurementBuffer(const ImuPos& imuPos, const char* nameId);
468
469 /// @brief Resizes the measurement and state buffers depending on the integration algorithm
470 void setBufferSizes();
471
472 /// List of states. Length depends on algorithm used
474
475 Eigen::Vector3d _p_lastBiasAcceleration = Eigen::Vector3d::Zero(); ///< Initial values for the acceleration bias [m/s^2]
476 Eigen::Vector3d _p_lastBiasAngularRate = Eigen::Vector3d::Zero(); ///< Initial values for the angular rate bias [rad/s]
477
478 // #########################################################################################################################################
479
480 /// Frame to integrate the observations in
482
483 /// Wether to lock the integration frame
485
486 /// Integration algorithm used for the update
488
489 /// If true, then the measurements are accumulated values over the last epoch. (always true when using delta measurements, so GUI has no effect)
491
492 // #########################################################################################################################################
493
494 /// Settings for the models to use
496
497 friend bool InertialIntegratorGui(const char* label, InertialIntegrator& integrator, bool& preferAccelerationOverDeltaMeasurements, float width);
498 friend void to_json(json& j, const InertialIntegrator& data);
499 friend void from_json(const json& j, InertialIntegrator& data);
500};
501
502/// @brief Shows a GUI for advanced configuration of the InertialIntegrator
503/// @param[in] label Label to show. This has to be a unique id for ImGui.
504/// @param[in] integrator Reference to the integrator to configure
505/// @param[in] preferAccelerationOverDeltaMeasurements Wether to prefer accelerations over delta measurements
506/// @param[in] width Width of the widget
507bool InertialIntegratorGui(const char* label, InertialIntegrator& integrator, bool& preferAccelerationOverDeltaMeasurements, float width = 250.0F);
508
509/// @brief Write info to a json object
510/// @param[out] j Json output
511/// @param[in] data Object to read info from
512void to_json(json& j, const InertialIntegrator& data);
513/// @brief Read info from a json object
514/// @param[in] j Json variable to read info from
515/// @param[out] data Output object
516void from_json(const json& j, InertialIntegrator& data);
517
518/// @brief Converts the enum to a string
519/// @param[in] algorithm Enum value to convert into text
520/// @return String representation of the enum
522
523/// @brief Converts the enum to a string
524/// @param[in] frame Enum value to convert into text
525/// @return String representation of the enum
527
528} // namespace NAV
Inertial Navigation Mechanization Functions in ECEF frame.
Vector space operations.
nlohmann::json json
json namespace
Different Gravity Models.
Imu Position Data.
The class is responsible for all time-related tasks.
Defines how to save certain datatypes to json.
Inertial Navigation Mechanization Functions in local navigation frame.
Utility class for logging to console and file.
#define LOG_CRITICAL(...)
Critical Event, which causes the program to work entirely and throws an exception.
Definition Logger.hpp:75
Inertial Navigation Mechanization Functions.
Provides Numerical integration methods.
Position, Velocity and Attitude Storage Class.
A buffer which is overwriting itself from the start when full.
IMU Position.
Definition ImuPos.hpp:26
const Eigen::Quaterniond & b_quat_p() const
Quaternion from IMU platform frame to body frame.
Definition ImuPos.hpp:35
Inertial Measurement Integrator.
IntegrationAlgorithm _integrationAlgorithm
Integration algorithm used for the update.
void applyStateErrors_e(const Eigen::Vector3d &e_positionError, const Eigen::Vector3d &e_velocityError, const Eigen::Vector3d &e_attitudeError_b)
Apply the errors to the latest state.
std::optional< Eigen::Vector3d > p_calcCurrentAcceleration() const
Calculate the current acceleration, if measurements area available.
friend const char * to_string(InertialIntegrator::IntegrationAlgorithm algorithm)
Converts the enum to a string.
bool hasInitialPosition() const
Checks if an initial position is set.
Eigen::Vector< T, 10 > calcInertialSolution(const ImuPos &imuPos, const GenericState< T > &state__t0, const GenericState< T > &state__t1, const char *nameId) const
Calculates the inertial solution going from state__t1 to state__t0 given that measurements are availa...
Eigen::Vector3d _p_lastBiasAcceleration
Initial values for the acceleration bias [m/s^2].
friend bool InertialIntegratorGui(const char *label, InertialIntegrator &integrator, bool &preferAccelerationOverDeltaMeasurements, float width)
Shows a GUI for advanced configuration of the InertialIntegrator.
std::shared_ptr< PosVelAtt > calcInertialSolution(const InsTime &obsTime, const Eigen::Vector3d &p_acceleration, const Eigen::Vector3d &p_angularRate, const ImuPos &imuPos, const char *nameId)
Calculates the inertial navigation solution.
std::optional< Eigen::Vector3d > p_calcCurrentAngularRate() const
Calculate the current angular rate, if measurements area available.
void addState(const PosVelAtt &state, const char *nameId)
Pushes the state to the list of states.
friend void to_json(json &j, const InertialIntegrator &data)
Write info to a json object.
void applyStateErrors_n(const Eigen::Vector3d &lla_positionError, const Eigen::Vector3d &n_velocityError, const Eigen::Vector3d &n_attitudeError_b)
Apply the errors to the latest state.
IntegrationAlgorithm
Available Integration Algorithms.
@ SingleStepRungeKutta4
Runge-Kutta 4th order (explicit)
@ MultiStepRK4
Multistep Runge-Kutta 4th order (explicit) (taking 2 old epochs into account)
@ SingleStepHeun2
Heun's method (2nd order) (explicit)
@ COUNT
Amount of available integration algorithms.
@ MultiStepRK3
Multistep Runge-Kutta 3rd order (explicit) / Simpson's rule (taking 2 old epochs into account)
@ SingleStepRungeKutta3
Runge-Kutta 3rd order (explicit) / Simpson's rule.
@ SingleStepRungeKutta2
Runge-Kutta 2nd order (explicit) / Explicit midpoint method.
@ SingleStepHeun3
Heun's method (3nd order) (explicit)
@ SingleStepRungeKutta1
Runge-Kutta 1st order (explicit) / (Forward) Euler method.
bool _accelerationsAreAveragedMeasurements
If true, then the measurements are accumulated values over the last epoch. (always true when using de...
void setTotalSensorBiases(const Eigen::Vector3d &p_biasAcceleration, const Eigen::Vector3d &p_biasAngularRate)
Sets the sensor biases total values.
const Eigen::Vector3d & p_getLastAccelerationBias() const
Return the last acceleration bias in platform frame coordinates in [m/s^2].
void setBufferSizes()
Resizes the measurement and state buffers depending on the integration algorithm.
friend void from_json(const json &j, InertialIntegrator &data)
Read info from a json object.
std::shared_ptr< PosVelAtt > calcInertialSolutionFromMeasurementBuffer(const ImuPos &imuPos, const char *nameId)
Calculates the inertial navigation solution.
void addMeasurement(const InsTime &epoch, double dt, const Eigen::Vector3d &p_acceleration, const Eigen::Vector3d &p_angularRate, const char *nameId)
Adds the measurement to the tracking buffer.
InertialIntegrator()=default
Default Constructor.
std::shared_ptr< PosVelAtt > calcInertialSolutionDelta(const InsTime &obsTime, double deltaTime, const Eigen::Vector3d &p_deltaVelocity, const Eigen::Vector3d &p_deltaTheta, const ImuPos &imuPos, const char *nameId)
Calculates the inertial navigation solution.
bool areAccelerationsAveragedMeasurements() const
Wether the measurements are accumulated values over the last epoch. (always true when using delta mea...
std::optional< std::reference_wrapper< const State > > getLatestState() const
Get the latest state if it exists.
void addDeltaMeasurement(const InsTime &epoch, double dt, double deltaTime, const Eigen::Vector3d &p_deltaVelocity, const Eigen::Vector3d &p_deltaTheta, const char *nameId)
Adds the measurement to the tracking buffer.
IntegrationFrame
Available Integration Frames.
@ ECEF
Earth-Centered Earth-Fixed frame.
void setInitialState(const PosVelAtt &state, const char *nameId)
Sets the initial state.
void reset()
Clears all internal data.
IntegrationFrame _integrationFrame
Frame to integrate the observations in.
PosVelAttDerivativeConstants _posVelAttDerivativeConstants
Settings for the models to use.
IntegrationFrame getIntegrationFrame() const
Returns the selected integration frame.
Eigen::Vector3d _p_lastBiasAngularRate
Initial values for the angular rate bias [rad/s].
ScrollingBuffer< State > _states
List of states. Length depends on algorithm used.
bool _lockIntegrationFrame
Wether to lock the integration frame.
std::shared_ptr< PosVelAtt > lastStateAsPosVelAtt() const
Returns the last state as PosVelAtt.
const Eigen::Vector3d & p_getLastAngularRateBias() const
Return the last angular rate bias in platform frame coordinates in [rad/s].
void applySensorBiasesIncrements(const Eigen::Vector3d &p_deltaBiasAcceleration, const Eigen::Vector3d &p_deltaBiasAngularRate)
Sets the sensor biases increment.
GenericState< double > State
Inertial state and measurements.
const PosVelAttDerivativeConstants & getConstants() const
Returns the selected compensation models.
The class is responsible for all time-related tasks.
Definition InsTime.hpp:710
Position, Velocity and Attitude Storage Class.
Definition PosVelAtt.hpp:25
A buffer which is overwriting itself from the start when full.
Derived::PlainObject lerp(const Eigen::MatrixBase< Derived > &a, const Eigen::MatrixBase< Derived > &b, auto t)
Linear interpolation between vectors.
Definition Math.hpp:351
void to_json(json &j, const Node &node)
Converts the provided node into a json object.
Definition Node.cpp:990
const char * to_string(gui::widgets::PositionWithFrame::ReferenceFrame refFrame)
Converts the enum to a string.
bool InertialIntegratorGui(const char *label, InertialIntegrator &integrator, bool &preferAccelerationOverDeltaMeasurements, float width)
Shows a GUI for advanced configuration of the InertialIntegrator.
Y RungeKutta3(const Y &y_n, const std::array< Z, 3 > &z, const Scalar &h, const auto &f, const auto &constParam, const Scalar &t_n=0)
Runge-Kutta 3rd order (explicit) / Simpson's rule .
void from_json(const json &j, Node &node)
Converts the provided json object into a node object.
Definition Node.cpp:1007
Y RungeKutta1(const Y &y_n, const std::array< Z, 1 > &z, const Scalar &h, const auto &f, const auto &constParam, const Scalar &t_n=0)
Runge-Kutta 1st order (explicit) / (Forward) Euler method .
Eigen::Vector< T, 10 > n_calcPosVelAttDerivative(const Eigen::Vector< T, 10 > &y, const Eigen::Vector< T, 6 > &z, const PosVelAttDerivativeConstants &c, double=0.0)
Calculates the derivative of the quaternion, velocity and curvilinear position.
Y RungeKutta4(const Y &y_n, const std::array< Z, 4 > &z, const Scalar &h, const auto &f, const auto &constParam, const Scalar &t_n=0)
Runge-Kutta 4th order (explicit) .
Eigen::Vector< T, 10 > e_calcPosVelAttDerivative(const Eigen::Vector< T, 10 > &y, const Eigen::Vector< T, 6 > &z, const PosVelAttDerivativeConstants &c, double=0.0)
Calculates the derivative of the quaternion, velocity and position in ECEF coordinates.
Y RungeKutta2(const Y &y_n, const std::array< Z, 2 > &z, const Scalar &h, const auto &f, const auto &constParam, const Scalar &t_n=0)
Runge-Kutta 2nd order (explicit) / Explicit midpoint method .
Y Heun2(const Y &y_n, const std::array< Z, 2 > &z, const Scalar &h, const auto &f, const auto &constParam, const Scalar &t_n=0)
Heun's method (2nd order) (explicit) .
Inertial state and measurements.
bool averagedMeasurement
Wether the acceleration is averaged over the last epoch.
Eigen::Vector3d p_angularRate
Angular rate in platform frame coordinates in [rad/s].
Eigen::Vector3d p_acceleration
Acceleration in platform frame coordinates in [m/s^2].
double dt
Time since previous observation in [s].
Values needed to calculate the PosVelAttDerivative for the local-navigation frame.