INSTINCT Code Coverage Report

Directory: src/
File: Nodes/DataProcessor/KalmanFilter/LooselyCoupledKF.hpp
Date: 2025-06-02 15:19:59
Exec Total Coverage
Lines: 15 71 21.1%
Functions: 0 2 0.0%
Branches: 15 111 13.5%
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 LooselyCoupledKF.hpp
10 /// @brief Kalman Filter class for the loosely coupled INS/GNSS integration
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @author M. Maier (marcel.maier@ins.uni-stuttgart.de)
13 /// @date 2021-08-04
14
15 #pragma once
16
17 #include <cstdint>
18 #include "internal/Node/Node.hpp"
19 #include "internal/gui/widgets/DynamicInputPins.hpp"
20
21 #include "Navigation/INS/Units.hpp"
22 #include "Navigation/Time/InsTime.hpp"
23 #include "Navigation/INS/InertialIntegrator.hpp"
24
25 #include "NodeData/IMU/ImuObs.hpp"
26 #include "NodeData/State/PosVelAtt.hpp"
27 #include "NodeData/Baro/BaroHgt.hpp"
28
29 #include "Navigation/Math/KeyedKalmanFilter.hpp"
30
31 namespace NAV
32 {
33 /// @brief Loosely-coupled Kalman Filter for INS/GNSS integration
34 class LooselyCoupledKF : public Node
35 {
36 public:
37 /// @brief Default constructor
38 LooselyCoupledKF();
39 /// @brief Destructor
40 ~LooselyCoupledKF() override;
41 /// @brief Copy constructor
42 LooselyCoupledKF(const LooselyCoupledKF&) = delete;
43 /// @brief Move constructor
44 LooselyCoupledKF(LooselyCoupledKF&&) = delete;
45 /// @brief Copy assignment operator
46 LooselyCoupledKF& operator=(const LooselyCoupledKF&) = delete;
47 /// @brief Move assignment operator
48 LooselyCoupledKF& operator=(LooselyCoupledKF&&) = delete;
49 /// @brief String representation of the class type
50 [[nodiscard]] static std::string typeStatic();
51
52 /// @brief String representation of the class type
53 [[nodiscard]] std::string type() const override;
54
55 /// @brief String representation of the class category
56 [[nodiscard]] static std::string category();
57
58 /// @brief ImGui config window which is shown on double click
59 /// @attention Don't forget to set _hasConfig to true in the constructor of the node
60 void guiConfig() override;
61
62 /// @brief Saves the node into a json object
63 [[nodiscard]] json save() const override;
64
65 /// @brief Restores the node from a json object
66 /// @param[in] j Json object with the node state
67 void restore(const json& j) override;
68
69 /// @brief State Keys of the Kalman filter
70 enum KFStates : uint8_t
71 {
72 Roll = 0, ///< Roll
73 Pitch = 1, ///< Pitch
74 Yaw = 2, ///< Yaw
75 VelN = 3, ///< Velocity North
76 VelE = 4, ///< Velocity East
77 VelD = 5, ///< Velocity Down
78 PosLat = 6, ///< Latitude
79 PosLon = 7, ///< Longitude
80 PosAlt = 8, ///< Altitude
81 AccBiasX = 9, ///< Accelerometer Bias X
82 AccBiasY = 10, ///< Accelerometer Bias Y
83 AccBiasZ = 11, ///< Accelerometer Bias Z
84 GyrBiasX = 12, ///< Gyroscope Bias X
85 GyrBiasY = 13, ///< Gyroscope Bias Y
86 GyrBiasZ = 14, ///< Gyroscope Bias Z
87 HeightBias = 15, ///< Baro Height Bias
88 HeightScale = 16, ///< Baro Height Scale
89 KFStates_COUNT = 17, ///< Amount of states
90
91 Psi_eb_1 = Roll, ///< Angle between Earth and Body frame around 1. axis
92 Psi_eb_2 = Pitch, ///< Angle between Earth and Body frame around 2. axis
93 Psi_eb_3 = Yaw, ///< Angle between Earth and Body frame around 3. axis
94 VelX = VelN, ///< ECEF Velocity X
95 VelY = VelE, ///< ECEF Velocity Y
96 VelZ = VelD, ///< ECEF Velocity Z
97 PosX = PosLat, ///< ECEF Position X
98 PosY = PosLon, ///< ECEF Position Y
99 PosZ = PosAlt, ///< ECEF Position Z
100 };
101
102 /// @brief Measurement Keys of the Kalman filter
103 enum KFMeas : uint8_t
104 {
105 dPosLat = 0, ///< Latitude difference
106 dPosLon = 1, ///< Longitude difference
107 dPosAlt = 2, ///< Altitude difference
108 dVelN = 3, ///< Velocity North difference
109 dVelE = 4, ///< Velocity East difference
110 dVelD = 5, ///< Velocity Down difference
111 dHgt = 6, ///< height difference
112
113 dPosX = dPosLat, ///< ECEF Position X difference
114 dPosY = dPosLon, ///< ECEF Position Y difference
115 dPosZ = dPosAlt, ///< ECEF Position Z difference
116 dVelX = dVelN, ///< ECEF Velocity X difference
117 dVelY = dVelE, ///< ECEF Velocity Y difference
118 dVelZ = dVelD, ///< ECEF Velocity Z difference
119 };
120
121 private:
122 constexpr static size_t INPUT_PORT_INDEX_IMU = 0; ///< @brief Flow (ImuObs)
123 constexpr static size_t INPUT_PORT_INDEX_POS_VEL_ATT_INIT = 1; ///< @brief Flow (PosVelAtt)
124 constexpr static size_t OUTPUT_PORT_INDEX_SOLUTION = 0; ///< @brief Flow (InsGnssLCKFSolution)
125
126 /// @brief Initialize the node
127 bool initialize() override;
128
129 /// @brief Deinitialize the node
130 void deinitialize() override;
131
132 /// @brief Checks wether there is an input pin with the same time
133 /// @param[in] insTime Time to check
134 bool hasInputPinWithSameTime(const InsTime& insTime) const;
135
136 /// @brief Invoke the callback with a PosVelAtt solution (without LCKF specific output)
137 /// @param[in] posVelAtt PosVelAtt solution
138 void invokeCallbackWithPosVelAtt(const PosVelAtt& posVelAtt);
139
140 /// @brief Receive Function for the IMU observation
141 /// @param[in] queue Queue with all the received data messages
142 /// @param[in] pinIdx Index of the pin the data is received on
143 void recvImuObservation(InputPin::NodeDataQueue& queue, size_t pinIdx);
144
145 /// @brief Receive Function for the PosVel observation
146 /// @param[in] queue Queue with all the received data messages
147 /// @param[in] pinIdx Index of the pin the data is received on
148 void recvPosVelObservation(InputPin::NodeDataQueue& queue, size_t pinIdx);
149
150 /// @brief Receive Function for the PosVelAtt observation
151 /// @param[in] queue Queue with all the received data messages
152 /// @param[in] pinIdx Index of the pin the data is received on
153 void recvPosVelAttInit(InputPin::NodeDataQueue& queue, size_t pinIdx);
154
155 /// @brief Receive Function for the BaroHgt observation
156 /// @param[in] queue Queue with all the received data messages
157 /// @param[in] pinIdx Index of the pin the data is received on
158 void recvBaroHeight(InputPin::NodeDataQueue& queue, size_t pinIdx);
159
160 /// @brief Predicts the state from the InertialNavSol
161 /// @param[in] inertialNavSol Inertial navigation solution triggering the prediction
162 /// @param[in] tau_i Time since the last prediction in [s]
163 /// @param[in] imuPos IMU platform frame position with respect to body frame
164 void looselyCoupledPrediction(const std::shared_ptr<const PosVelAtt>& inertialNavSol, double tau_i, const ImuPos& imuPos);
165
166 /// @brief Updates the predicted state from the InertialNavSol with the PosVel observation
167 /// @param[in] posVelObs PosVel measurement triggering the update
168 void looselyCoupledUpdate(const std::shared_ptr<const PosVel>& posVelObs);
169
170 /// @brief Updates the predicted state from the InertialNavSol with the Baro observation
171 /// @param[in] baroHgtObs Barometric height measurement triggering the update
172 void looselyCoupledUpdate(const std::shared_ptr<const BaroHgt>& baroHgtObs);
173
174 /// @brief Sets the covariance matrix P of the LCKF (and does the necessary unit conversions)
175 /// @param[in] lckfSolution LCKF solution from prediction or update
176 /// @param[in] R_N Prime vertical radius of curvature (East/West) [m]
177 /// @param[in] R_E Meridian radius of curvature in [m]
178 void setSolutionPosVelAttAndCov(const std::shared_ptr<PosVelAtt>& lckfSolution, double R_N, double R_E);
179
180 /// @brief Inertial Integrator
181 InertialIntegrator _inertialIntegrator;
182 /// Prefer the raw acceleration measurements over the deltaVel & deltaTheta values
183 bool _preferAccelerationOverDeltaMeasurements = false;
184
185 /// Last received IMU observation (to get ImuPos)
186 std::shared_ptr<const ImuObs> _lastImuObs = nullptr;
187
188 /// Accumulator for height bias [m]
189 double _heightBiasTotal = 0.0;
190 /// Accumulator for height scale [m/m]
191 double _heightScaleTotal = 1.0;
192
193 /// Accumulator for acceleration bias [m/s²]
194 Eigen::Vector3d _accelBiasTotal = Eigen::Vector3d::Zero();
195 /// Accumulator gyro bias [rad/s]
196 Eigen::Vector3d _gyroBiasTotal = Eigen::Vector3d::Zero();
197
198 /// Roll, Pitch and Yaw angles in [deg] used for initialization if not taken from separate pin
199 std::array<double, 3> _initalRollPitchYaw{};
200 /// Whether to initialize the state over an external pin
201 bool _initializeStateOverExternalPin{};
202
203 /// Whether to enable barometric height (including the corresponding pin)
204 bool _enableBaroHgt{};
205
206 /// Time from the external init
207 InsTime _externalInitTime;
208
209 /// Whether the accumulated biases have been initialized in the 'inertialIntegrator'
210 bool _initialSensorBiasesApplied = false;
211
212 /// @brief Vector with all state keys
213 inline static const std::vector<KFStates> States = { KFStates::Roll, KFStates::Pitch, KFStates::Yaw,
214 KFStates::VelN, KFStates::VelE, KFStates::VelD,
215 KFStates::PosLat, KFStates::PosLon, KFStates::PosAlt,
216 KFStates::AccBiasX, KFStates::AccBiasY, KFStates::AccBiasZ,
217 KFStates::GyrBiasX, KFStates::GyrBiasY, KFStates::GyrBiasZ,
218 KFStates::HeightBias, KFStates::HeightScale };
219 /// @brief All position keys
220 inline static const std::vector<KFStates> KFPos = { KFStates::PosLat, KFStates::PosLon, KFStates::PosAlt };
221 /// @brief All velocity keys
222 inline static const std::vector<KFStates> KFVel = { KFStates::VelN, KFStates::VelE, KFStates::VelD };
223 /// @brief All attitude keys
224 inline static const std::vector<KFStates> KFAtt = { KFStates::Roll, KFStates::Pitch, KFStates::Yaw };
225 /// @brief All acceleration bias keys
226 inline static const std::vector<KFStates> KFAccBias = { KFStates::AccBiasX, KFStates::AccBiasY, KFStates::AccBiasZ };
227 /// @brief All gyroscope bias keys
228 inline static const std::vector<KFStates> KFGyrBias = { KFStates::GyrBiasX, KFStates::GyrBiasY, KFStates::GyrBiasZ };
229
230 /// @brief All position and velocity keys
231 inline static const std::vector<KFStates> KFPosVel = { KFStates::PosLat, KFStates::PosLon, KFStates::PosAlt,
232 KFStates::VelN, KFStates::VelE, KFStates::VelD };
233 /// @brief All position, velocity and attitude keys
234 inline static const std::vector<KFStates> KFPosVelAtt = { KFStates::PosLat, KFStates::PosLon, KFStates::PosAlt,
235 KFStates::VelN, KFStates::VelE, KFStates::VelD,
236 KFStates::Roll, KFStates::Pitch, KFStates::Yaw };
237
238 /// @brief Vector with all measurement keys
239 inline static const std::vector<KFMeas> Meas = { KFMeas::dPosLat, KFMeas::dPosLon, KFMeas::dPosAlt, KFMeas::dVelN, KFMeas::dVelE, KFMeas::dVelD };
240 /// @brief All position difference keys
241 inline static const std::vector<KFMeas> dPos = { KFMeas::dPosLat, KFMeas::dPosLon, KFMeas::dPosAlt };
242 /// @brief All velocity difference keys
243 inline static const std::vector<KFMeas> dVel = { KFMeas::dVelN, KFMeas::dVelE, KFMeas::dVelD };
244
245 /// Kalman Filter representation
246 KeyedKalmanFilterD<KFStates, KFMeas> _kalmanFilter{ States, Meas };
247
248 // #########################################################################################################################################
249 // GUI settings
250 // #########################################################################################################################################
251
252 /// @brief Check the rank of the Kalman matrices every iteration (computational expensive)
253 bool _checkKalmanMatricesRanks = true;
254
255 // ###########################################################################################################
256 // Parameters
257 // ###########################################################################################################
258
259 /// Gui selection for the Unit of the input stdev_ra parameter
260 Units::ImuAccelerometerFilterNoiseUnits _stdevAccelNoiseUnits = Units::ImuAccelerometerFilterNoiseUnits::mg_sqrtHz;
261
262 /// @brief 𝜎_ra Standard deviation of the noise on the accelerometer specific-force measurements
263 /// @note Value from VN-310 Datasheet but verify with values from Brown (2012) table 9.3 for 'High quality'
264
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 Eigen::Vector3d _stdev_ra = 0.04 /* [mg/√(Hz)] */ * Eigen::Vector3d::Ones();
265
266 // ###########################################################################################################
267
268 /// Gui selection for the Unit of the input stdev_rg parameter
269 Units::ImuGyroscopeFilterNoiseUnits _stdevGyroNoiseUnits = Units::ImuGyroscopeFilterNoiseUnits::deg_hr_sqrtHz;
270
271 /// @brief 𝜎_rg Standard deviation of the noise on the gyro angular-rate measurements
272 /// @note Value from VN-310 Datasheet but verify with values from Brown (2012) table 9.3 for 'High quality'
273
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 Eigen::Vector3d _stdev_rg = 5 /* [deg/hr/√(Hz)]^2 */ * Eigen::Vector3d::Ones();
274
275 // ###########################################################################################################
276
277 /// Gui selection for the Unit of the input variance_bad parameter
278 Units::ImuAccelerometerFilterBiasUnits _stdevAccelBiasUnits = Units::ImuAccelerometerFilterBiasUnits::microg;
279
280 /// @brief 𝜎²_bad Variance of the accelerometer dynamic bias
281 /// @note Value from VN-310 Datasheet (In-Run Bias Stability (Allan Variance))
282
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 Eigen::Vector3d _stdev_bad = 10 /* [µg] */ * Eigen::Vector3d::Ones();
283
284 /// @brief Correlation length of the accelerometer dynamic bias in [s]
285
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 Eigen::Vector3d _tau_bad = 0.1 * Eigen::Vector3d::Ones();
286
287 // ###########################################################################################################
288
289 /// Gui selection for the Unit of the input variance_bad parameter
290 Units::ImuGyroscopeFilterBiasUnits _stdevGyroBiasUnits = Units::ImuGyroscopeFilterBiasUnits::deg_h;
291
292 /// @brief 𝜎²_bgd Variance of the gyro dynamic bias
293 /// @note Value from VN-310 Datasheet (In-Run Bias Stability (Allan Variance))
294
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 Eigen::Vector3d _stdev_bgd = 1 /* [°/h] */ * Eigen::Vector3d::Ones();
295
296 /// @brief Correlation length of the gyro dynamic bias in [s]
297
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 Eigen::Vector3d _tau_bgd = 0.1 * Eigen::Vector3d::Ones();
298
299 // ###########################################################################################################
300
301 /// Possible Units for the Variance of the barometric height bias
302 enum class StdevBaroHeightBiasUnits : uint8_t
303 {
304 m, ///< [m]
305 };
306 /// Gui selection for the Unit of the barometric height bias uncertainty
307 StdevBaroHeightBiasUnits _stdevBaroHeightBiasUnits = StdevBaroHeightBiasUnits::m;
308
309 /// Uncertainty of the barometric height bias
310 double _stdevBaroHeightBias = 1e-6;
311
312 // ###########################################################################################################
313
314 /// Possible Units for the Variance of the barometric height scale
315 enum class StdevBaroHeightScaleUnits : uint8_t
316 {
317 m_m, ///< [m/m]
318 };
319 /// Gui selection for the Unit of the barometric height scale uncertainty
320 StdevBaroHeightScaleUnits _stdevBaroHeightScaleUnits = StdevBaroHeightScaleUnits::m_m;
321
322 /// Uncertainty of the barometric height scale
323 double _stdevBaroHeightScale = 1e-8;
324
325 // ###########################################################################################################
326
327 /// @brief Available Random processes
328 enum class RandomProcess : uint8_t
329 {
330 // WhiteNoise, ///< White noise
331 // RandomConstant, ///< Random constant
332
333 RandomWalk, ///< Random Walk
334 GaussMarkov1, ///< Gauss-Markov 1st Order
335
336 // GaussMarkov2, ///< Gauss-Markov 2nd Order
337 // GaussMarkov3, ///< Gauss-Markov 3rd Order
338 };
339
340 /// @brief Random Process used to estimate the accelerometer biases
341 RandomProcess _randomProcessAccel = RandomProcess::RandomWalk;
342 /// @brief Random Process used to estimate the gyroscope biases
343 RandomProcess _randomProcessGyro = RandomProcess::RandomWalk;
344
345 // ###########################################################################################################
346
347 /// Possible Units for the GNSS measurement uncertainty for the position (standard deviation σ or Variance σ²)
348 enum class GnssMeasurementUncertaintyPositionUnit : uint8_t
349 {
350 meter2, ///< Variance [m^2, m^2, m^2]
351 meter, ///< Standard deviation [m, m, m]
352 };
353 /// Gui selection for the Unit of the GNSS measurement uncertainty for the position
354 GnssMeasurementUncertaintyPositionUnit _gnssMeasurementUncertaintyPositionUnit = GnssMeasurementUncertaintyPositionUnit::meter;
355
356 /// @brief GUI selection of the GNSS position measurement uncertainty (standard deviation σ or Variance σ²).
357 /// SPP accuracy approx. 3m in horizontal direction and 3 times worse in vertical direction
358
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 Eigen::Vector3d _gnssMeasurementUncertaintyPosition{ 0.3, 0.3, 0.3 * 3 };
359
360 /// Whether to override the position uncertainty or use the one included in the measurement
361 bool _gnssMeasurementUncertaintyPositionOverride = false;
362
363 // ###########################################################################################################
364
365 /// Possible Units for the GNSS measurement uncertainty for the velocity (standard deviation σ or Variance σ²)
366 enum class GnssMeasurementUncertaintyVelocityUnit : uint8_t
367 {
368 m2_s2, ///< Variance [m^2/s^2]
369 m_s, ///< Standard deviation [m/s]
370 };
371 /// Gui selection for the Unit of the GNSS measurement uncertainty for the velocity
372 GnssMeasurementUncertaintyVelocityUnit _gnssMeasurementUncertaintyVelocityUnit = GnssMeasurementUncertaintyVelocityUnit::m_s;
373
374 /// GUI selection of the GNSS NED velocity measurement uncertainty (standard deviation σ or Variance σ²)
375
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 Eigen::Vector3d _gnssMeasurementUncertaintyVelocity{ 0.5, 0.5, 0.5 };
376
377 /// Whether to override the velocity uncertainty or use the one included in the measurement
378 bool _gnssMeasurementUncertaintyVelocityOverride = false;
379
380 // ###########################################################################################################
381
382 /// Possible Units for the barometric height measurement uncertainty (standard deviation σ or Variance σ²)
383 enum class BaroHeightMeasurementUncertaintyUnit : uint8_t
384 {
385 m2, ///< Variance [m²]
386 m, ///< Standard deviation [m]
387 };
388 /// Gui selection for the Unit of the barometric height measurement uncertainty
389 BaroHeightMeasurementUncertaintyUnit _barometricHeightMeasurementUncertaintyUnit = BaroHeightMeasurementUncertaintyUnit::m;
390
391 /// GUI selection of the barometric height measurement uncertainty (standard deviation σ or Variance σ²)
392 double _barometricHeightMeasurementUncertainty = 1.0;
393
394 /// Whether to override the barometric height uncertainty or use the one included in the measurement
395 bool _baroHeightMeasurementUncertaintyOverride = false;
396
397 // ###########################################################################################################
398
399 /// Possible Units for the initial covariance for the position (standard deviation σ or Variance σ²)
400 enum class InitCovariancePositionUnit : uint8_t
401 {
402 rad2_rad2_m2, ///< Variance LatLonAlt^2 [rad^2, rad^2, m^2]
403 rad_rad_m, ///< Standard deviation LatLonAlt [rad, rad, m]
404 meter2, ///< Variance NED [m^2, m^2, m^2]
405 meter, ///< Standard deviation NED [m, m, m]
406 };
407 /// Gui selection for the Unit of the initial covariance for the position
408 InitCovariancePositionUnit _initCovariancePositionUnit = InitCovariancePositionUnit::meter;
409
410 /// GUI selection of the initial covariance diagonal values for position (standard deviation σ or Variance σ²)
411
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 Eigen::Vector3d _initCovariancePosition{ 100.0, 100.0, 100.0 };
412
413 // ###########################################################################################################
414
415 /// Possible Units for the initial covariance for the velocity (standard deviation σ or Variance σ²)
416 enum class InitCovarianceVelocityUnit : uint8_t
417 {
418 m2_s2, ///< Variance [m^2/s^2]
419 m_s, ///< Standard deviation [m/s]
420 };
421 /// Gui selection for the Unit of the initial covariance for the velocity
422 InitCovarianceVelocityUnit _initCovarianceVelocityUnit = InitCovarianceVelocityUnit::m_s;
423
424 /// GUI selection of the initial covariance diagonal values for velocity (standard deviation σ or Variance σ²)
425
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 Eigen::Vector3d _initCovarianceVelocity{ 10.0, 10.0, 10.0 };
426
427 // ###########################################################################################################
428
429 /// Possible Units for the initial covariance for the attitude angles (standard deviation σ or Variance σ²)
430 enum class InitCovarianceAttitudeAnglesUnit : uint8_t
431 {
432 rad2, ///< Variance [rad^2]
433 deg2, ///< Variance [deg^2]
434 rad, ///< Standard deviation [rad]
435 deg, ///< Standard deviation [deg]
436 };
437 /// Gui selection for the Unit of the initial covariance for the attitude angles
438 InitCovarianceAttitudeAnglesUnit _initCovarianceAttitudeAnglesUnit = InitCovarianceAttitudeAnglesUnit::deg;
439
440 /// GUI selection of the initial covariance diagonal values for attitude angles (standard deviation σ or Variance σ²)
441
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 Eigen::Vector3d _initCovarianceAttitudeAngles{ 10.0, 10.0, 10.0 };
442
443 // ###########################################################################################################
444
445 /// Possible Units for the initial covariance for the accelerometer biases (standard deviation σ or Variance σ²)
446 enum class InitCovarianceBiasAccelUnit : uint8_t
447 {
448 m2_s4, ///< Variance [m^2/s^4]
449 m_s2, ///< Standard deviation [m/s^2]
450 };
451 /// Gui selection for the Unit of the initial covariance for the accelerometer biases
452 InitCovarianceBiasAccelUnit _initCovarianceBiasAccelUnit = InitCovarianceBiasAccelUnit::m_s2;
453
454 /// GUI selection of the initial covariance diagonal values for accelerometer biases (standard deviation σ or Variance σ²)
455
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 Eigen::Vector3d _initCovarianceBiasAccel{ 1.0, 1.0, 1.0 };
456
457 // ###########################################################################################################
458
459 /// Possible Units for the initial covariance for the gyroscope biases (standard deviation σ or Variance σ²)
460 enum class InitCovarianceBiasGyroUnit : uint8_t
461 {
462 rad2_s2, ///< Variance [rad²/s²]
463 deg2_s2, ///< Variance [deg²/s²]
464 rad_s, ///< Standard deviation [rad/s]
465 deg_s, ///< Standard deviation [deg/s]
466 };
467 /// Gui selection for the Unit of the initial covariance for the gyroscope biases
468 InitCovarianceBiasGyroUnit _initCovarianceBiasGyroUnit = InitCovarianceBiasGyroUnit::deg_s;
469
470 /// GUI selection of the initial covariance diagonal values for gyroscope biases (standard deviation σ or Variance σ²)
471
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 Eigen::Vector3d _initCovarianceBiasGyro{ 0.5, 0.5, 0.5 };
472
473 // ###########################################################################################################
474
475 /// Possible Units for the initial covariance for the height bias (standard deviation σ or Variance σ²)
476 enum class InitCovarianceBiasHeightUnit : uint8_t
477 {
478 m2, ///< Variance [m²]
479 m, ///< Standard deviation [m]
480 };
481 /// Gui selection for the Unit of the initial covariance for the height bias
482 InitCovarianceBiasHeightUnit _initCovarianceBiasHeightUnit = InitCovarianceBiasHeightUnit::m2;
483
484 /// GUI selection of the initial covariance diagonal values for the height bias (standard deviation σ or Variance σ²)
485 double _initCovarianceBiasHeight{ 100.0 };
486
487 // ###########################################################################################################
488
489 /// Possible Units for the initial covariance for the height scale (standard deviation σ or Variance σ²)
490 enum class InitCovarianceScaleHeight : uint8_t
491 {
492 m2_m2, ///< Variance [m²/m²]
493 m_m, ///< Standard devation [m/m]
494 };
495 /// Gui selection for the Unit of the initial covariance for the height scale
496 InitCovarianceScaleHeight _initCovarianceScaleHeightUnit = InitCovarianceScaleHeight::m2_m2;
497
498 /// GUI selection of the initial covariance diagonal values for the height scale (standard deviation σ or Variance σ²)
499 double _initCovarianceScaleHeight{ 0.01 };
500
501 // ###########################################################################################################
502
503 /// Possible Units for the initial accelerometer biases
504 enum class InitBiasAccelUnit : uint8_t
505 {
506 microg, ///< [µg]
507 m_s2, ///< acceleration [m/s^2]
508 };
509 /// Gui selection for the unit of the initial accelerometer biases
510 InitBiasAccelUnit _initBiasAccelUnit = InitBiasAccelUnit::m_s2;
511
512 /// GUI selection of the initial accelerometer biases
513
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 Eigen::Vector3d _initBiasAccel{ 0.0, 0.0, 0.0 };
514
515 // ###########################################################################################################
516
517 /// Possible Units for the initial gyroscope biases
518 enum class InitBiasGyroUnit : uint8_t
519 {
520 rad_s, ///< angular rate [rad/s]
521 deg_s, ///< angular rate [deg/s]
522 };
523 /// Gui selection for the unit of the initial gyroscope biases
524 InitBiasGyroUnit _initBiasGyroUnit = InitBiasGyroUnit::deg_s;
525
526 /// GUI selection of the initial gyroscope biases
527
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 Eigen::Vector3d _initBiasGyro{ 0.0, 0.0, 0.0 };
528
529 // ###########################################################################################################
530
531 /// GUI option for the Phi calculation algorithm
532 enum class PhiCalculationAlgorithm : uint8_t
533 {
534 Exponential, ///< Van-Loan
535 Taylor, ///< Taylor
536 };
537 /// GUI option for the Phi calculation algorithm
538 PhiCalculationAlgorithm _phiCalculationAlgorithm = PhiCalculationAlgorithm::Taylor;
539
540 /// GUI option for the order of the Taylor polynom to calculate the Phi matrix
541 int _phiCalculationTaylorOrder = 2;
542
543 /// GUI option for the Q calculation algorithm
544 enum class QCalculationAlgorithm : uint8_t
545 {
546 VanLoan, ///< Van-Loan
547 Taylor1, ///< Taylor
548 };
549 /// GUI option for the Q calculation algorithm
550 QCalculationAlgorithm _qCalculationAlgorithm = QCalculationAlgorithm::Taylor1;
551
552 // ###########################################################################################################
553 // Dynamic Input Pins
554 // ###########################################################################################################
555 //
556 /// @brief Function to call to add a new pin
557 /// @param[in, out] node Pointer to this node
558 static void pinAddCallback(Node* node);
559 /// @brief Function to call to delete a pin
560 /// @param[in, out] node Pointer to this node
561 /// @param[in] pinIdx Input pin index to delete
562 static void pinDeleteCallback(Node* node, size_t pinIdx);
563
564 /// Update the input pins depending on the GUI
565 void updateInputPins();
566
567 /// @brief Dynamic input pins
568 /// @attention This should always be the last variable in the header, because it accesses others through the function callbacks
569 gui::widgets::DynamicInputPins _dynamicInputPins{ 1, this, pinAddCallback, pinDeleteCallback };
570
571 // ###########################################################################################################
572 // Prediction
573 // ###########################################################################################################
574
575 // ###########################################################################################################
576 // System matrix 𝐅
577 // ###########################################################################################################
578
579 /// @brief Calculates the system matrix 𝐅 for the local navigation frame
580 /// @param[in] n_Quat_b Attitude of the body with respect to n-system
581 /// @param[in] b_specForce_ib Specific force of the body with respect to inertial frame in [m / s^2], resolved in body coordinates
582 /// @param[in] n_omega_in Angular rate of navigation system with respect to the inertial system [rad / s], resolved in navigation coordinates.
583 /// @param[in] n_velocity Velocity in n-system in [m / s]
584 /// @param[in] lla_position Position as Lat Lon Alt in [rad rad m]
585 /// @param[in] R_N Meridian radius of curvature in [m]
586 /// @param[in] R_E Prime vertical radius of curvature (East/West) [m]
587 /// @param[in] g_0 Magnitude of the gravity vector in [m/s^2] (see \cite Groves2013 Groves, ch. 2.4.7, eq. 2.135, p. 70)
588 /// @param[in] r_eS_e Geocentric radius. The distance of a point on the Earth's surface from the center of the Earth in [m]
589 /// @param[in] tau_bad Correlation length for the accelerometer in [s]
590 /// @param[in] tau_bgd Correlation length for the gyroscope in [s]
591 /// @note See Groves (2013) chapter 14.2.4, equation (14.63)
592 [[nodiscard]] KeyedMatrix<double, KFStates, KFStates, 17, 17> n_systemMatrix_F(const Eigen::Quaterniond& n_Quat_b,
593 const Eigen::Vector3d& b_specForce_ib,
594 const Eigen::Vector3d& n_omega_in,
595 const Eigen::Vector3d& n_velocity,
596 const Eigen::Vector3d& lla_position,
597 double R_N,
598 double R_E,
599 double g_0,
600 double r_eS_e,
601 const Eigen::Vector3d& tau_bad,
602 const Eigen::Vector3d& tau_bgd) const;
603
604 /// @brief Calculates the system matrix 𝐅 for the ECEF frame
605 /// @param[in] e_Quat_b Attitude of the body with respect to e-system
606 /// @param[in] b_specForce_ib Specific force of the body with respect to inertial frame in [m / s^2], resolved in body coordinates
607 /// @param[in] e_position Position in ECEF coordinates in [m]
608 /// @param[in] e_gravitation Gravitational acceleration in [m/s^2]
609 /// @param[in] r_eS_e Geocentric radius. The distance of a point on the Earth's surface from the center of the Earth in [m]
610 /// @param[in] e_omega_ie Angular velocity of Earth with respect to inertial system, represented in e-sys in [rad/s]
611 /// @param[in] tau_bad Correlation length for the accelerometer in [s]
612 /// @param[in] tau_bgd Correlation length for the gyroscope in [s]
613 /// @note See Groves (2013) chapter 14.2.3, equation (14.48)
614 [[nodiscard]] KeyedMatrix<double, KFStates, KFStates, 17, 17> e_systemMatrix_F(const Eigen::Quaterniond& e_Quat_b,
615 const Eigen::Vector3d& b_specForce_ib,
616 const Eigen::Vector3d& e_position,
617 const Eigen::Vector3d& e_gravitation,
618 double r_eS_e,
619 const Eigen::Vector3d& e_omega_ie,
620 const Eigen::Vector3d& tau_bad,
621 const Eigen::Vector3d& tau_bgd) const;
622
623 // ###########################################################################################################
624 // Noise input matrix 𝐆 & Noise scale matrix 𝐖
625 // System noise covariance matrix 𝐐
626 // ###########################################################################################################
627
628 /// @brief Calculates the noise input matrix 𝐆
629 /// @param[in] ien_Quat_b Quaternion from body frame to {i,e,n} frame
630 /// @note See \cite Groves2013 Groves, ch. 14.2.6, eq. 14.79, p. 590
631 [[nodiscard]] static KeyedMatrix<double, KFStates, KFStates, 17, 17> noiseInputMatrix_G(const Eigen::Quaterniond& ien_Quat_b);
632
633 /// @brief Calculates the noise scale matrix 𝐖
634 /// @param[in] sigma_ra Standard deviation of the noise on the accelerometer specific-force measurements
635 /// @param[in] sigma_rg Standard deviation of the noise on the gyro angular-rate measurements
636 /// @param[in] sigma_bad Standard deviation of the accelerometer dynamic bias
637 /// @param[in] sigma_bgd Standard deviation of the gyro dynamic bias
638 /// @param[in] tau_bad Correlation length for the accelerometer in [s]
639 /// @param[in] tau_bgd Correlation length for the gyroscope in [s]
640 /// @param[in] sigma_heightBias Standard deviation of the height bias
641 /// @param[in] sigma_heightScale Standard deviation of the height scale
642 /// @note See \cite Groves2013 Groves, ch. 14.2.6, eq. 14.79, p. 590
643 [[nodiscard]] Eigen::Matrix<double, 17, 17> noiseScaleMatrix_W(const Eigen::Vector3d& sigma_ra, const Eigen::Vector3d& sigma_rg,
644 const Eigen::Vector3d& sigma_bad, const Eigen::Vector3d& sigma_bgd,
645 const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd,
646 const double& sigma_heightBias, const double& sigma_heightScale);
647
648 /// @brief System noise covariance matrix 𝐐_{k-1}
649 /// @param[in] sigma2_ra Variance of the noise on the accelerometer specific-force measurements
650 /// @param[in] sigma2_rg Variance of the noise on the gyro angular-rate measurements
651 /// @param[in] sigma2_bad Variance of the accelerometer dynamic bias
652 /// @param[in] sigma2_bgd Variance of the gyro dynamic bias
653 /// @param[in] sigma_heightBias Standard deviation of the height bias
654 /// @param[in] sigma_heightScale Standard deviation of the height scale
655 /// @param[in] tau_bad Correlation length for the accelerometer in [s]
656 /// @param[in] tau_bgd Correlation length for the gyroscope in [s]
657 /// @param[in] n_F_21 Submatrix 𝐅_21 of the system matrix 𝐅
658 /// @param[in] T_rn_p Conversion matrix between cartesian and curvilinear perturbations to the position
659 /// @param[in] n_Dcm_b Direction Cosine Matrix from body to navigation coordinates
660 /// @param[in] tau_s Time interval in [s]
661 /// @return The 17x17 matrix of system noise covariances
662 [[nodiscard]] static KeyedMatrix<double, KFStates, KFStates, 17, 17> n_systemNoiseCovarianceMatrix_Q(const Eigen::Vector3d& sigma2_ra, const Eigen::Vector3d& sigma2_rg,
663 const Eigen::Vector3d& sigma2_bad, const Eigen::Vector3d& sigma2_bgd,
664 const double& sigma_heightBias, const double& sigma_heightScale,
665 const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd,
666 const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p,
667 const Eigen::Matrix3d& n_Dcm_b, const double& tau_s);
668
669 /// @brief System noise covariance matrix 𝐐_{k-1}
670 /// @param[in] sigma2_ra Variance of the noise on the accelerometer specific-force measurements
671 /// @param[in] sigma2_rg Variance of the noise on the gyro angular-rate measurements
672 /// @param[in] sigma2_bad Variance of the accelerometer dynamic bias
673 /// @param[in] sigma2_bgd Variance of the gyro dynamic bias
674 /// @param[in] sigma_heightBias Standard deviation of the height bias
675 /// @param[in] sigma_heightScale Standard deviation of the height scale
676 /// @param[in] tau_bad Correlation length for the accelerometer in [s]
677 /// @param[in] tau_bgd Correlation length for the gyroscope in [s]
678 /// @param[in] e_F_21 Submatrix 𝐅_21 of the system matrix 𝐅
679 /// @param[in] e_Dcm_b Direction Cosine Matrix from body to Earth coordinates
680 /// @param[in] tau_s Time interval in [s]
681 /// @return The 17x17 matrix of system noise covariances
682 [[nodiscard]] static KeyedMatrix<double, KFStates, KFStates, 17, 17> e_systemNoiseCovarianceMatrix_Q(const Eigen::Vector3d& sigma2_ra, const Eigen::Vector3d& sigma2_rg,
683 const Eigen::Vector3d& sigma2_bad, const Eigen::Vector3d& sigma2_bgd,
684 const double& sigma_heightBias, const double& sigma_heightScale,
685 const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd,
686 const Eigen::Matrix3d& e_F_21,
687 const Eigen::Matrix3d& e_Dcm_b, const double& tau_s);
688
689 // ###########################################################################################################
690 // Error covariance matrix P
691 // ###########################################################################################################
692
693 /// @brief Initial error covariance matrix P_0
694 /// @param[in] variance_angles Initial Covariance of the attitude angles in [rad²]
695 /// @param[in] variance_vel Initial Covariance of the velocity in [m²/s²]
696 /// @param[in] variance_pos Initial Covariance of the position in [rad² rad² m²] n-frame / [m²] i,e-frame
697 /// @param[in] variance_accelBias Initial Covariance of the accelerometer biases in [m^2/s^4]
698 /// @param[in] variance_gyroBias Initial Covariance of the gyroscope biases in [rad^2/s^2]
699 /// @param[in] variance_heightBias Initial Covariance of the height bias [m^2]
700 /// @param[in] variance_heightScale Initial Covariance of the height scale in [m^2 / m^2]
701 /// @return The 17x17 matrix of initial state variances
702 [[nodiscard]] KeyedMatrix<double, KFStates, KFStates, 17, 17> initialErrorCovarianceMatrix_P0(const Eigen::Vector3d& variance_angles,
703 const Eigen::Vector3d& variance_vel,
704 const Eigen::Vector3d& variance_pos,
705 const Eigen::Vector3d& variance_accelBias,
706 const Eigen::Vector3d& variance_gyroBias,
707 const double& variance_heightBias,
708 const double& variance_heightScale) const;
709
710 // ###########################################################################################################
711 // Correction
712 // ###########################################################################################################
713
714 /// @brief Measurement matrix for GNSS measurements at timestep k, represented in navigation coordinates
715 /// @param[in] T_rn_p Conversion matrix between cartesian and curvilinear perturbations to the position
716 /// @param[in] n_Dcm_b Direction Cosine Matrix from body to navigation coordinates
717 /// @param[in] b_omega_ib Angular rate of body with respect to inertial system in body-frame coordinates in [rad/s]
718 /// @param[in] b_leverArm_InsGnss l_{ba}^b lever arm from the INS to the GNSS antenna in body-frame coordinates [m]
719 /// @param[in] n_Omega_ie Skew-symmetric matrix of the Earth-rotation vector in local navigation frame axes
720 /// @return The 6x17 measurement matrix 𝐇
721 [[nodiscard]] static KeyedMatrix<double, KFMeas, KFStates, 6, 17> n_measurementMatrix_H(const Eigen::Matrix3d& T_rn_p,
722 const Eigen::Matrix3d& n_Dcm_b,
723 const Eigen::Vector3d& b_omega_ib,
724 const Eigen::Vector3d& b_leverArm_InsGnss,
725 const Eigen::Matrix3d& n_Omega_ie);
726
727 /// @brief Measurement matrix for baro height measurements at timestep k, represented in navigation coordinates
728 /// @param[in] height predicted height
729 /// @param[in] scale height scale
730 /// @return The 6x17 measurement matrix 𝐇
731 [[nodiscard]] static KeyedMatrix<double, KFMeas, KFStates, 1, 17> n_measurementMatrix_H(const double& height, const double& scale);
732
733 /// @brief Measurement matrix for GNSS measurements at timestep k, represented in Earth frame coordinates
734 /// @param[in] e_Dcm_b Direction Cosine Matrix from body to Earth coordinates
735 /// @param[in] b_omega_ib Angular rate of body with respect to inertial system in body-frame coordinates in [rad/s]
736 /// @param[in] b_leverArm_InsGnss l_{ba}^b lever arm from the INS to the GNSS antenna in body-frame coordinates [m]
737 /// @param[in] e_Omega_ie Skew-symmetric matrix of the Earth-rotation vector in Earth frame axes
738 /// @return The 6x17 measurement matrix 𝐇
739 [[nodiscard]] static KeyedMatrix<double, KFMeas, KFStates, 6, 17> e_measurementMatrix_H(const Eigen::Matrix3d& e_Dcm_b,
740 const Eigen::Vector3d& b_omega_ib,
741 const Eigen::Vector3d& b_leverArm_InsGnss,
742 const Eigen::Matrix3d& e_Omega_ie);
743
744 /// @brief Measurement matrix for barometric height measurements at timestep k, represented in Earth frame coordinates
745 /// @param[in] e_positionEstimate predicted position
746 /// @param[in] height predicted height (assuming that the KF internally converts to LLA at every epoch)
747 /// @param[in] scale height scale
748 /// @return The 1x17 measurement matrix 𝐇
749 [[nodiscard]] static KeyedMatrix<double, KFMeas, KFStates, 1, 17> e_measurementMatrix_H(const Eigen::Vector3d& e_positionEstimate,
750 const double& height,
751 const double& scale);
752
753 /// @brief Measurement noise covariance matrix 𝐑
754 /// @param[in] posVelObs Position and velocity observation
755 /// @param[in] lla_position Position as Lat Lon Alt in [rad rad m]
756 /// @param[in] R_N Meridian radius of curvature in [m]
757 /// @param[in] R_E Prime vertical radius of curvature (East/West) [m]
758 /// @return The 6x6 measurement covariance matrix 𝐑
759 [[nodiscard]] KeyedMatrix<double, KFMeas, KFMeas, 6, 6> n_measurementNoiseCovariance_R(const std::shared_ptr<const PosVel>& posVelObs,
760 const Eigen::Vector3d& lla_position,
761 double R_N,
762 double R_E) const;
763
764 /// @brief Measurement noise covariance matrix 𝐑
765 /// @param[in] baroVarianceHeight Variance of height in [m²]
766 /// @return The 1x1 measurement covariance matrix 𝐑
767 [[nodiscard]] static KeyedMatrix<double, KFMeas, KFMeas, 1, 1> n_measurementNoiseCovariance_R(const double& baroVarianceHeight);
768
769 /// @brief Measurement noise covariance matrix 𝐑
770 /// @param[in] posVelObs Position and velocity observation
771 /// @return The 6x6 measurement covariance matrix 𝐑
772 [[nodiscard]] KeyedMatrix<double, KFMeas, KFMeas, 6, 6> e_measurementNoiseCovariance_R(const std::shared_ptr<const PosVel>& posVelObs) const;
773
774 /// @brief Measurement innovation vector 𝜹𝐳
775 /// @param[in] lla_positionMeasurement Position measurement as Lat Lon Alt in [rad rad m]
776 /// @param[in] lla_positionEstimate Position estimate as Lat Lon Alt in [rad rad m]
777 /// @param[in] n_velocityMeasurement Velocity measurement in the n frame in [m/s]
778 /// @param[in] n_velocityEstimate Velocity estimate in the n frame in [m/s]
779 /// @param[in] T_rn_p Conversion matrix between cartesian and curvilinear perturbations to the position
780 /// @param[in] n_Quat_b Rotation quaternion from body to navigation coordinates
781 /// @param[in] b_leverArm_InsGnss l_{ba}^b lever arm from the INS to the GNSS antenna in body-frame coordinates [m]
782 /// @param[in] b_omega_ib Angular rate of body with respect to inertial system in body-frame coordinates in [rad/s]
783 /// @param[in] n_Omega_ie Skew-symmetric matrix of the Earth-rotation vector in local navigation frame axes
784 /// @return The 6x1 measurement innovation vector 𝜹𝐳
785 [[nodiscard]] static KeyedVector<double, KFMeas, 6> n_measurementInnovation_dz(const Eigen::Vector3d& lla_positionMeasurement, const Eigen::Vector3d& lla_positionEstimate,
786 const Eigen::Vector3d& n_velocityMeasurement, const Eigen::Vector3d& n_velocityEstimate,
787 const Eigen::Matrix3d& T_rn_p, const Eigen::Quaterniond& n_Quat_b, const Eigen::Vector3d& b_leverArm_InsGnss,
788 const Eigen::Vector3d& b_omega_ib, const Eigen::Matrix3d& n_Omega_ie);
789
790 /// @brief Measurement innovation vector 𝜹𝐳
791 /// @param[in] baroheight barometric height measurement in [m]
792 /// @param[in] height predicted height in [m]
793 /// @param[in] heightbias height bias in [m]
794 /// @param[in] heightscale height scale [m/m]
795 /// @return The 1x1 measurement innovation vector 𝜹𝐳
796 [[nodiscard]] static KeyedVector<double, KFMeas, 1> n_measurementInnovation_dz(const double& baroheight, const double& height,
797 const double& heightbias, const double& heightscale);
798
799 /// @brief Measurement innovation vector 𝜹𝐳
800 /// @param[in] e_positionMeasurement Position measurement in ECEF coordinates in [m]
801 /// @param[in] e_positionEstimate Position estimate in ECEF coordinates in [m]
802 /// @param[in] e_velocityMeasurement Velocity measurement in the e frame in [m/s]
803 /// @param[in] e_velocityEstimate Velocity estimate in the e frame in [m/s]
804 /// @param[in] e_Quat_b Rotation quaternion from body to Earth coordinates
805 /// @param[in] b_leverArm_InsGnss l_{ba}^b lever arm from the INS to the GNSS antenna in body-frame coordinates [m]
806 /// @param[in] b_omega_ib Angular rate of body with respect to inertial system in body-frame coordinates in [rad/s]
807 /// @param[in] e_Omega_ie Skew-symmetric matrix of the Earth-rotation vector in Earth frame axes
808 /// @return The 6x1 measurement innovation vector 𝜹𝐳
809 [[nodiscard]] static KeyedVector<double, KFMeas, 6> e_measurementInnovation_dz(const Eigen::Vector3d& e_positionMeasurement, const Eigen::Vector3d& e_positionEstimate,
810 const Eigen::Vector3d& e_velocityMeasurement, const Eigen::Vector3d& e_velocityEstimate,
811 const Eigen::Quaterniond& e_Quat_b, const Eigen::Vector3d& b_leverArm_InsGnss,
812 const Eigen::Vector3d& b_omega_ib, const Eigen::Matrix3d& e_Omega_ie);
813 };
814
815 } // namespace NAV
816
817 #ifndef DOXYGEN_IGNORE
818
819 template<>
820 struct fmt::formatter<NAV::LooselyCoupledKF::KFStates> : fmt::formatter<const char*>
821 {
822 /// @brief Defines how to format structs
823 /// @param[in] st Struct to format
824 /// @param[in, out] ctx Format context
825 /// @return Output iterator
826 template<typename FormatContext>
827 ✗ auto format(const NAV::LooselyCoupledKF::KFStates& st, FormatContext& ctx) const
828 {
829 ✗ switch (st)
830 {
831 ✗ case NAV::LooselyCoupledKF::KFStates::Roll:
832 ✗ return fmt::formatter<const char*>::format("Roll/Psi_eb_1", ctx);
833 ✗ case NAV::LooselyCoupledKF::KFStates::Pitch:
834 ✗ return fmt::formatter<const char*>::format("Pitch/Psi_eb_2", ctx);
835 ✗ case NAV::LooselyCoupledKF::KFStates::Yaw:
836 ✗ return fmt::formatter<const char*>::format("Yaw/Psi_eb_3", ctx);
837 ✗ case NAV::LooselyCoupledKF::KFStates::VelN:
838 ✗ return fmt::formatter<const char*>::format("VelN/VelX", ctx);
839 ✗ case NAV::LooselyCoupledKF::KFStates::VelE:
840 ✗ return fmt::formatter<const char*>::format("VelE/VelY", ctx);
841 ✗ case NAV::LooselyCoupledKF::KFStates::VelD:
842 ✗ return fmt::formatter<const char*>::format("VelD/VelZ", ctx);
843 ✗ case NAV::LooselyCoupledKF::KFStates::PosLat:
844 ✗ return fmt::formatter<const char*>::format("PosLat/PosX", ctx);
845 ✗ case NAV::LooselyCoupledKF::KFStates::PosLon:
846 ✗ return fmt::formatter<const char*>::format("PosLon/PosY", ctx);
847 ✗ case NAV::LooselyCoupledKF::KFStates::PosAlt:
848 ✗ return fmt::formatter<const char*>::format("PosAlt/PosZ", ctx);
849 ✗ case NAV::LooselyCoupledKF::KFStates::AccBiasX:
850 ✗ return fmt::formatter<const char*>::format("AccBiasX", ctx);
851 ✗ case NAV::LooselyCoupledKF::KFStates::AccBiasY:
852 ✗ return fmt::formatter<const char*>::format("AccBiasY", ctx);
853 ✗ case NAV::LooselyCoupledKF::KFStates::AccBiasZ:
854 ✗ return fmt::formatter<const char*>::format("AccBiasZ", ctx);
855 ✗ case NAV::LooselyCoupledKF::KFStates::GyrBiasX:
856 ✗ return fmt::formatter<const char*>::format("GyrBiasX", ctx);
857 ✗ case NAV::LooselyCoupledKF::KFStates::GyrBiasY:
858 ✗ return fmt::formatter<const char*>::format("GyrBiasY", ctx);
859 ✗ case NAV::LooselyCoupledKF::KFStates::GyrBiasZ:
860 ✗ return fmt::formatter<const char*>::format("GyrBiasZ", ctx);
861 ✗ case NAV::LooselyCoupledKF::KFStates::HeightBias:
862 ✗ return fmt::formatter<const char*>::format("HeightBias", ctx);
863 ✗ case NAV::LooselyCoupledKF::KFStates::HeightScale:
864 ✗ return fmt::formatter<const char*>::format("HeightScale", ctx);
865 ✗ case NAV::LooselyCoupledKF::KFStates::KFStates_COUNT:
866 ✗ return fmt::formatter<const char*>::format("COUNT", ctx);
867 }
868
869 ✗ return fmt::formatter<const char*>::format("ERROR", ctx);
870 }
871 };
872 template<>
873 struct fmt::formatter<NAV::LooselyCoupledKF::KFMeas> : fmt::formatter<const char*>
874 {
875 /// @brief Defines how to format structs
876 /// @param[in] st Struct to format
877 /// @param[in, out] ctx Format context
878 /// @return Output iterator
879 template<typename FormatContext>
880 ✗ auto format(const NAV::LooselyCoupledKF::KFMeas& st, FormatContext& ctx) const
881 {
882 ✗ switch (st)
883 {
884 ✗ case NAV::LooselyCoupledKF::KFMeas::dPosLat:
885 ✗ return fmt::formatter<const char*>::format("dPosLat/dPosX", ctx);
886 ✗ case NAV::LooselyCoupledKF::KFMeas::dPosLon:
887 ✗ return fmt::formatter<const char*>::format("dPosLon/dPosY", ctx);
888 ✗ case NAV::LooselyCoupledKF::KFMeas::dPosAlt:
889 ✗ return fmt::formatter<const char*>::format("dPosAlt/dPosZ", ctx);
890 ✗ case NAV::LooselyCoupledKF::KFMeas::dVelN:
891 ✗ return fmt::formatter<const char*>::format("dVelN/dVelX", ctx);
892 ✗ case NAV::LooselyCoupledKF::KFMeas::dVelE:
893 ✗ return fmt::formatter<const char*>::format("dVelE/dVelY", ctx);
894 ✗ case NAV::LooselyCoupledKF::KFMeas::dVelD:
895 ✗ return fmt::formatter<const char*>::format("dVelD/dVelZ", ctx);
896 ✗ case NAV::LooselyCoupledKF::KFMeas::dHgt:
897 ✗ return fmt::formatter<const char*>::format("dHgt", ctx);
898 }
899
900 ✗ return fmt::formatter<const char*>::format("ERROR", ctx);
901 }
902 };
903
904 #endif
905
906 /// @brief Stream insertion operator overload
907 /// @param[in, out] os Output stream object to stream the time into
908 /// @param[in] obj Object to print
909 /// @return Returns the output stream object in order to chain stream insertions
910 std::ostream& operator<<(std::ostream& os, const NAV::LooselyCoupledKF::KFStates& obj);
911
912 /// @brief Stream insertion operator overload
913 /// @param[in, out] os Output stream object to stream the time into
914 /// @param[in] obj Object to print
915 /// @return Returns the output stream object in order to chain stream insertions
916 std::ostream& operator<<(std::ostream& os, const NAV::LooselyCoupledKF::KFMeas& obj);
917