53 [[nodiscard]] std::string
type()
const override;
56 [[nodiscard]]
static std::string
category();
63 [[nodiscard]]
json save()
const override;
264 Eigen::Vector3d
_stdev_ra = 0.04 * Eigen::Vector3d::Ones();
273 Eigen::Vector3d
_stdev_rg = 5 * Eigen::Vector3d::Ones();
285 Eigen::Vector3d
_tau_bad = 0.1 * Eigen::Vector3d::Ones();
297 Eigen::Vector3d
_tau_bgd = 0.1 * Eigen::Vector3d::Ones();
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,
601 const Eigen::Vector3d& tau_bad,
602 const Eigen::Vector3d& tau_bgd)
const;
615 const Eigen::Vector3d& b_specForce_ib,
616 const Eigen::Vector3d& e_position,
617 const Eigen::Vector3d& e_gravitation,
619 const Eigen::Vector3d& e_omega_ie,
620 const Eigen::Vector3d& tau_bad,
621 const Eigen::Vector3d& tau_bgd)
const;
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);
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);
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);
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;
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);
740 const Eigen::Vector3d& b_omega_ib,
741 const Eigen::Vector3d& b_leverArm_InsGnss,
742 const Eigen::Matrix3d& e_Omega_ie);
750 const double& height,
751 const double& scale);
760 const Eigen::Vector3d& lla_position,
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);
797 const double& heightbias,
const double& heightscale);
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);
817#ifndef DOXYGEN_IGNORE
820struct fmt::formatter<
NAV::LooselyCoupledKF::KFStates> : fmt::formatter<const char*>
826 template<
typename FormatContext>
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);
869 return fmt::formatter<const char*>::format(
"ERROR", ctx);
873struct fmt::formatter<
NAV::LooselyCoupledKF::KFMeas> : fmt::formatter<const char*>
879 template<
typename FormatContext>
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);
900 return fmt::formatter<const char*>::format(
"ERROR", ctx);
Barometric Height Storage Class.
nlohmann::json json
json namespace
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.
Position, Velocity and Attitude Storage Class.
Inertial Measurement Integrator.
The class is responsible for all time-related tasks.
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 σ²)
@ m_s
Standard deviation [m/s].
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 σ...
@ rad
Standard deviation [rad].
@ deg
Standard deviation [deg].
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.
@ deg_s
angular rate [deg/s]
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].
@ meter
Standard deviation NED [m, m, m].
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 ...
@ rad2_s2
Variance [rad²/s²].
@ deg2_s2
Variance [deg²/s²].
@ deg_s
Standard deviation [deg/s].
@ rad_s
Standard deviation [rad/s].
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.
@ 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.
@ m_s2
acceleration [m/s^2]
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...
@ m2_s4
Variance [m^2/s^4].
@ m_s2
Standard deviation [m/s^2].
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 σ²...
@ m
Standard deviation [m].
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...
@ meter2
Variance [m^2, m^2, m^2].
@ meter
Standard deviation [m, m, m].
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.
@ GaussMarkov1
Gauss-Markov 1st Order.
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...
@ m_s
Standard deviation [m/s].
@ m2_s2
Variance [m^2/s^2].
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.
Position, Velocity and Attitude Storage Class.
ImuAccelerometerFilterNoiseUnits
Possible units to specify an accelerometer noise in a filter.
ImuAccelerometerFilterBiasUnits
Possible units for the accelerometer dynamic bias.
ImuGyroscopeFilterBiasUnits
Possible units for the gyroscope dynamic bias.
ImuGyroscopeFilterNoiseUnits
Possible units to specify an gyro noise in a filter.
@ deg_hr_sqrtHz
[deg / hr /√(Hz)]
KeyedKalmanFilter< double, StateKeyType, MeasKeyType > KeyedKalmanFilterD
Keyed Kalman Filter class with double as type.