52 [[nodiscard]] std::string
type()
const override;
263 Eigen::Vector3d
_stdev_ra = 0.04 * Eigen::Vector3d::Ones();
272 Eigen::Vector3d
_stdev_rg = 5 * Eigen::Vector3d::Ones();
284 Eigen::Vector3d
_tau_bad = 0.1 * Eigen::Vector3d::Ones();
296 Eigen::Vector3d
_tau_bgd = 0.1 * Eigen::Vector3d::Ones();
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,
581 const Eigen::Vector3d& tau_bad,
582 const Eigen::Vector3d& tau_bgd)
const;
595 const Eigen::Vector3d& b_specForce_ib,
596 const Eigen::Vector3d& e_position,
597 const Eigen::Vector3d& e_gravitation,
599 const Eigen::Vector3d& e_omega_ie,
600 const Eigen::Vector3d& tau_bad,
601 const Eigen::Vector3d& tau_bgd)
const;
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);
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);
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);
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;
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);
720 const Eigen::Vector3d& b_omega_ib,
721 const Eigen::Vector3d& b_leverArm_InsGnss,
722 const Eigen::Matrix3d& e_Omega_ie);
730 const double& height,
731 const double& scale);
740 const Eigen::Vector3d& lla_position,
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);
777 const double& heightbias,
const double& heightscale);
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);
797#ifndef DOXYGEN_IGNORE
800struct fmt::formatter<NAV::LooselyCoupledKF::KFStates> : fmt::formatter<const char*>
806 template<
typename FormatContext>
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);
849 return fmt::formatter<const char*>::format(
"ERROR", ctx);
853struct fmt::formatter<NAV::LooselyCoupledKF::KFMeas> : fmt::formatter<const char*>
859 template<
typename FormatContext>
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);
880 return fmt::formatter<const char*>::format(
"ERROR", ctx);
Barometric Height Storage Class.
nlohmann::json json
json namespace
Definition FlowManager.hpp:21
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.
Position, Velocity and Attitude Storage Class.
IMU Position.
Definition ImuPos.hpp:26
Inertial Measurement Integrator.
Definition InertialIntegrator.hpp:40
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