BsplineKF.hpp File Reference

Kalman filter matrices for the B-spline Multi-IMU fusion. More...

Go to the source code of this file.

Functions
Eigen::MatrixXd	NAV::BsplineKF::designMatrix_H (const double &ti, const double &splineSpacing, 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::BsplineKF::initialErrorCovarianceMatrix_P0 (const std::vector< Eigen::VectorXd > &initCovariances, const uint8_t &numStates, const bool &imuCharacteristicsIdentical=false)
	Initial error covariance matrix P_0.
KalmanFilter	NAV::BsplineKF::initializeKalmanFilterManually (const size_t &numInputPins, const std::vector< PinData > &pinData, const PinDataBsplineKF &pinDataBsplineKF, const uint8_t &numStates, const double &dtInit, std::vector< Eigen::VectorXd > &processNoiseVariances, KalmanFilter &kalmanFilter, const bool &imuCharacteristicsIdentical, const bool &imuBiasesIdentical)
	Initializes the BsplineKF manually (i.e. using GUI inputs instead of averaging)
void	NAV::BsplineKF::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::BsplineKF::rotateCoeffStates (Eigen::MatrixXd &x)
	Rotates the B-spline coefficient states in the state vector x, once a new B-spline is introduced.
void	NAV::BsplineKF::rotateErrorCovariances (Eigen::MatrixXd &P, uint8_t &numStates, const double &sigmaScalingFactorAngRate=3.0, const double &sigmaScalingFactorAccel=3.0)
	Rotates the B-spline coefficient error covariances in P, once a new B-spline is introduced.

Detailed Description

Kalman filter matrices for the B-spline 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::BsplineKF::designMatrix_H	(	const double &	ti,
		const double &	splineSpacing,
		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]	ti	Current point in time
[in]	splineSpacing	distance - in time - between two splines of the stacked B-spline
[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::BsplineKF::initialErrorCovarianceMatrix_P0	(	const std::vector< Eigen::VectorXd > &	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 initial P matrix, i.e. P_0, which contains the initial state variances (numStates) x (numStates)

initializeKalmanFilterManually()

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

Initializes the BsplineKF 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 BsplineKF with manual initialization)
[in]	pinDataBsplineKF	pin data specific to the B-spline-KF
[in]	numStates	Number of states of the BsplineKF
[in]	dtInit	Initial value for dt (discrete state transition time)
[in]	processNoiseVariances	Container for process noise of each state
[in]	kalmanFilter	Initial BsplineKF 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 BsplineKF object

processNoiseMatrix_Q()

void NAV::BsplineKF::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

rotateCoeffStates()

void NAV::BsplineKF::rotateCoeffStates

(

Eigen::MatrixXd &

x

)

Rotates the B-spline coefficient states in the state vector x, once a new B-spline is introduced.

Parameters

[in]

x

Old state vector

rotateErrorCovariances()

void NAV::BsplineKF::rotateErrorCovariances	(	Eigen::MatrixXd &	P,
		uint8_t &	numStates,
		const double &	sigmaScalingFactorAngRate = 3.0,
		const double &	sigmaScalingFactorAccel = 3.0 )

Rotates the B-spline coefficient error covariances in P, once a new B-spline is introduced.

Parameters

[in]	P	Old error covariance matrix
[in]	numStates	Number of states of the B-spline KF
[in]	sigmaScalingFactorAngRate	Scaling factor relating the error covariance of the newly introduced B-spline coefficients for angular rate to their old error covariance
[in]	sigmaScalingFactorAccel	Scaling factor relating the error covariance of the newly introduced B-spline coefficients for acceleration to their old error covariance