Kalman filter matrices for the Integrated-Random-Walk (IRW) Multi-IMU fusion. More...

Functions
Eigen::MatrixXd	NAV::IRWKF::designMatrix_H (const Eigen::Matrix3d &DCM_accel, const Eigen::Matrix3d &DCM_gyro, const size_t &pinIndex, const uint8_t &numMeasurements, const uint8_t &numStates, const uint8_t &numStatesEst, const uint8_t &numStatesPerPin)
	Calculates the design matrix H.

Eigen::MatrixXd	NAV::IRWKF::initialErrorCovarianceMatrix_P0 (const std::vector< Eigen::Vector3d > &initCovariances, const uint8_t &numStates, const bool &imuCharacteristicsIdentical=false)
	Initial error covariance matrix P_0.

KalmanFilter	NAV::IRWKF::initializeKalmanFilterAuto (const size_t &nInputPins, const std::vector< PinData > &pinData, const PinDataIRWKF &pinDataIRWKF, const std::vector< size_t > &cumulatedPinIds, const std::vector< std::shared_ptr< const NAV::ImuObs > > &cumulatedImuObs, const bool &initJerkAngAcc, const double &dtInit, const uint8_t &numStates, const uint8_t &numMeasurements, std::vector< Eigen::VectorXd > &processNoiseVariances, KalmanFilter &kalmanFilter)
	Initializes the IRWKF automatically, i.e. init values are calculated by averaging the data in the first T seconds.

KalmanFilter	NAV::IRWKF::initializeKalmanFilterManually (const size_t &numInputPins, const std::vector< PinData > &pinData, const PinDataIRWKF &pinDataIRWKF, const uint8_t &numStates, const double &dtInit, std::vector< Eigen::VectorXd > &processNoiseVariances, KalmanFilter &kalmanFilter, const bool &imuCharacteristicsIdentical, const bool &imuBiasesIdentical)
	Initializes the IRWKF manually (i.e. using GUI inputs instead of averaging)

Eigen::MatrixXd	NAV::IRWKF::initialStateTransitionMatrix_Phi (const double &dt, const uint8_t &numStates)
	Calculates the state-transition-matrix 𝚽

Eigen::Vector3d	NAV::IRWKF::mean (const std::vector< std::vector< double > > &sensorType, const size_t &containerPos)
	Calculates the mean values of each axis in a vector that contains 3d measurements of a certain sensor type.

void	NAV::IRWKF::processNoiseMatrix_Q (Eigen::MatrixXd &Q, const double &dt, const std::vector< Eigen::VectorXd > &processNoiseVariances, const uint8_t &numStates, const bool &imuCharacteristicsIdentical=false)
	Calculates the process noise matrix Q.

void	NAV::IRWKF::stateTransitionMatrix_Phi (Eigen::MatrixXd &Phi, const double &dt)
	Calculates the state-transition-matrix 𝚽

Eigen::Vector3d	NAV::IRWKF::variance (const std::vector< std::vector< double > > &sensorType, const size_t &containerPos)
	Calculates the variance of each axis in a vector that contains 3d measurements of a certain sensor type.

Detailed Description

Kalman filter matrices for the Integrated-Random-Walk (IRW) Multi-IMU fusion.

Author: M. Maier (marce.nosp@m.l.ma.nosp@m.ier@i.nosp@m.ns.u.nosp@m.ni-st.nosp@m.uttg.nosp@m.art.d.nosp@m.e)

Date: 2024-04-02

Function Documentation

◆ designMatrix_H()

Eigen::MatrixXd NAV::IRWKF::designMatrix_H	(	const Eigen::Matrix3d &	DCM_accel,
		const Eigen::Matrix3d &	DCM_gyro,
		const size_t &	pinIndex,
		const uint8_t &	numMeasurements,
		const uint8_t &	numStates,
		const uint8_t &	numStatesEst,
		const uint8_t &	numStatesPerPin )

Calculates the design matrix H.

Parameters

[in]	DCM_accel	Rotation matrix of mounting angles of an accelerometer w.r.t. a common reference
[in]	DCM_gyro	Rotation matrix of mounting angles of a gyroscope w.r.t. a common reference
[in]	pinIndex	Index of pin to identify sensor
[in]	numMeasurements	Number of measurements
[in]	numStates	Number of KF states
[in]	numStatesEst	Number of estimated states (depends on imuFusionType)
[in]	numStatesPerPin	Number of states per pin (biases of accel and gyro)

Returns: Design matrix H

◆ initialErrorCovarianceMatrix_P0()

Eigen::MatrixXd NAV::IRWKF::initialErrorCovarianceMatrix_P0	(	const std::vector< Eigen::Vector3d > &	initCovariances,
		const uint8_t &	numStates,
		const bool &	imuCharacteristicsIdentical = false )

Initial error covariance matrix P_0.

Parameters

[in]	initCovariances	Initial covariances of the states
[in]	numStates	Number of KF states
[in]	imuCharacteristicsIdentical	If the multiple IMUs have the same characteristics, GUI input cells can be reduced considerably

Returns: The (numStates) x (numStates) matrix of initial state variances

◆ initializeKalmanFilterAuto()

KalmanFilter NAV::IRWKF::initializeKalmanFilterAuto	(	const size_t &	nInputPins,
		const std::vector< PinData > &	pinData,
		const PinDataIRWKF &	pinDataIRWKF,
		const std::vector< size_t > &	cumulatedPinIds,
		const std::vector< std::shared_ptr< const NAV::ImuObs > > &	cumulatedImuObs,
		const bool &	initJerkAngAcc,
		const double &	dtInit,
		const uint8_t &	numStates,
		const uint8_t &	numMeasurements,
		std::vector< Eigen::VectorXd > &	processNoiseVariances,
		KalmanFilter &	kalmanFilter )

Initializes the IRWKF automatically, i.e. init values are calculated by averaging the data in the first T seconds.

Parameters

[in]	nInputPins	Number of input pins of the IMU fusion node
[in]	pinData	pin data (i.e. GUI settings for the IRWKF)
[in]	pinDataIRWKF	pin data (i.e. GUI settings specific to the IRWKF)
[in]	cumulatedPinIds	Container that collects all pinIds for averaging for auto-init of the KF
[in]	cumulatedImuObs	Container that collects all imuObs for averaging for auto-init of the KF
[in]	initJerkAngAcc	flag to determine how jerk and angular acceleration states are initialized if '_autoInitKF = true'
[in]	dtInit	Initial value for dt (discrete state transition time)
[in]	processNoiseVariances	Container for process noise of each state
[in]	numStates	Number of states of the IRWKF
[in]	numMeasurements	Number of measurements of the IRWKF
[in]	kalmanFilter	Initial IRWKF object, which has the size defined in the GUI (numStates and numMeasurements)

Returns: Initialized IRWKF object

◆ initializeKalmanFilterManually()

KalmanFilter NAV::IRWKF::initializeKalmanFilterManually	(	const size_t &	numInputPins,
		const std::vector< PinData > &	pinData,
		const PinDataIRWKF &	pinDataIRWKF,
		const uint8_t &	numStates,
		const double &	dtInit,
		std::vector< Eigen::VectorXd > &	processNoiseVariances,
		KalmanFilter &	kalmanFilter,
		const bool &	imuCharacteristicsIdentical,
		const bool &	imuBiasesIdentical )

Initializes the IRWKF manually (i.e. using GUI inputs instead of averaging)

Parameters

[in]	numInputPins	Number of input pins of the IMU fusion node
[in]	pinData	pin data (i.e. GUI settings for the IRWKF with manual initialization)
[in]	pinDataIRWKF	pin data specific to the IRWKF
[in]	numStates	Number of states of the IRWKF
[in]	dtInit	Initial value for dt (discrete state transition time)
[in]	processNoiseVariances	Container for process noise of each state
[in]	kalmanFilter	Initial IRWKF object, which has the size defined in the GUI (numStates and numMeasurements)
[in]	imuCharacteristicsIdentical	If the multiple IMUs have the same characteristics, GUI input cells can be reduced considerably
[in]	imuBiasesIdentical	If the multiple IMUs have the same characteristics, GUI input cells can be reduced considerably

Returns: Initialized IRWKF object

◆ initialStateTransitionMatrix_Phi()

Eigen::MatrixXd NAV::IRWKF::initialStateTransitionMatrix_Phi	(	const double &	dt,
		const uint8_t &	numStates )

Calculates the state-transition-matrix 𝚽

Parameters

[in]	dt	Time difference between two successive measurements
[in]	numStates	Number of KF states

Returns: State-transition-matrix 𝚽

◆ mean()

Eigen::Vector3d NAV::IRWKF::mean	(	const std::vector< std::vector< double > > &	sensorType,
		const size_t &	containerPos )

Calculates the mean values of each axis in a vector that contains 3d measurements of a certain sensor type.

Parameters

[in]	sensorType	type of measurement, i.e. Acceleration or gyro measurements in 3d (axisIndex / msgIndex)
[in]	containerPos	position-Index in 'sensorType' where data starts (e.g. Accel at 0, Gyro at 3)

Returns: Vector of mean values in 3d of a certain sensor type

◆ processNoiseMatrix_Q()

void NAV::IRWKF::processNoiseMatrix_Q	(	Eigen::MatrixXd &	Q,
		const double &	dt,
		const std::vector< Eigen::VectorXd > &	processNoiseVariances,
		const uint8_t &	numStates,
		const bool &	imuCharacteristicsIdentical = false )

Calculates the process noise matrix Q.

Parameters

[in]	Q	Process noise matrix of the previous time step
[in]	dt	Time difference between two successive measurements
[in]	processNoiseVariances	Vector that contains the variances for each state's process noise
[in]	numStates	Number of KF states
[in]	imuCharacteristicsIdentical	If the multiple IMUs have the same characteristics, GUI input cells can be reduced considerably

◆ stateTransitionMatrix_Phi()

void NAV::IRWKF::stateTransitionMatrix_Phi	(	Eigen::MatrixXd &	Phi,
		const double &	dt )

Calculates the state-transition-matrix 𝚽

Parameters

[in]	Phi	State transition matrix from previous iteration. Returns the matrix for the current iteration.
[in]	dt	Time difference between two successive measurements

◆ variance()

Eigen::Vector3d NAV::IRWKF::variance	(	const std::vector< std::vector< double > > &	sensorType,
		const size_t &	containerPos )

Calculates the variance of each axis in a vector that contains 3d measurements of a certain sensor type.

Parameters

[in]	sensorType	type of measurement, i.e. Acceleration or gyro measurements in 3d (axisIndex / msgIndex)
[in]	containerPos	position-Index in 'sensorType' where data starts (e.g. Accel at 0, Gyro at 3)

Returns: Vector of variance values in 3d of a certain sensor type

Functions

Detailed Description

Function Documentation

◆ designMatrix_H()

◆ initialErrorCovarianceMatrix_P0()

◆ initializeKalmanFilterAuto()

◆ initializeKalmanFilterManually()

◆ initialStateTransitionMatrix_Phi()

◆ mean()

◆ processNoiseMatrix_Q()

◆ stateTransitionMatrix_Phi()

◆ variance()