0.3.0
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
14
15#pragma once
16
17#include <cstdint>
19
23
27
29
30namespace NAV
31{
33class LooselyCoupledKF : public Node
34{
35 public:
49 [[nodiscard]] static std::string typeStatic();
50
52 [[nodiscard]] std::string type() const override;
53
55 [[nodiscard]] static std::string category();
56
59 void guiConfig() override;
60
62 [[nodiscard]] json save() const override;
63
66 void restore(const json& j) override;
67
100
119
120 private:
121 constexpr static size_t INPUT_PORT_INDEX_IMU = 0;
122 constexpr static size_t INPUT_PORT_INDEX_GNSS = 1;
123 constexpr static size_t INPUT_PORT_INDEX_POS_VEL_ATT_INIT = 2;
124 constexpr static size_t OUTPUT_PORT_INDEX_SOLUTION = 0;
125
127 bool initialize() override;
128
130 void deinitialize() override;
131
135
139 void recvImuObservation(InputPin::NodeDataQueue& queue, size_t pinIdx);
140
145
149 void recvPosVelAttInit(InputPin::NodeDataQueue& queue, size_t pinIdx);
150
154 void recvBaroHeight(InputPin::NodeDataQueue& queue, size_t pinIdx);
155
160 void looselyCoupledPrediction(const std::shared_ptr<const PosVelAtt>& inertialNavSol, double tau_i, const ImuPos& imuPos);
161
164 void looselyCoupledUpdate(const std::shared_ptr<const PosVel>& posVelObs);
165
168 void looselyCoupledUpdate(const std::shared_ptr<const BaroHgt>& baroHgtObs);
169
172
177 void setSolutionPosVelAttAndCov(const std::shared_ptr<PosVelAtt>& lckfSolution, double R_N, double R_E);
178
183
185 std::shared_ptr<const ImuObs> _lastImuObs = nullptr;
186
188 double _heightBiasTotal = 0.0;
190 double _heightScaleTotal = 1.0;
191
193 Eigen::Vector3d _accelBiasTotal = Eigen::Vector3d::Zero();
195 Eigen::Vector3d _gyroBiasTotal = Eigen::Vector3d::Zero();
196
198 std::array<double, 3> _initalRollPitchYaw{};
201
204
207
210
218
219 inline static const std::vector<KFStates> KFPos = { KFStates::PosLat, KFStates::PosLon, KFStates::PosAlt };
221 inline static const std::vector<KFStates> KFVel = { KFStates::VelN, KFStates::VelE, KFStates::VelD };
223 inline static const std::vector<KFStates> KFAtt = { KFStates::Roll, KFStates::Pitch, KFStates::Yaw };
225 inline static const std::vector<KFStates> KFAccBias = { KFStates::AccBiasX, KFStates::AccBiasY, KFStates::AccBiasZ };
227 inline static const std::vector<KFStates> KFGyrBias = { KFStates::GyrBiasX, KFStates::GyrBiasY, KFStates::GyrBiasZ };
228
232
236
240 inline static const std::vector<KFMeas> dPos = { KFMeas::dPosLat, KFMeas::dPosLon, KFMeas::dPosAlt };
242 inline static const std::vector<KFMeas> dVel = { KFMeas::dVelN, KFMeas::dVelE, KFMeas::dVelD };
243
246
247 // #########################################################################################################################################
248 // GUI settings
249 // #########################################################################################################################################
250
253
254 // ###########################################################################################################
255 // Parameters
256 // ###########################################################################################################
257
260
263 Eigen::Vector3d _stdev_ra = 0.04 /* [mg/√(Hz)] */ * Eigen::Vector3d::Ones();
264
265 // ###########################################################################################################
266
269
272 Eigen::Vector3d _stdev_rg = 5 /* [deg/hr/√(Hz)]^2 */ * Eigen::Vector3d::Ones();
273
274 // ###########################################################################################################
275
278
281 Eigen::Vector3d _stdev_bad = 10 /* [µg] */ * Eigen::Vector3d::Ones();
282
284 Eigen::Vector3d _tau_bad = 0.1 * Eigen::Vector3d::Ones();
285
286 // ###########################################################################################################
287
290
293 Eigen::Vector3d _stdev_bgd = 1 /* [°/h] */ * Eigen::Vector3d::Ones();
294
296 Eigen::Vector3d _tau_bgd = 0.1 * Eigen::Vector3d::Ones();
297
298 // ###########################################################################################################
299
301 enum class StdevBaroHeightBiasUnits : uint8_t
302 {
304 };
305
307
309 double _stdevBaroHeightBias = 1e-6;
310
311 // ###########################################################################################################
312
314 enum class StdevBaroHeightScaleUnits : uint8_t
315 {
317 };
318
320
323
324 // ###########################################################################################################
325
327 enum class RandomProcess : uint8_t
328 {
329 // WhiteNoise, ///< White noise
330 // RandomConstant, ///< Random constant
331
334
335 // GaussMarkov2, ///< Gauss-Markov 2nd Order
336 // GaussMarkov3, ///< Gauss-Markov 3rd Order
337 };
338
343
344 // ###########################################################################################################
345
352
354
357 Eigen::Vector3d _gnssMeasurementUncertaintyPosition{ 0.3, 0.3, 0.3 * 3 };
358
361
362 // ###########################################################################################################
363
366 {
369 };
370
372
374 Eigen::Vector3d _gnssMeasurementUncertaintyVelocity{ 0.5, 0.5, 0.5 };
375
378
379 // ###########################################################################################################
380
383 {
386 };
387
389
392
395
396 // ###########################################################################################################
397
406
408
410 Eigen::Vector3d _initCovariancePosition{ 100.0, 100.0, 100.0 };
411
412 // ###########################################################################################################
413
415 enum class InitCovarianceVelocityUnit : uint8_t
416 {
419 };
420
422
424 Eigen::Vector3d _initCovarianceVelocity{ 10.0, 10.0, 10.0 };
425
426 // ###########################################################################################################
427
430 {
435 };
436
438
440 Eigen::Vector3d _initCovarianceAttitudeAngles{ 10.0, 10.0, 10.0 };
441
442 // ###########################################################################################################
443
445 enum class InitCovarianceBiasAccelUnit : uint8_t
446 {
449 };
450
452
454 Eigen::Vector3d _initCovarianceBiasAccel{ 1.0, 1.0, 1.0 };
455
456 // ###########################################################################################################
457
466
468
470 Eigen::Vector3d _initCovarianceBiasGyro{ 0.5, 0.5, 0.5 };
471
472 // ###########################################################################################################
473
475 enum class InitCovarianceBiasHeightUnit : uint8_t
476 {
479 };
480
482
485
486 // ###########################################################################################################
487
489 enum class InitCovarianceScaleHeight : uint8_t
490 {
493 };
494
496
499
500 // ###########################################################################################################
501
503 enum class InitBiasAccelUnit : uint8_t
504 {
507 };
508
510
512 Eigen::Vector3d _initBiasAccel{ 0.0, 0.0, 0.0 };
513
514 // ###########################################################################################################
515
517 enum class InitBiasGyroUnit : uint8_t
518 {
521 };
522
524
526 Eigen::Vector3d _initBiasGyro{ 0.0, 0.0, 0.0 };
527
528 // ###########################################################################################################
529
531 enum class PhiCalculationAlgorithm : uint8_t
532 {
535 };
536
538
541
543 enum class QCalculationAlgorithm : uint8_t
544 {
547 };
548
550
551 // ###########################################################################################################
552 // Prediction
553 // ###########################################################################################################
554
555 // ###########################################################################################################
556 // System matrix 𝐅
557 // ###########################################################################################################
558
572 [[nodiscard]] KeyedMatrix<double, KFStates, KFStates, 17, 17> n_systemMatrix_F(const Eigen::Quaterniond& n_Quat_b,
573 const Eigen::Vector3d& b_specForce_ib,
574 const Eigen::Vector3d& n_omega_in,
575 const Eigen::Vector3d& n_velocity,
576 const Eigen::Vector3d& lla_position,
577 double R_N,
578 double R_E,
579 double g_0,
580 double r_eS_e,
581 const Eigen::Vector3d& tau_bad,
582 const Eigen::Vector3d& tau_bgd) const;
583
594 [[nodiscard]] KeyedMatrix<double, KFStates, KFStates, 17, 17> e_systemMatrix_F(const Eigen::Quaterniond& e_Quat_b,
595 const Eigen::Vector3d& b_specForce_ib,
596 const Eigen::Vector3d& e_position,
597 const Eigen::Vector3d& e_gravitation,
598 double r_eS_e,
599 const Eigen::Vector3d& e_omega_ie,
600 const Eigen::Vector3d& tau_bad,
601 const Eigen::Vector3d& tau_bgd) const;
602
603 // ###########################################################################################################
604 // Noise input matrix 𝐆 & Noise scale matrix 𝐖
605 // System noise covariance matrix 𝐐
606 // ###########################################################################################################
607
611 [[nodiscard]] static KeyedMatrix<double, KFStates, KFStates, 17, 17> noiseInputMatrix_G(const Eigen::Quaterniond& ien_Quat_b);
612
623 [[nodiscard]] Eigen::Matrix<double, 17, 17> noiseScaleMatrix_W(const Eigen::Vector3d& sigma_ra, const Eigen::Vector3d& sigma_rg,
624 const Eigen::Vector3d& sigma_bad, const Eigen::Vector3d& sigma_bgd,
625 const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd,
626 const double& sigma_heightBias, const double& sigma_heightScale);
627
642 [[nodiscard]] static KeyedMatrix<double, KFStates, KFStates, 17, 17> n_systemNoiseCovarianceMatrix_Q(const Eigen::Vector3d& sigma2_ra, const Eigen::Vector3d& sigma2_rg,
643 const Eigen::Vector3d& sigma2_bad, const Eigen::Vector3d& sigma2_bgd,
644 const double& sigma_heightBias, const double& sigma_heightScale,
645 const Eigen::Vector3d& tau_bad, const Eigen::Vector3d& tau_bgd,
646 const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p,
647 const Eigen::Matrix3d& n_Dcm_b, const double& tau_s);
648
662 [[nodiscard]] static KeyedMatrix<double, KFStates, KFStates, 17, 17> e_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& e_F_21,
667 const Eigen::Matrix3d& e_Dcm_b, const double& tau_s);
668
669 // ###########################################################################################################
670 // Error covariance matrix P
671 // ###########################################################################################################
672
682 [[nodiscard]] KeyedMatrix<double, KFStates, KFStates, 17, 17> initialErrorCovarianceMatrix_P0(const Eigen::Vector3d& variance_angles,
683 const Eigen::Vector3d& variance_vel,
684 const Eigen::Vector3d& variance_pos,
685 const Eigen::Vector3d& variance_accelBias,
686 const Eigen::Vector3d& variance_gyroBias,
687 const double& variance_heightBias,
688 const double& variance_heightScale) const;
689
690 // ###########################################################################################################
691 // Correction
692 // ###########################################################################################################
693
701 [[nodiscard]] static KeyedMatrix<double, KFMeas, KFStates, 6, 17> n_measurementMatrix_H(const Eigen::Matrix3d& T_rn_p,
702 const Eigen::Matrix3d& n_Dcm_b,
703 const Eigen::Vector3d& b_omega_ib,
704 const Eigen::Vector3d& b_leverArm_InsGnss,
705 const Eigen::Matrix3d& n_Omega_ie);
706
711 [[nodiscard]] static KeyedMatrix<double, KFMeas, KFStates, 1, 17> n_measurementMatrix_H(const double& height, const double& scale);
712
719 [[nodiscard]] static KeyedMatrix<double, KFMeas, KFStates, 6, 17> e_measurementMatrix_H(const Eigen::Matrix3d& e_Dcm_b,
720 const Eigen::Vector3d& b_omega_ib,
721 const Eigen::Vector3d& b_leverArm_InsGnss,
722 const Eigen::Matrix3d& e_Omega_ie);
723
729 [[nodiscard]] static KeyedMatrix<double, KFMeas, KFStates, 1, 17> e_measurementMatrix_H(const Eigen::Vector3d& e_positionEstimate,
730 const double& height,
731 const double& scale);
732
739 [[nodiscard]] KeyedMatrix<double, KFMeas, KFMeas, 6, 6> n_measurementNoiseCovariance_R(const std::shared_ptr<const PosVel>& posVelObs,
740 const Eigen::Vector3d& lla_position,
741 double R_N,
742 double R_E) const;
743
747 [[nodiscard]] static KeyedMatrix<double, KFMeas, KFMeas, 1, 1> n_measurementNoiseCovariance_R(const double& baroVarianceHeight);
748
752 [[nodiscard]] KeyedMatrix<double, KFMeas, KFMeas, 6, 6> e_measurementNoiseCovariance_R(const std::shared_ptr<const PosVel>& posVelObs) const;
753
765 [[nodiscard]] static KeyedVector<double, KFMeas, 6> n_measurementInnovation_dz(const Eigen::Vector3d& lla_positionMeasurement, const Eigen::Vector3d& lla_positionEstimate,
766 const Eigen::Vector3d& n_velocityMeasurement, const Eigen::Vector3d& n_velocityEstimate,
767 const Eigen::Matrix3d& T_rn_p, const Eigen::Quaterniond& n_Quat_b, const Eigen::Vector3d& b_leverArm_InsGnss,
768 const Eigen::Vector3d& b_omega_ib, const Eigen::Matrix3d& n_Omega_ie);
769
776 [[nodiscard]] static KeyedVector<double, KFMeas, 1> n_measurementInnovation_dz(const double& baroheight, const double& height,
777 const double& heightbias, const double& heightscale);
778
789 [[nodiscard]] static KeyedVector<double, KFMeas, 6> e_measurementInnovation_dz(const Eigen::Vector3d& e_positionMeasurement, const Eigen::Vector3d& e_positionEstimate,
790 const Eigen::Vector3d& e_velocityMeasurement, const Eigen::Vector3d& e_velocityEstimate,
791 const Eigen::Quaterniond& e_Quat_b, const Eigen::Vector3d& b_leverArm_InsGnss,
792 const Eigen::Vector3d& b_omega_ib, const Eigen::Matrix3d& e_Omega_ie);
793};
794
795} // namespace NAV
796
797#ifndef DOXYGEN_IGNORE
798
799template<>
800struct fmt::formatter<NAV::LooselyCoupledKF::KFStates> : fmt::formatter<const char*>
801{
806 template<typename FormatContext>
807 auto format(const NAV::LooselyCoupledKF::KFStates& st, FormatContext& ctx) const
808 {
809 switch (st)
810 {
812 return fmt::formatter<const char*>::format("Roll/Psi_eb_1", ctx);
814 return fmt::formatter<const char*>::format("Pitch/Psi_eb_2", ctx);
816 return fmt::formatter<const char*>::format("Yaw/Psi_eb_3", ctx);
818 return fmt::formatter<const char*>::format("VelN/VelX", ctx);
820 return fmt::formatter<const char*>::format("VelE/VelY", ctx);
822 return fmt::formatter<const char*>::format("VelD/VelZ", ctx);
824 return fmt::formatter<const char*>::format("PosLat/PosX", ctx);
826 return fmt::formatter<const char*>::format("PosLon/PosY", ctx);
828 return fmt::formatter<const char*>::format("PosAlt/PosZ", ctx);
830 return fmt::formatter<const char*>::format("AccBiasX", ctx);
832 return fmt::formatter<const char*>::format("AccBiasY", ctx);
834 return fmt::formatter<const char*>::format("AccBiasZ", ctx);
836 return fmt::formatter<const char*>::format("GyrBiasX", ctx);
838 return fmt::formatter<const char*>::format("GyrBiasY", ctx);
840 return fmt::formatter<const char*>::format("GyrBiasZ", ctx);
842 return fmt::formatter<const char*>::format("HeightBias", ctx);
844 return fmt::formatter<const char*>::format("HeightScale", ctx);
846 return fmt::formatter<const char*>::format("COUNT", ctx);
847 }
848
849 return fmt::formatter<const char*>::format("ERROR", ctx);
850 }
851};
852template<>
853struct fmt::formatter<NAV::LooselyCoupledKF::KFMeas> : fmt::formatter<const char*>
854{
859 template<typename FormatContext>
860 auto format(const NAV::LooselyCoupledKF::KFMeas& st, FormatContext& ctx) const
861 {
862 switch (st)
863 {
865 return fmt::formatter<const char*>::format("dPosLat/dPosX", ctx);
867 return fmt::formatter<const char*>::format("dPosLon/dPosY", ctx);
869 return fmt::formatter<const char*>::format("dPosAlt/dPosZ", ctx);
871 return fmt::formatter<const char*>::format("dVelN/dVelX", ctx);
873 return fmt::formatter<const char*>::format("dVelE/dVelY", ctx);
875 return fmt::formatter<const char*>::format("dVelD/dVelZ", ctx);
877 return fmt::formatter<const char*>::format("dHgt", ctx);
878 }
879
880 return fmt::formatter<const char*>::format("ERROR", ctx);
881 }
882};
883
884#endif
885
890std::ostream& operator<<(std::ostream& os, const NAV::LooselyCoupledKF::KFStates& obj);
891
896std::ostream& operator<<(std::ostream& os, const NAV::LooselyCoupledKF::KFMeas& obj);
Barometric Height Storage Class.
nlohmann::json json
json namespace
Definition FlowManager.hpp:21
Units used by INS.
ImuAccelerometerFilterNoiseUnits
Possible units to specify an accelerometer noise in a filter.
Definition Units.hpp:84
@ mg_sqrtHz
[mg / √(Hz)]
Definition Units.hpp:86
ImuAccelerometerFilterBiasUnits
Possible units for the accelerometer dynamic bias.
Definition Units.hpp:102
@ microg
[µg]
Definition Units.hpp:104
ImuGyroscopeFilterBiasUnits
Possible units for the gyroscope dynamic bias.
Definition Units.hpp:110
@ deg_h
[°/h]
Definition Units.hpp:114
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
Parent Class for all IMU Observations.
Inertial Measurement Integrator.
The class is responsible for all time-related tasks.
Kalman Filter with keyed states.
KeyedKalmanFilter< double, StateKeyType, MeasKeyType > KeyedKalmanFilterD
Keyed Kalman Filter class with double as type.
Definition KeyedKalmanFilter.hpp:584
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.
Definition InertialIntegrator.hpp:40
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.
Definition KeyedMatrix.hpp:1921
Static sized KeyedVector.
Definition KeyedMatrix.hpp:1468
InitCovarianceScaleHeight _initCovarianceScaleHeightUnit
Gui selection for the Unit of the initial covariance for the height scale.
Definition LooselyCoupledKF.hpp:495
InitCovarianceVelocityUnit
Possible Units for the initial covariance for the velocity (standard deviation σ or Variance σ²)
Definition LooselyCoupledKF.hpp:416
@ m_s
Standard deviation [m/s].
Definition LooselyCoupledKF.hpp:418
KFStates
State Keys of the Kalman filter.
Definition LooselyCoupledKF.hpp:70
@ Pitch
Pitch.
Definition LooselyCoupledKF.hpp:72
@ VelY
ECEF Velocity Y.
Definition LooselyCoupledKF.hpp:94
@ HeightScale
Baro Height Scale.
Definition LooselyCoupledKF.hpp:87
@ GyrBiasX
Gyroscope Bias X.
Definition LooselyCoupledKF.hpp:83
@ PosLat
Latitude.
Definition LooselyCoupledKF.hpp:77
@ HeightBias
Baro Height Bias.
Definition LooselyCoupledKF.hpp:86
@ VelE
Velocity East.
Definition LooselyCoupledKF.hpp:75
@ Roll
Roll.
Definition LooselyCoupledKF.hpp:71
@ Psi_eb_3
Angle between Earth and Body frame around 3. axis.
Definition LooselyCoupledKF.hpp:92
@ AccBiasZ
Accelerometer Bias Z.
Definition LooselyCoupledKF.hpp:82
@ VelX
ECEF Velocity X.
Definition LooselyCoupledKF.hpp:93
@ PosAlt
Altitude.
Definition LooselyCoupledKF.hpp:79
@ GyrBiasZ
Gyroscope Bias Z.
Definition LooselyCoupledKF.hpp:85
@ GyrBiasY
Gyroscope Bias Y.
Definition LooselyCoupledKF.hpp:84
@ VelD
Velocity Down.
Definition LooselyCoupledKF.hpp:76
@ AccBiasX
Accelerometer Bias X.
Definition LooselyCoupledKF.hpp:80
@ VelN
Velocity North.
Definition LooselyCoupledKF.hpp:74
@ PosLon
Longitude.
Definition LooselyCoupledKF.hpp:78
@ VelZ
ECEF Velocity Z.
Definition LooselyCoupledKF.hpp:95
@ PosX
ECEF Position X.
Definition LooselyCoupledKF.hpp:96
@ Yaw
Yaw.
Definition LooselyCoupledKF.hpp:73
@ AccBiasY
Accelerometer Bias Y.
Definition LooselyCoupledKF.hpp:81
@ KFStates_COUNT
Amount of states.
Definition LooselyCoupledKF.hpp:88
@ PosZ
ECEF Position Z.
Definition LooselyCoupledKF.hpp:98
@ PosY
ECEF Position Y.
Definition LooselyCoupledKF.hpp:97
@ Psi_eb_2
Angle between Earth and Body frame around 2. axis.
Definition LooselyCoupledKF.hpp:91
@ Psi_eb_1
Angle between Earth and Body frame around 1. axis.
Definition LooselyCoupledKF.hpp:90
Units::ImuAccelerometerFilterNoiseUnits _stdevAccelNoiseUnits
Gui selection for the Unit of the input stdev_ra parameter.
Definition LooselyCoupledKF.hpp:259
StdevBaroHeightScaleUnits _stdevBaroHeightScaleUnits
Gui selection for the Unit of the barometric height scale uncertainty.
Definition LooselyCoupledKF.hpp:319
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.
static KeyedMatrix< double, KFStates, KFStates, 17, 17 > noiseInputMatrix_G(const Eigen::Quaterniond &ien_Quat_b)
Calculates the noise input matrix 𝐆
RandomProcess _randomProcessAccel
Random Process used to estimate the accelerometer biases.
Definition LooselyCoupledKF.hpp:340
json save() const override
Saves the node into a json object.
InitBiasAccelUnit _initBiasAccelUnit
Gui selection for the unit of the initial accelerometer biases.
Definition LooselyCoupledKF.hpp:509
InitCovarianceVelocityUnit _initCovarianceVelocityUnit
Gui selection for the Unit of the initial covariance for the velocity.
Definition LooselyCoupledKF.hpp:421
bool _preferAccelerationOverDeltaMeasurements
Prefer the raw acceleration measurements over the deltaVel & deltaTheta values.
Definition LooselyCoupledKF.hpp:182
Eigen::Vector3d _initBiasAccel
GUI selection of the initial accelerometer biases.
Definition LooselyCoupledKF.hpp:512
InitCovarianceAttitudeAnglesUnit
Possible Units for the initial covariance for the attitude angles (standard deviation σ or Variance σ...
Definition LooselyCoupledKF.hpp:430
@ rad
Standard deviation [rad].
Definition LooselyCoupledKF.hpp:433
@ deg
Standard deviation [deg].
Definition LooselyCoupledKF.hpp:434
@ rad2
Variance [rad^2].
Definition LooselyCoupledKF.hpp:431
@ deg2
Variance [deg^2].
Definition LooselyCoupledKF.hpp:432
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 _gnssMeasurementUncertaintyVelocity
GUI selection of the GNSS NED velocity measurement uncertainty (standard deviation σ or Variance σ²)
Definition LooselyCoupledKF.hpp:374
LooselyCoupledKF & operator=(const LooselyCoupledKF &)=delete
Copy assignment operator.
StdevBaroHeightBiasUnits _stdevBaroHeightBiasUnits
Gui selection for the Unit of the barometric height bias uncertainty.
Definition LooselyCoupledKF.hpp:306
static KeyedVector< double, KFMeas, 1 > n_measurementInnovation_dz(const double &baroheight, const double &height, const double &heightbias, const double &heightscale)
Measurement innovation vector 𝜹𝐳
PhiCalculationAlgorithm _phiCalculationAlgorithm
GUI option for the Phi calculation algorithm.
Definition LooselyCoupledKF.hpp:537
int _phiCalculationTaylorOrder
GUI option for the order of the Taylor polynom to calculate the Phi matrix.
Definition LooselyCoupledKF.hpp:540
Eigen::Vector3d _initCovariancePosition
GUI selection of the initial covariance diagonal values for position (standard deviation σ or Varianc...
Definition LooselyCoupledKF.hpp:410
void looselyCoupledUpdate(const std::shared_ptr< const BaroHgt > &baroHgtObs)
Updates the predicted state from the InertialNavSol with the Baro observation.
InitBiasGyroUnit
Possible Units for the initial gyroscope biases.
Definition LooselyCoupledKF.hpp:518
@ deg_s
angular rate [deg/s]
Definition LooselyCoupledKF.hpp:520
static const std::vector< KFMeas > dPos
All position difference keys.
Definition LooselyCoupledKF.hpp:240
void updateInputPins()
Add or remove input pins for external PVA init and Baro.
static const std::vector< KFMeas > Meas
Vector with all measurement keys.
Definition LooselyCoupledKF.hpp:238
double _heightScaleTotal
Accumulator for height scale [m/m].
Definition LooselyCoupledKF.hpp:190
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.
Definition LooselyCoupledKF.hpp:225
GnssMeasurementUncertaintyVelocityUnit _gnssMeasurementUncertaintyVelocityUnit
Gui selection for the Unit of the GNSS measurement uncertainty for the velocity.
Definition LooselyCoupledKF.hpp:371
KeyedMatrix< double, KFMeas, KFMeas, 6, 6 > e_measurementNoiseCovariance_R(const std::shared_ptr< const PosVel > &posVelObs) const
Measurement noise covariance matrix 𝐑
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 𝐑
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...
Definition LooselyCoupledKF.hpp:424
Eigen::Vector3d _initCovarianceBiasGyro
GUI selection of the initial covariance diagonal values for gyroscope biases (standard deviation σ or...
Definition LooselyCoupledKF.hpp:470
LooselyCoupledKF & operator=(LooselyCoupledKF &&)=delete
Move assignment operator.
bool _gnssMeasurementUncertaintyPositionOverride
Whether to override the position uncertainty or use the one included in the measurement.
Definition LooselyCoupledKF.hpp:360
Units::ImuGyroscopeFilterNoiseUnits _stdevGyroNoiseUnits
Gui selection for the Unit of the input stdev_rg parameter.
Definition LooselyCoupledKF.hpp:268
Eigen::Vector3d _gnssMeasurementUncertaintyPosition
GUI selection of the GNSS position measurement uncertainty (standard deviation σ or Variance σ²)....
Definition LooselyCoupledKF.hpp:357
static constexpr size_t INPUT_PORT_INDEX_POS_VEL_ATT_INIT
Flow (PosVelAtt)
Definition LooselyCoupledKF.hpp:123
~LooselyCoupledKF() override
Destructor.
QCalculationAlgorithm _qCalculationAlgorithm
GUI option for the Q calculation algorithm.
Definition LooselyCoupledKF.hpp:549
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.
InitCovariancePositionUnit
Possible Units for the initial covariance for the position (standard deviation σ or Variance σ²)
Definition LooselyCoupledKF.hpp:400
@ rad_rad_m
Standard deviation LatLonAlt [rad, rad, m].
Definition LooselyCoupledKF.hpp:402
@ rad2_rad2_m2
Variance LatLonAlt^2 [rad^2, rad^2, m^2].
Definition LooselyCoupledKF.hpp:401
@ meter
Standard deviation NED [m, m, m].
Definition LooselyCoupledKF.hpp:404
static const std::vector< KFStates > KFVel
All velocity keys.
Definition LooselyCoupledKF.hpp:221
bool _initializeStateOverExternalPin
Whether to initialize the state over an external pin.
Definition LooselyCoupledKF.hpp:200
Eigen::Vector3d _tau_bgd
Correlation length of the gyro dynamic bias in [s].
Definition LooselyCoupledKF.hpp:296
static KeyedMatrix< double, KFMeas, KFMeas, 1, 1 > n_measurementNoiseCovariance_R(const double &baroVarianceHeight)
Measurement noise covariance matrix 𝐑
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.
QCalculationAlgorithm
GUI option for the Q calculation algorithm.
Definition LooselyCoupledKF.hpp:544
@ Taylor1
Taylor.
Definition LooselyCoupledKF.hpp:546
@ VanLoan
Van-Loan.
Definition LooselyCoupledKF.hpp:545
double _initCovarianceBiasHeight
GUI selection of the initial covariance diagonal values for the height bias (standard deviation σ or ...
Definition LooselyCoupledKF.hpp:484
StdevBaroHeightBiasUnits
Possible Units for the Variance of the barometric height bias.
Definition LooselyCoupledKF.hpp:302
@ m
[m]
Definition LooselyCoupledKF.hpp:303
InitCovarianceScaleHeight
Possible Units for the initial covariance for the height scale (standard deviation σ or Variance σ²)
Definition LooselyCoupledKF.hpp:490
@ m2_m2
Variance [m²/m²].
Definition LooselyCoupledKF.hpp:491
bool _checkKalmanMatricesRanks
Check the rank of the Kalman matrices every iteration (computational expensive)
Definition LooselyCoupledKF.hpp:252
Units::ImuAccelerometerFilterBiasUnits _stdevAccelBiasUnits
Gui selection for the Unit of the input variance_bad parameter.
Definition LooselyCoupledKF.hpp:277
GnssMeasurementUncertaintyPositionUnit _gnssMeasurementUncertaintyPositionUnit
Gui selection for the Unit of the GNSS measurement uncertainty for the position.
Definition LooselyCoupledKF.hpp:353
double _stdevBaroHeightBias
Uncertainty of the barometric height bias.
Definition LooselyCoupledKF.hpp:309
static const std::vector< KFStates > KFAtt
All attitude keys.
Definition LooselyCoupledKF.hpp:223
KeyedKalmanFilterD< KFStates, KFMeas > _kalmanFilter
Kalman Filter representation.
Definition LooselyCoupledKF.hpp:245
InitBiasGyroUnit _initBiasGyroUnit
Gui selection for the unit of the initial gyroscope biases.
Definition LooselyCoupledKF.hpp:523
double _initCovarianceScaleHeight
GUI selection of the initial covariance diagonal values for the height scale (standard deviation σ or...
Definition LooselyCoupledKF.hpp:498
static KeyedMatrix< double, KFMeas, KFStates, 1, 17 > n_measurementMatrix_H(const double &height, const double &scale)
Measurement matrix for baro height measurements at timestep k, represented in navigation coordinates.
static const std::vector< KFMeas > dVel
All velocity difference keys.
Definition LooselyCoupledKF.hpp:242
LooselyCoupledKF(LooselyCoupledKF &&)=delete
Move constructor.
Eigen::Vector3d _accelBiasTotal
Accumulator for acceleration bias [m/s²].
Definition LooselyCoupledKF.hpp:193
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)
Definition LooselyCoupledKF.hpp:185
PhiCalculationAlgorithm
GUI option for the Phi calculation algorithm.
Definition LooselyCoupledKF.hpp:532
@ Taylor
Taylor.
Definition LooselyCoupledKF.hpp:534
@ Exponential
Van-Loan.
Definition LooselyCoupledKF.hpp:533
static const std::vector< KFStates > States
Vector with all state keys.
Definition LooselyCoupledKF.hpp:212
InitCovarianceBiasAccelUnit _initCovarianceBiasAccelUnit
Gui selection for the Unit of the initial covariance for the accelerometer biases.
Definition LooselyCoupledKF.hpp:451
InitCovariancePositionUnit _initCovariancePositionUnit
Gui selection for the Unit of the initial covariance for the position.
Definition LooselyCoupledKF.hpp:407
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}.
Eigen::Vector3d _stdev_rg
𝜎_rg Standard deviation of the noise on the gyro angular-rate measurements
Definition LooselyCoupledKF.hpp:272
static const std::vector< KFStates > KFPosVelAtt
All position, velocity and attitude keys.
Definition LooselyCoupledKF.hpp:233
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 ...
Definition LooselyCoupledKF.hpp:460
@ rad2_s2
Variance [rad²/s²].
Definition LooselyCoupledKF.hpp:461
@ deg2_s2
Variance [deg²/s²].
Definition LooselyCoupledKF.hpp:462
@ deg_s
Standard deviation [deg/s].
Definition LooselyCoupledKF.hpp:464
@ rad_s
Standard deviation [rad/s].
Definition LooselyCoupledKF.hpp:463
InsTime _externalInitTime
Time from the external init.
Definition LooselyCoupledKF.hpp:206
static constexpr size_t OUTPUT_PORT_INDEX_SOLUTION
Flow (InsGnssLCKFSolution)
Definition LooselyCoupledKF.hpp:124
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 𝐖
static constexpr size_t INPUT_PORT_INDEX_IMU
Flow (ImuObs)
Definition LooselyCoupledKF.hpp:121
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)
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 _stdev_bad
𝜎²_bad Variance of the accelerometer dynamic bias
Definition LooselyCoupledKF.hpp:281
InertialIntegrator _inertialIntegrator
Inertial Integrator.
Definition LooselyCoupledKF.hpp:180
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)
void looselyCoupledPrediction(const std::shared_ptr< const PosVelAtt > &inertialNavSol, double tau_i, const ImuPos &imuPos)
Predicts the state from the InertialNavSol.
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 𝜹𝐳
KFMeas
Measurement Keys of the Kalman filter.
Definition LooselyCoupledKF.hpp:103
@ dPosLat
Latitude difference.
Definition LooselyCoupledKF.hpp:104
@ dPosLon
Longitude difference.
Definition LooselyCoupledKF.hpp:105
@ dPosY
ECEF Position Y difference.
Definition LooselyCoupledKF.hpp:113
@ dPosX
ECEF Position X difference.
Definition LooselyCoupledKF.hpp:112
@ dVelE
Velocity East difference.
Definition LooselyCoupledKF.hpp:108
@ dPosZ
ECEF Position Z difference.
Definition LooselyCoupledKF.hpp:114
@ dPosAlt
Altitude difference.
Definition LooselyCoupledKF.hpp:106
@ dHgt
height difference
Definition LooselyCoupledKF.hpp:110
@ dVelN
Velocity North difference.
Definition LooselyCoupledKF.hpp:107
@ dVelZ
ECEF Velocity Z difference.
Definition LooselyCoupledKF.hpp:117
@ dVelX
ECEF Velocity X difference.
Definition LooselyCoupledKF.hpp:115
@ dVelY
ECEF Velocity Y difference.
Definition LooselyCoupledKF.hpp:116
@ dVelD
Velocity Down difference.
Definition LooselyCoupledKF.hpp:109
static const std::vector< KFStates > KFPosVel
All position and velocity keys.
Definition LooselyCoupledKF.hpp:230
InitBiasAccelUnit
Possible Units for the initial accelerometer biases.
Definition LooselyCoupledKF.hpp:504
@ microg
[µg]
Definition LooselyCoupledKF.hpp:505
@ m_s2
acceleration [m/s^2]
Definition LooselyCoupledKF.hpp:506
BaroHeightMeasurementUncertaintyUnit _barometricHeightMeasurementUncertaintyUnit
Gui selection for the Unit of the barometric height measurement uncertainty.
Definition LooselyCoupledKF.hpp:388
double _stdevBaroHeightScale
Uncertainty of the barometric height scale.
Definition LooselyCoupledKF.hpp:322
InitCovarianceBiasAccelUnit
Possible Units for the initial covariance for the accelerometer biases (standard deviation σ or Varia...
Definition LooselyCoupledKF.hpp:446
@ m2_s4
Variance [m^2/s^4].
Definition LooselyCoupledKF.hpp:447
@ m_s2
Standard deviation [m/s^2].
Definition LooselyCoupledKF.hpp:448
double _heightBiasTotal
Accumulator for height bias [m].
Definition LooselyCoupledKF.hpp:188
StdevBaroHeightScaleUnits
Possible Units for the Variance of the barometric height scale.
Definition LooselyCoupledKF.hpp:315
@ m_m
[m/m]
Definition LooselyCoupledKF.hpp:316
static std::string typeStatic()
String representation of the class type.
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.
Definition LooselyCoupledKF.hpp:394
bool _gnssMeasurementUncertaintyVelocityOverride
Whether to override the velocity uncertainty or use the one included in the measurement.
Definition LooselyCoupledKF.hpp:377
Units::ImuGyroscopeFilterBiasUnits _stdevGyroBiasUnits
Gui selection for the Unit of the input variance_bad parameter.
Definition LooselyCoupledKF.hpp:289
static const std::vector< KFStates > KFPos
All position keys.
Definition LooselyCoupledKF.hpp:219
BaroHeightMeasurementUncertaintyUnit
Possible Units for the barometric height measurement uncertainty (standard deviation σ or Variance σ²...
Definition LooselyCoupledKF.hpp:383
@ m
Standard deviation [m].
Definition LooselyCoupledKF.hpp:385
@ m2
Variance [m²].
Definition LooselyCoupledKF.hpp:384
Eigen::Vector3d _stdev_ra
𝜎_ra Standard deviation of the noise on the accelerometer specific-force measurements
Definition LooselyCoupledKF.hpp:263
Eigen::Vector3d _tau_bad
Correlation length of the accelerometer dynamic bias in [s].
Definition LooselyCoupledKF.hpp:284
InitCovarianceBiasGyroUnit _initCovarianceBiasGyroUnit
Gui selection for the Unit of the initial covariance for the gyroscope biases.
Definition LooselyCoupledKF.hpp:467
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.
std::string type() const override
String representation of the class type.
static const std::vector< KFStates > KFGyrBias
All gyroscope bias keys.
Definition LooselyCoupledKF.hpp:227
GnssMeasurementUncertaintyPositionUnit
Possible Units for the GNSS measurement uncertainty for the position (standard deviation σ or Varianc...
Definition LooselyCoupledKF.hpp:348
@ meter2
Variance [m^2, m^2, m^2].
Definition LooselyCoupledKF.hpp:349
@ meter
Standard deviation [m, m, m].
Definition LooselyCoupledKF.hpp:350
static KeyedMatrix< double, KFMeas, KFStates, 1, 17 > e_measurementMatrix_H(const Eigen::Vector3d &e_positionEstimate, const double &height, const double &scale)
Measurement matrix for barometric height measurements at timestep k, represented in Earth frame coord...
std::array< double, 3 > _initalRollPitchYaw
Roll, Pitch and Yaw angles in [deg] used for initialization if not taken from separate pin.
Definition LooselyCoupledKF.hpp:198
LooselyCoupledKF()
Default constructor.
static constexpr size_t INPUT_PORT_INDEX_GNSS
Flow (PosVel)
Definition LooselyCoupledKF.hpp:122
RandomProcess
Available Random processes.
Definition LooselyCoupledKF.hpp:328
@ GaussMarkov1
Gauss-Markov 1st Order.
Definition LooselyCoupledKF.hpp:333
@ RandomWalk
Random Walk.
Definition LooselyCoupledKF.hpp:332
double _barometricHeightMeasurementUncertainty
GUI selection of the barometric height measurement uncertainty (standard deviation σ or Variance σ²)
Definition LooselyCoupledKF.hpp:391
Eigen::Vector3d _gyroBiasTotal
Accumulator gyro bias [rad/s].
Definition LooselyCoupledKF.hpp:195
bool _initialSensorBiasesApplied
Whether the accumulated biases have been initialized in the 'inertialIntegrator'.
Definition LooselyCoupledKF.hpp:209
RandomProcess _randomProcessGyro
Random Process used to estimate the gyroscope biases.
Definition LooselyCoupledKF.hpp:342
Eigen::Vector3d _initCovarianceBiasAccel
GUI selection of the initial covariance diagonal values for accelerometer biases (standard deviation ...
Definition LooselyCoupledKF.hpp:454
InitCovarianceBiasHeightUnit
Possible Units for the initial covariance for the height bias (standard deviation σ or Variance σ²)
Definition LooselyCoupledKF.hpp:476
@ m2
Variance [m²].
Definition LooselyCoupledKF.hpp:477
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)
Definition LooselyCoupledKF.hpp:203
Eigen::Vector3d _initCovarianceAttitudeAngles
GUI selection of the initial covariance diagonal values for attitude angles (standard deviation σ or ...
Definition LooselyCoupledKF.hpp:440
GnssMeasurementUncertaintyVelocityUnit
Possible Units for the GNSS measurement uncertainty for the velocity (standard deviation σ or Varianc...
Definition LooselyCoupledKF.hpp:366
@ m_s
Standard deviation [m/s].
Definition LooselyCoupledKF.hpp:368
@ m2_s2
Variance [m^2/s^2].
Definition LooselyCoupledKF.hpp:367
Eigen::Vector3d _stdev_bgd
𝜎²_bgd Variance of the gyro dynamic bias
Definition LooselyCoupledKF.hpp:293
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}.
static std::string category()
String representation of the class category.
Eigen::Vector3d _initBiasGyro
GUI selection of the initial gyroscope biases.
Definition LooselyCoupledKF.hpp:526
InitCovarianceAttitudeAnglesUnit _initCovarianceAttitudeAnglesUnit
Gui selection for the Unit of the initial covariance for the attitude angles.
Definition LooselyCoupledKF.hpp:437
InitCovarianceBiasHeightUnit _initCovarianceBiasHeightUnit
Gui selection for the Unit of the initial covariance for the height bias.
Definition LooselyCoupledKF.hpp:481
Node(std::string name)
Constructor.
Position, Velocity and Attitude Storage Class.
Definition PosVelAtt.hpp:25