Functions
Eigen::MatrixXd	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	initialErrorCovarianceMatrix_P0 (const std::vector< Eigen::VectorXd > &initCovariances, const uint8_t &numStates, const bool &imuCharacteristicsIdentical=false)
	Initial error covariance matrix P_0.
KalmanFilter	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	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.
std::array< double, 3 >	quadraticBsplines (const double &ti, const double &splineSpacing=1.0)
	Set the points/knots of the four B-splines.
void	rotateCoeffStates (Eigen::MatrixXd &x)
	Rotates the B-spline coefficient states in the state vector x, once a new B-spline is introduced.
void	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.

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 )

nodiscard

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

Definition at line 91 of file BsplineKF.cpp.

◆ initialErrorCovarianceMatrix_P0()

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

nodiscard

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)

Definition at line 16 of file BsplineKF.cpp.

◆ initializeKalmanFilterManually()

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

Definition at line 123 of file BsplineKF.cpp.

◆ 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

Definition at line 38 of file BsplineKF.cpp.

◆ quadraticBsplines()

std::array< double, 3 > NAV::BsplineKF::quadraticBsplines	(	const double &	ti,
		const double &	splineSpacing = 1.0 )

Set the points/knots of the four B-splines.

Parameters

[in]	ti	time i to evaluate the spline at in [s]
[in]	splineSpacing	time difference between one and another B-spline in [s]

Returns: Vector that contains the values of each quadratic B-spline at time ti, from the stacked B-spline

Definition at line 16 of file QuadraticBsplines.cpp.

◆ 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

Definition at line 56 of file BsplineKF.cpp.

◆ 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

Definition at line 62 of file BsplineKF.cpp.

Functions

Function Documentation

◆ designMatrix_H()

◆ initialErrorCovarianceMatrix_P0()

◆ initializeKalmanFilterManually()

◆ processNoiseMatrix_Q()

◆ quadraticBsplines()

◆ rotateCoeffStates()

◆ rotateErrorCovariances()