0.4.1
Loading...
Searching...
No Matches
LooselyCoupledKF.hpp
Go to the documentation of this file.
1// This file is part of INSTINCT, the INS Toolkit for Integrated
2// Navigation Concepts and Training by the Institute of Navigation of
3// the University of Stuttgart, Germany.
4//
5// This Source Code Form is subject to the terms of the Mozilla Public
6// License, v. 2.0. If a copy of the MPL was not distributed with this
7// file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9/// @file 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>
20
24
28
30
31namespace NAV
32{
33/// @brief Loosely-coupled Kalman Filter for INS/GNSS integration
34class LooselyCoupledKF : public Node
35{
36 public:
37 /// @brief Default constructor
39 /// @brief Destructor
40 ~LooselyCoupledKF() override;
41 /// @brief Copy constructor
43 /// @brief Move constructor
45 /// @brief Copy assignment operator
47 /// @brief Move assignment operator
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
182 /// Prefer the raw acceleration measurements over the deltaVel & deltaTheta values
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
202
203 /// Whether to enable barometric height (including the corresponding pin)
205
206 /// Time from the external init
208
209 /// Whether the accumulated biases have been initialized in the 'inertialIntegrator'
211
212 /// @brief Vector with all state keys
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
233 /// @brief All position, velocity and attitude keys
237
238 /// @brief Vector with all measurement keys
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
247
248 // #########################################################################################################################################
249 // GUI settings
250 // #########################################################################################################################################
251
252 /// @brief Check the rank of the Kalman matrices every iteration (computational expensive)
254
255 // ###########################################################################################################
256 // Parameters
257 // ###########################################################################################################
258
259 /// Gui selection for the Unit of the input stdev_ra parameter
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 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
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 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
279
280 /// @brief 𝜎²_bad Variance of the accelerometer dynamic bias
281 /// @note Value from VN-310 Datasheet (In-Run Bias Stability (Allan Variance))
282 Eigen::Vector3d _stdev_bad = 10 /* [µg] */ * Eigen::Vector3d::Ones();
283
284 /// @brief Correlation length of the accelerometer dynamic bias in [s]
285 Eigen::Vector3d _tau_bad = 0.1 * Eigen::Vector3d::Ones();
286
287 // ###########################################################################################################
288
289 /// Gui selection for the Unit of the input variance_bad parameter
291
292 /// @brief 𝜎²_bgd Variance of the gyro dynamic bias
293 /// @note Value from VN-310 Datasheet (In-Run Bias Stability (Allan Variance))
294 Eigen::Vector3d _stdev_bgd = 1 /* [°/h] */ * Eigen::Vector3d::Ones();
295
296 /// @brief Correlation length of the gyro dynamic bias in [s]
297 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
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
321
322 /// Uncertainty of the barometric height scale
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
342 /// @brief Random Process used to estimate the gyroscope biases
344
345 // ###########################################################################################################
346
347 /// Possible Units for the GNSS measurement uncertainty for the position (standard deviation σ or Variance σ²)
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
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 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
362
363 // ###########################################################################################################
364
365 /// Possible Units for the GNSS measurement uncertainty for the velocity (standard deviation σ or Variance σ²)
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
373
374 /// GUI selection of the GNSS NED velocity measurement uncertainty (standard deviation σ or Variance σ²)
375 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
379
380 // ###########################################################################################################
381
382 /// Possible Units for the barometric height measurement uncertainty (standard deviation σ or Variance σ²)
384 {
385 m2, ///< Variance [m²]
386 m, ///< Standard deviation [m]
387 };
388 /// Gui selection for the Unit of the barometric height measurement uncertainty
390
391 /// GUI selection of the barometric height measurement uncertainty (standard deviation σ or Variance σ²)
393
394 /// Whether to override the barometric height uncertainty or use the one included in the measurement
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
409
410 /// GUI selection of the initial covariance diagonal values for position (standard deviation σ or Variance σ²)
411 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
423
424 /// GUI selection of the initial covariance diagonal values for velocity (standard deviation σ or Variance σ²)
425 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 σ²)
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
439
440 /// GUI selection of the initial covariance diagonal values for attitude angles (standard deviation σ or Variance σ²)
441 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
453
454 /// GUI selection of the initial covariance diagonal values for accelerometer biases (standard deviation σ or Variance σ²)
455 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
469
470 /// GUI selection of the initial covariance diagonal values for gyroscope biases (standard deviation σ or Variance σ²)
471 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
483
484 /// GUI selection of the initial covariance diagonal values for the height bias (standard deviation σ or Variance σ²)
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
497
498 /// GUI selection of the initial covariance diagonal values for the height scale (standard deviation σ or Variance σ²)
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
511
512 /// GUI selection of the initial accelerometer biases
513 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
525
526 /// GUI selection of the initial gyroscope biases
527 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
539
540 /// GUI option for the order of the Taylor polynom to calculate the Phi matrix
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
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
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
819template<>
820struct 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 {
832 return fmt::formatter<const char*>::format("Roll/Psi_eb_1", ctx);
834 return fmt::formatter<const char*>::format("Pitch/Psi_eb_2", ctx);
836 return fmt::formatter<const char*>::format("Yaw/Psi_eb_3", ctx);
838 return fmt::formatter<const char*>::format("VelN/VelX", ctx);
840 return fmt::formatter<const char*>::format("VelE/VelY", ctx);
842 return fmt::formatter<const char*>::format("VelD/VelZ", ctx);
844 return fmt::formatter<const char*>::format("PosLat/PosX", ctx);
846 return fmt::formatter<const char*>::format("PosLon/PosY", ctx);
848 return fmt::formatter<const char*>::format("PosAlt/PosZ", ctx);
850 return fmt::formatter<const char*>::format("AccBiasX", ctx);
852 return fmt::formatter<const char*>::format("AccBiasY", ctx);
854 return fmt::formatter<const char*>::format("AccBiasZ", ctx);
856 return fmt::formatter<const char*>::format("GyrBiasX", ctx);
858 return fmt::formatter<const char*>::format("GyrBiasY", ctx);
860 return fmt::formatter<const char*>::format("GyrBiasZ", ctx);
862 return fmt::formatter<const char*>::format("HeightBias", ctx);
864 return fmt::formatter<const char*>::format("HeightScale", ctx);
866 return fmt::formatter<const char*>::format("COUNT", ctx);
867 }
868
869 return fmt::formatter<const char*>::format("ERROR", ctx);
870 }
871};
872template<>
873struct 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 {
885 return fmt::formatter<const char*>::format("dPosLat/dPosX", ctx);
887 return fmt::formatter<const char*>::format("dPosLon/dPosY", ctx);
889 return fmt::formatter<const char*>::format("dPosAlt/dPosZ", ctx);
891 return fmt::formatter<const char*>::format("dVelN/dVelX", ctx);
893 return fmt::formatter<const char*>::format("dVelE/dVelY", ctx);
895 return fmt::formatter<const char*>::format("dVelD/dVelZ", ctx);
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
910std::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
916std::ostream& operator<<(std::ostream& os, const NAV::LooselyCoupledKF::KFMeas& obj);
Barometric Height Storage Class.
Inputs pins which can be added dynamically.
nlohmann::json json
json namespace
Units used by INS.
Parent Class for all IMU Observations.
Inertial Measurement Integrator.
The class is responsible for all time-related tasks.
Kalman Filter with keyed states.
std::ostream & operator<<(std::ostream &os, const NAV::LooselyCoupledKF::KFStates &obj)
Stream insertion operator overload.
Node Class.
Position, Velocity and Attitude Storage Class.
IMU Position.
Definition ImuPos.hpp:26
Inertial Measurement Integrator.
TsDeque< std::shared_ptr< const NAV::NodeData > > NodeDataQueue
Node data queue type.
Definition Pin.hpp:707
The class is responsible for all time-related tasks.
Definition InsTime.hpp:710
Static sized KeyedMatrix.
Static sized KeyedVector.
InitCovarianceScaleHeight _initCovarianceScaleHeightUnit
Gui selection for the Unit of the initial covariance for the height scale.
InitCovarianceVelocityUnit
Possible Units for the initial covariance for the velocity (standard deviation σ or Variance σ²)
KFStates
State Keys of the Kalman filter.
@ HeightScale
Baro Height Scale.
@ GyrBiasX
Gyroscope Bias X.
@ HeightBias
Baro Height Bias.
@ Psi_eb_3
Angle between Earth and Body frame around 3. axis.
@ AccBiasZ
Accelerometer Bias Z.
@ GyrBiasZ
Gyroscope Bias Z.
@ GyrBiasY
Gyroscope Bias Y.
@ AccBiasX
Accelerometer Bias X.
@ AccBiasY
Accelerometer Bias Y.
@ KFStates_COUNT
Amount of states.
@ Psi_eb_2
Angle between Earth and Body frame around 2. axis.
@ Psi_eb_1
Angle between Earth and Body frame around 1. axis.
Units::ImuAccelerometerFilterNoiseUnits _stdevAccelNoiseUnits
Gui selection for the Unit of the input stdev_ra parameter.
StdevBaroHeightScaleUnits _stdevBaroHeightScaleUnits
Gui selection for the Unit of the barometric height scale uncertainty.
RandomProcess _randomProcessAccel
Random Process used to estimate the accelerometer biases.
static KeyedMatrix< double, KFMeas, KFStates, 6, 17 > e_measurementMatrix_H(const Eigen::Matrix3d &e_Dcm_b, const Eigen::Vector3d &b_omega_ib, const Eigen::Vector3d &b_leverArm_InsGnss, const Eigen::Matrix3d &e_Omega_ie)
Measurement matrix for GNSS measurements at timestep k, represented in Earth frame coordinates.
json save() const override
Saves the node into a json object.
InitBiasAccelUnit _initBiasAccelUnit
Gui selection for the unit of the initial accelerometer biases.
InitCovarianceVelocityUnit _initCovarianceVelocityUnit
Gui selection for the Unit of the initial covariance for the velocity.
bool _preferAccelerationOverDeltaMeasurements
Prefer the raw acceleration measurements over the deltaVel & deltaTheta values.
static void pinAddCallback(Node *node)
Function to call to add a new pin.
Eigen::Vector3d _initBiasAccel
GUI selection of the initial accelerometer biases.
InitCovarianceAttitudeAnglesUnit
Possible Units for the initial covariance for the attitude angles (standard deviation σ or Variance σ...
static KeyedVector< double, KFMeas, 6 > n_measurementInnovation_dz(const Eigen::Vector3d &lla_positionMeasurement, const Eigen::Vector3d &lla_positionEstimate, const Eigen::Vector3d &n_velocityMeasurement, const Eigen::Vector3d &n_velocityEstimate, const Eigen::Matrix3d &T_rn_p, const Eigen::Quaterniond &n_Quat_b, const Eigen::Vector3d &b_leverArm_InsGnss, const Eigen::Vector3d &b_omega_ib, const Eigen::Matrix3d &n_Omega_ie)
Measurement innovation vector 𝜹𝐳
Eigen::Vector3d _gnssMeasurementUncertaintyVelocity
GUI selection of the GNSS NED velocity measurement uncertainty (standard deviation σ or Variance σ²)
LooselyCoupledKF & operator=(const LooselyCoupledKF &)=delete
Copy assignment operator.
StdevBaroHeightBiasUnits _stdevBaroHeightBiasUnits
Gui selection for the Unit of the barometric height bias uncertainty.
PhiCalculationAlgorithm _phiCalculationAlgorithm
GUI option for the Phi calculation algorithm.
int _phiCalculationTaylorOrder
GUI option for the order of the Taylor polynom to calculate the Phi matrix.
Eigen::Vector3d _initCovariancePosition
GUI selection of the initial covariance diagonal values for position (standard deviation σ or Varianc...
InitBiasGyroUnit
Possible Units for the initial gyroscope biases.
static std::string category()
String representation of the class category.
static const std::vector< KFMeas > dPos
All position difference keys.
void updateInputPins()
Update the input pins depending on the GUI.
KeyedMatrix< double, KFMeas, KFMeas, 6, 6 > e_measurementNoiseCovariance_R(const std::shared_ptr< const PosVel > &posVelObs) const
Measurement noise covariance matrix 𝐑
static KeyedVector< double, KFMeas, 6 > e_measurementInnovation_dz(const Eigen::Vector3d &e_positionMeasurement, const Eigen::Vector3d &e_positionEstimate, const Eigen::Vector3d &e_velocityMeasurement, const Eigen::Vector3d &e_velocityEstimate, const Eigen::Quaterniond &e_Quat_b, const Eigen::Vector3d &b_leverArm_InsGnss, const Eigen::Vector3d &b_omega_ib, const Eigen::Matrix3d &e_Omega_ie)
Measurement innovation vector 𝜹𝐳
static const std::vector< KFMeas > Meas
Vector with all measurement keys.
KeyedMatrix< double, KFMeas, KFMeas, 6, 6 > n_measurementNoiseCovariance_R(const std::shared_ptr< const PosVel > &posVelObs, const Eigen::Vector3d &lla_position, double R_N, double R_E) const
Measurement noise covariance matrix 𝐑
double _heightScaleTotal
Accumulator for height scale [m/m].
LooselyCoupledKF(const LooselyCoupledKF &)=delete
Copy constructor.
void recvImuObservation(InputPin::NodeDataQueue &queue, size_t pinIdx)
Receive Function for the IMU observation.
static const std::vector< KFStates > KFAccBias
All acceleration bias keys.
GnssMeasurementUncertaintyVelocityUnit _gnssMeasurementUncertaintyVelocityUnit
Gui selection for the Unit of the GNSS measurement uncertainty for the velocity.
void looselyCoupledUpdate(const std::shared_ptr< const PosVel > &posVelObs)
Updates the predicted state from the InertialNavSol with the PosVel observation.
Eigen::Vector3d _initCovarianceVelocity
GUI selection of the initial covariance diagonal values for velocity (standard deviation σ or Varianc...
static KeyedMatrix< double, KFStates, KFStates, 17, 17 > noiseInputMatrix_G(const Eigen::Quaterniond &ien_Quat_b)
Calculates the noise input matrix 𝐆
bool hasInputPinWithSameTime(const InsTime &insTime) const
Checks wether there is an input pin with the same time.
Eigen::Vector3d _initCovarianceBiasGyro
GUI selection of the initial covariance diagonal values for gyroscope biases (standard deviation σ or...
LooselyCoupledKF & operator=(LooselyCoupledKF &&)=delete
Move assignment operator.
bool _gnssMeasurementUncertaintyPositionOverride
Whether to override the position uncertainty or use the one included in the measurement.
Units::ImuGyroscopeFilterNoiseUnits _stdevGyroNoiseUnits
Gui selection for the Unit of the input stdev_rg parameter.
Eigen::Vector3d _gnssMeasurementUncertaintyPosition
GUI selection of the GNSS position measurement uncertainty (standard deviation σ or Variance σ²)....
static constexpr size_t INPUT_PORT_INDEX_POS_VEL_ATT_INIT
Flow (PosVelAtt)
~LooselyCoupledKF() override
Destructor.
QCalculationAlgorithm _qCalculationAlgorithm
GUI option for the Q calculation algorithm.
InitCovariancePositionUnit
Possible Units for the initial covariance for the position (standard deviation σ or Variance σ²)
@ rad_rad_m
Standard deviation LatLonAlt [rad, rad, m].
@ rad2_rad2_m2
Variance LatLonAlt^2 [rad^2, rad^2, m^2].
static const std::vector< KFStates > KFVel
All velocity keys.
bool _initializeStateOverExternalPin
Whether to initialize the state over an external pin.
Eigen::Vector3d _tau_bgd
Correlation length of the gyro dynamic bias in [s].
QCalculationAlgorithm
GUI option for the Q calculation algorithm.
double _initCovarianceBiasHeight
GUI selection of the initial covariance diagonal values for the height bias (standard deviation σ or ...
StdevBaroHeightBiasUnits
Possible Units for the Variance of the barometric height bias.
InitCovarianceScaleHeight
Possible Units for the initial covariance for the height scale (standard deviation σ or Variance σ²)
bool _checkKalmanMatricesRanks
Check the rank of the Kalman matrices every iteration (computational expensive)
Units::ImuAccelerometerFilterBiasUnits _stdevAccelBiasUnits
Gui selection for the Unit of the input variance_bad parameter.
GnssMeasurementUncertaintyPositionUnit _gnssMeasurementUncertaintyPositionUnit
Gui selection for the Unit of the GNSS measurement uncertainty for the position.
double _stdevBaroHeightBias
Uncertainty of the barometric height bias.
static const std::vector< KFStates > KFAtt
All attitude keys.
KeyedKalmanFilterD< KFStates, KFMeas > _kalmanFilter
Kalman Filter representation.
InitBiasGyroUnit _initBiasGyroUnit
Gui selection for the unit of the initial gyroscope biases.
double _initCovarianceScaleHeight
GUI selection of the initial covariance diagonal values for the height scale (standard deviation σ or...
static const std::vector< KFMeas > dVel
All velocity difference keys.
LooselyCoupledKF(LooselyCoupledKF &&)=delete
Move constructor.
Eigen::Vector3d _accelBiasTotal
Accumulator for acceleration bias [m/s²].
void recvPosVelAttInit(InputPin::NodeDataQueue &queue, size_t pinIdx)
Receive Function for the PosVelAtt observation.
std::shared_ptr< const ImuObs > _lastImuObs
Last received IMU observation (to get ImuPos)
PhiCalculationAlgorithm
GUI option for the Phi calculation algorithm.
static const std::vector< KFStates > States
Vector with all state keys.
InitCovarianceBiasAccelUnit _initCovarianceBiasAccelUnit
Gui selection for the Unit of the initial covariance for the accelerometer biases.
InitCovariancePositionUnit _initCovariancePositionUnit
Gui selection for the Unit of the initial covariance for the position.
gui::widgets::DynamicInputPins _dynamicInputPins
Dynamic input pins.
Eigen::Vector3d _stdev_rg
𝜎_rg Standard deviation of the noise on the gyro angular-rate measurements
static const std::vector< KFStates > KFPosVelAtt
All position, velocity and attitude keys.
void recvBaroHeight(InputPin::NodeDataQueue &queue, size_t pinIdx)
Receive Function for the BaroHgt observation.
InitCovarianceBiasGyroUnit
Possible Units for the initial covariance for the gyroscope biases (standard deviation σ or Variance ...
InsTime _externalInitTime
Time from the external init.
static constexpr size_t OUTPUT_PORT_INDEX_SOLUTION
Flow (InsGnssLCKFSolution)
static constexpr size_t INPUT_PORT_INDEX_IMU
Flow (ImuObs)
void deinitialize() override
Deinitialize the node.
void setSolutionPosVelAttAndCov(const std::shared_ptr< PosVelAtt > &lckfSolution, double R_N, double R_E)
Sets the covariance matrix P of the LCKF (and does the necessary unit conversions)
Eigen::Vector3d _stdev_bad
𝜎²_bad Variance of the accelerometer dynamic bias
InertialIntegrator _inertialIntegrator
Inertial Integrator.
void restore(const json &j) override
Restores the node from a json object.
void invokeCallbackWithPosVelAtt(const PosVelAtt &posVelAtt)
Invoke the callback with a PosVelAtt solution (without LCKF specific output)
KeyedMatrix< double, KFStates, KFStates, 17, 17 > e_systemMatrix_F(const Eigen::Quaterniond &e_Quat_b, const Eigen::Vector3d &b_specForce_ib, const Eigen::Vector3d &e_position, const Eigen::Vector3d &e_gravitation, double r_eS_e, const Eigen::Vector3d &e_omega_ie, const Eigen::Vector3d &tau_bad, const Eigen::Vector3d &tau_bgd) const
Calculates the system matrix 𝐅 for the ECEF frame.
void looselyCoupledPrediction(const std::shared_ptr< const PosVelAtt > &inertialNavSol, double tau_i, const ImuPos &imuPos)
Predicts the state from the InertialNavSol.
KFMeas
Measurement Keys of the Kalman filter.
@ dPosLat
Latitude difference.
@ dPosLon
Longitude difference.
@ dPosY
ECEF Position Y difference.
@ dPosX
ECEF Position X difference.
@ dVelE
Velocity East difference.
@ dPosZ
ECEF Position Z difference.
@ dPosAlt
Altitude difference.
@ dHgt
height difference
@ dVelN
Velocity North difference.
@ dVelZ
ECEF Velocity Z difference.
@ dVelX
ECEF Velocity X difference.
@ dVelY
ECEF Velocity Y difference.
@ dVelD
Velocity Down difference.
static const std::vector< KFStates > KFPosVel
All position and velocity keys.
InitBiasAccelUnit
Possible Units for the initial accelerometer biases.
BaroHeightMeasurementUncertaintyUnit _barometricHeightMeasurementUncertaintyUnit
Gui selection for the Unit of the barometric height measurement uncertainty.
double _stdevBaroHeightScale
Uncertainty of the barometric height scale.
KeyedMatrix< double, KFStates, KFStates, 17, 17 > initialErrorCovarianceMatrix_P0(const Eigen::Vector3d &variance_angles, const Eigen::Vector3d &variance_vel, const Eigen::Vector3d &variance_pos, const Eigen::Vector3d &variance_accelBias, const Eigen::Vector3d &variance_gyroBias, const double &variance_heightBias, const double &variance_heightScale) const
Initial error covariance matrix P_0.
InitCovarianceBiasAccelUnit
Possible Units for the initial covariance for the accelerometer biases (standard deviation σ or Varia...
double _heightBiasTotal
Accumulator for height bias [m].
StdevBaroHeightScaleUnits
Possible Units for the Variance of the barometric height scale.
void recvPosVelObservation(InputPin::NodeDataQueue &queue, size_t pinIdx)
Receive Function for the PosVel observation.
bool _baroHeightMeasurementUncertaintyOverride
Whether to override the barometric height uncertainty or use the one included in the measurement.
static std::string typeStatic()
String representation of the class type.
bool _gnssMeasurementUncertaintyVelocityOverride
Whether to override the velocity uncertainty or use the one included in the measurement.
Units::ImuGyroscopeFilterBiasUnits _stdevGyroBiasUnits
Gui selection for the Unit of the input variance_bad parameter.
static const std::vector< KFStates > KFPos
All position keys.
BaroHeightMeasurementUncertaintyUnit
Possible Units for the barometric height measurement uncertainty (standard deviation σ or Variance σ²...
Eigen::Vector3d _stdev_ra
𝜎_ra Standard deviation of the noise on the accelerometer specific-force measurements
Eigen::Vector3d _tau_bad
Correlation length of the accelerometer dynamic bias in [s].
KeyedMatrix< double, KFStates, KFStates, 17, 17 > n_systemMatrix_F(const Eigen::Quaterniond &n_Quat_b, const Eigen::Vector3d &b_specForce_ib, const Eigen::Vector3d &n_omega_in, const Eigen::Vector3d &n_velocity, const Eigen::Vector3d &lla_position, double R_N, double R_E, double g_0, double r_eS_e, const Eigen::Vector3d &tau_bad, const Eigen::Vector3d &tau_bgd) const
Calculates the system matrix 𝐅 for the local navigation frame.
InitCovarianceBiasGyroUnit _initCovarianceBiasGyroUnit
Gui selection for the Unit of the initial covariance for the gyroscope biases.
static void pinDeleteCallback(Node *node, size_t pinIdx)
Function to call to delete a pin.
std::string type() const override
String representation of the class type.
static const std::vector< KFStates > KFGyrBias
All gyroscope bias keys.
GnssMeasurementUncertaintyPositionUnit
Possible Units for the GNSS measurement uncertainty for the position (standard deviation σ or Varianc...
std::array< double, 3 > _initalRollPitchYaw
Roll, Pitch and Yaw angles in [deg] used for initialization if not taken from separate pin.
LooselyCoupledKF()
Default constructor.
static KeyedMatrix< double, KFStates, KFStates, 17, 17 > e_systemNoiseCovarianceMatrix_Q(const Eigen::Vector3d &sigma2_ra, const Eigen::Vector3d &sigma2_rg, const Eigen::Vector3d &sigma2_bad, const Eigen::Vector3d &sigma2_bgd, const double &sigma_heightBias, const double &sigma_heightScale, const Eigen::Vector3d &tau_bad, const Eigen::Vector3d &tau_bgd, const Eigen::Matrix3d &e_F_21, const Eigen::Matrix3d &e_Dcm_b, const double &tau_s)
System noise covariance matrix 𝐐_{k-1}.
RandomProcess
Available Random processes.
double _barometricHeightMeasurementUncertainty
GUI selection of the barometric height measurement uncertainty (standard deviation σ or Variance σ²)
static KeyedMatrix< double, KFMeas, KFStates, 6, 17 > n_measurementMatrix_H(const Eigen::Matrix3d &T_rn_p, const Eigen::Matrix3d &n_Dcm_b, const Eigen::Vector3d &b_omega_ib, const Eigen::Vector3d &b_leverArm_InsGnss, const Eigen::Matrix3d &n_Omega_ie)
Measurement matrix for GNSS measurements at timestep k, represented in navigation coordinates.
Eigen::Vector3d _gyroBiasTotal
Accumulator gyro bias [rad/s].
bool _initialSensorBiasesApplied
Whether the accumulated biases have been initialized in the 'inertialIntegrator'.
RandomProcess _randomProcessGyro
Random Process used to estimate the gyroscope biases.
Eigen::Vector3d _initCovarianceBiasAccel
GUI selection of the initial covariance diagonal values for accelerometer biases (standard deviation ...
InitCovarianceBiasHeightUnit
Possible Units for the initial covariance for the height bias (standard deviation σ or Variance σ²)
void guiConfig() override
ImGui config window which is shown on double click.
bool initialize() override
Initialize the node.
bool _enableBaroHgt
Whether to enable barometric height (including the corresponding pin)
Eigen::Vector3d _initCovarianceAttitudeAngles
GUI selection of the initial covariance diagonal values for attitude angles (standard deviation σ or ...
GnssMeasurementUncertaintyVelocityUnit
Possible Units for the GNSS measurement uncertainty for the velocity (standard deviation σ or Varianc...
Eigen::Matrix< double, 17, 17 > noiseScaleMatrix_W(const Eigen::Vector3d &sigma_ra, const Eigen::Vector3d &sigma_rg, const Eigen::Vector3d &sigma_bad, const Eigen::Vector3d &sigma_bgd, const Eigen::Vector3d &tau_bad, const Eigen::Vector3d &tau_bgd, const double &sigma_heightBias, const double &sigma_heightScale)
Calculates the noise scale matrix 𝐖
Eigen::Vector3d _stdev_bgd
𝜎²_bgd Variance of the gyro dynamic bias
Eigen::Vector3d _initBiasGyro
GUI selection of the initial gyroscope biases.
InitCovarianceAttitudeAnglesUnit _initCovarianceAttitudeAnglesUnit
Gui selection for the Unit of the initial covariance for the attitude angles.
static KeyedMatrix< double, KFStates, KFStates, 17, 17 > n_systemNoiseCovarianceMatrix_Q(const Eigen::Vector3d &sigma2_ra, const Eigen::Vector3d &sigma2_rg, const Eigen::Vector3d &sigma2_bad, const Eigen::Vector3d &sigma2_bgd, const double &sigma_heightBias, const double &sigma_heightScale, const Eigen::Vector3d &tau_bad, const Eigen::Vector3d &tau_bgd, const Eigen::Matrix3d &n_F_21, const Eigen::Matrix3d &T_rn_p, const Eigen::Matrix3d &n_Dcm_b, const double &tau_s)
System noise covariance matrix 𝐐_{k-1}.
InitCovarianceBiasHeightUnit _initCovarianceBiasHeightUnit
Gui selection for the Unit of the initial covariance for the height bias.
Node(std::string name)
Constructor.
Definition Node.cpp:30
Position, Velocity and Attitude Storage Class.
Definition PosVelAtt.hpp:25
ImuAccelerometerFilterNoiseUnits
Possible units to specify an accelerometer noise in a filter.
Definition Units.hpp:84
ImuAccelerometerFilterBiasUnits
Possible units for the accelerometer dynamic bias.
Definition Units.hpp:102
ImuGyroscopeFilterBiasUnits
Possible units for the gyroscope dynamic bias.
Definition Units.hpp:110
ImuGyroscopeFilterNoiseUnits
Possible units to specify an gyro noise in a filter.
Definition Units.hpp:92
@ deg_hr_sqrtHz
[deg / hr /√(Hz)]
Definition Units.hpp:96
KeyedKalmanFilter< double, StateKeyType, MeasKeyType > KeyedKalmanFilterD
Keyed Kalman Filter class with double as type.
Inputs pins which can be added dynamically.