INSTINCT Code Coverage Report

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