Keyed Kalman Filter class. More...

Classes
struct	NISResult
	Normalized Innovation Squared (NIS) test results. More...

struct	SavedPreUpdate
	Saved pre-update state and measurement. More...

Public Member Functions
void	addState (const StateKeyType &stateKey)
	Add a new state to the filter.

void	addStates (const std::vector< StateKeyType > &stateKeys)
	Add new states to the filter.

void	calcPhiAndQWithVanLoanMethod (Scalar dt)
	Numerical Method to calculate the State transition matrix 𝚽 and System/Process noise covariance matrix 𝐐

void	calcTransitionMatrix_Phi_exp (Scalar tau)
	Calculates the state transition matrix 𝚽 using the exponential matrix.

void	calcTransitionMatrix_Phi_Taylor (Scalar tau, size_t order)
	Calculates the state transition matrix 𝚽 limited to specified order in 𝐅𝜏ₛ

auto	checkForOutliersNIS (const std::string &nameId)
	Performs the Normalized Innovation Squared (NIS) test on the measurement innovation 𝜹𝐳

void	correct ()
	Do a Measurement Update with a Measurement 𝐳

void	correctWithMeasurementInnovation ()
	Do a Measurement Update with a Measurement Innovation 𝜹𝐳

void	discardPreUpdate ()
	Discards the saved x̂, 𝐏, 𝐳, 𝐇, 𝐑 a-priori (pre-update)

bool	hasAnyStates (const std::vector< StateKeyType > &stateKeys) const
	Checks if the filter has any of the provided keys.

bool	hasState (const StateKeyType &stateKey) const
	Checks if the filter has the key.

bool	hasStates (const std::vector< StateKeyType > &stateKeys) const
	Checks if the filter has the keys.

bool	isNISenabled () const
	Whether the NIS check should be performed.

bool	isPreUpdateSaved () const
	Whether a pre-update was saved.

	KeyedKalmanFilter ()=default
	Default Constructor.

	KeyedKalmanFilter (const std::vector< StateKeyType > &stateKeys, const std::vector< MeasKeyType > &measKeys)
	Constructor.

void	predict ()
	Do a Time Update.

void	removeMeasurement (const MeasKeyType &measKey)
	Remove a measurement from the filter.

void	removeMeasurements (const std::vector< MeasKeyType > &measKeys)
	Remove measurements from the filter.

void	removeState (const StateKeyType &stateKey)
	Remove a state from the filter.

void	removeStates (const std::vector< StateKeyType > &stateKeys)
	Remove states from the filter.

void	replaceState (const StateKeyType &oldKey, const StateKeyType &newKey)
	Replace the old with the new key.

void	restorePreUpdate ()
	Restores the saved x̂, 𝐏, 𝐳, 𝐇, 𝐑 a-priori (pre-update)

const SavedPreUpdate &	savedPreUpdate () const
	Accesses the saved pre-update matrices.

void	savePreUpdate ()
	Saves x̂, 𝐏, 𝐳, 𝐇, 𝐑 a-priori (pre-update)

void	setMeasurements (const std::vector< MeasKeyType > &measKeys)
	Sets the measurement keys and initializes matrices z, H, R, S, K with Zero.

void	setZero ()
	Sets all Vectors and matrices to 0.

bool	showKalmanFilterGUI (const char *id, float width=0.0F)
	Shows GUI elements for the Kalman Filter.

void	showKalmanFilterMatrixViews (const char *id, int nRows=-2)
	Shows ImGui Tree nodes for all matrices.

Public Attributes
KeyedMatrixX< Scalar, StateKeyType, StateKeyType >	F
	𝐅 System model matrix (n x n)

KeyedMatrixX< Scalar, StateKeyType, StateKeyType >	G
	𝐆 Noise input matrix (n x o)

KeyedMatrixX< Scalar, MeasKeyType, StateKeyType >	H
	𝐇 Measurement sensitivity matrix (m x n)

KeyedMatrixX< Scalar, StateKeyType, MeasKeyType >	K
	𝐊 Kalman gain matrix (n x m)

KeyedMatrixX< Scalar, StateKeyType, StateKeyType >	P
	𝐏 Error covariance matrix (n x n)

KeyedMatrixX< Scalar, StateKeyType, StateKeyType >	Phi
	𝚽 State transition matrix (n x n)

KeyedMatrixX< Scalar, StateKeyType, StateKeyType >	Q
	𝐐 System/Process noise covariance matrix (n x n)

KeyedMatrixX< Scalar, MeasKeyType, MeasKeyType >	R
	𝐑 = 𝐸{𝐰ₘ𝐰ₘᵀ} Measurement noise covariance matrix (m x m)

KeyedMatrixX< Scalar, MeasKeyType, MeasKeyType >	S
	𝗦 Measurement prediction covariance matrix (m x m)

KeyedMatrixX< Scalar, StateKeyType, StateKeyType >	W
	𝐖 Noise scale matrix (o x o)

KeyedVectorX< Scalar, StateKeyType >	x
	x̂ State vector (n x 1)

KeyedVectorX< Scalar, MeasKeyType >	z
	𝐳 Measurement vector (m x 1)

Detailed Description

template<typename Scalar, typename StateKeyType, typename MeasKeyType>
class NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >

Keyed Kalman Filter class.

Template Parameters

Scalar	Numeric type, e.g. float, double, int or std::complex<float>.
StateKeyType	Type of the key used for state lookup
MeasKeyType	Type of the key used for measurement lookup

Constructor & Destructor Documentation

◆ KeyedKalmanFilter()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::KeyedKalmanFilter	(	const std::vector< StateKeyType > &	stateKeys,
		const std::vector< MeasKeyType > &	measKeys )

inline

Constructor.

Parameters

stateKeys	State keys
measKeys	Measurement keys

Member Function Documentation

◆ addState()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::addState ( const StateKeyType & stateKey )

inline

Add a new state to the filter.

Parameters

stateKey State key

◆ addStates()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::addStates ( const std::vector< StateKeyType > & stateKeys )

inline

Add new states to the filter.

Parameters

stateKeys State keys

◆ calcPhiAndQWithVanLoanMethod()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::calcPhiAndQWithVanLoanMethod ( Scalar dt )

inline

Numerical Method to calculate the State transition matrix 𝚽 and System/Process noise covariance matrix 𝐐

Parameters

[in] dt Time step in [s]

Note: See C.F. van Loan (1978) - Computing Integrals Involving the Matrix Exponential [46]

◆ calcTransitionMatrix_Phi_exp()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::calcTransitionMatrix_Phi_exp ( Scalar tau )

inline

Calculates the state transition matrix 𝚽 using the exponential matrix.

Parameters

[in] tau Time interval in [s]

Note: See [17] Groves, ch. 3.2.3, eq. 3.33, p. 97

Attention: The cost of the computation is approximately 20n^3 for matrices of size n. The number 20 depends weakly on the norm of the matrix.

◆ calcTransitionMatrix_Phi_Taylor()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::calcTransitionMatrix_Phi_Taylor	(	Scalar	tau,
		size_t	order )

inline

Calculates the state transition matrix 𝚽 limited to specified order in 𝐅𝜏ₛ

Parameters

[in]	tau	Time interval in [s]
[in]	order	The order of the Taylor polynom to calculate

Note: See [17] Groves, ch. 3.2.3, eq. 3.34, p. 98

◆ checkForOutliersNIS()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

auto NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::checkForOutliersNIS ( const std::string & nameId )

inline

Performs the Normalized Innovation Squared (NIS) test on the measurement innovation 𝜹𝐳

Parameters

[in] nameId Caller node for debug output

H_0: The measurement residual 𝜹𝐳 is consistent with the innovation covariance matrix 𝗦 The acceptance interval is chosen such that the probability that H_0 is accepted is (1 - alpha)

Returns: The hypothesis test result if it failed, otherwise nothing.

Attention: Needs to be called before the update

◆ correct()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::correct ( )

inline

Do a Measurement Update with a Measurement 𝐳

Attention: Update the Measurement sensitivity Matrix (𝐇), the Measurement noise covariance matrix (𝐑) and the Measurement vector (𝐳) before calling this

Note: See P. Groves (2013) - Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (ch. 3.2.2)

◆ correctWithMeasurementInnovation()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::correctWithMeasurementInnovation ( )

inline

Do a Measurement Update with a Measurement Innovation 𝜹𝐳

Attention: Update the Measurement sensitivity Matrix (𝐇), the Measurement noise covariance matrix (𝐑) and the Measurement vector (𝐳) before calling this

Note: See P. Groves (2013) - Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (ch. 3.2.2); See Brown & Hwang (2012) - Introduction to Random Signals and Applied Kalman Filtering (ch. 5.5 - figure 5.5)

◆ hasAnyStates()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

bool NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::hasAnyStates ( const std::vector< StateKeyType > & stateKeys ) const

inline

Checks if the filter has any of the provided keys.

Parameters

stateKeys State keys

◆ hasState()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

bool NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::hasState ( const StateKeyType & stateKey ) const

inline

Checks if the filter has the key.

Parameters

stateKey State key

◆ hasStates()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

bool NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::hasStates ( const std::vector< StateKeyType > & stateKeys ) const

inline

Checks if the filter has the keys.

Parameters

stateKeys State keys

◆ predict()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::predict ( )

inline

Do a Time Update.

Attention: Update the State transition matrix (𝚽) and the Process noise covariance matrix (𝐐) before calling this

Note: See P. Groves (2013) - Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (ch. 3.2.2)

◆ removeMeasurement()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::removeMeasurement ( const MeasKeyType & measKey )

inline

Remove a measurement from the filter.

Parameters

measKey Measurement key

◆ removeMeasurements()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::removeMeasurements ( const std::vector< MeasKeyType > & measKeys )

inline

Remove measurements from the filter.

Parameters

measKeys Measurement keys

◆ removeState()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::removeState ( const StateKeyType & stateKey )

inline

Remove a state from the filter.

Parameters

stateKey State key

◆ removeStates()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::removeStates ( const std::vector< StateKeyType > & stateKeys )

inline

Remove states from the filter.

Parameters

stateKeys State keys

◆ replaceState()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::replaceState	(	const StateKeyType &	oldKey,
		const StateKeyType &	newKey )

inline

Replace the old with the new key.

Parameters

[in]	oldKey	Old key to replace
[in]	newKey	New key to use instead

◆ setMeasurements()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::setMeasurements ( const std::vector< MeasKeyType > & measKeys )

inline

Sets the measurement keys and initializes matrices z, H, R, S, K with Zero.

Parameters

measKeys Measurement keys

◆ showKalmanFilterGUI()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

bool NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::showKalmanFilterGUI	(	const char *	id,
		float	width = 0.0F )

inline

Shows GUI elements for the Kalman Filter.

Parameters

[in]	id	Unique id for ImGui
[in]	width	Width of the widget

Returns: True if something was changed

◆ showKalmanFilterMatrixViews()

template<typename Scalar , typename StateKeyType , typename MeasKeyType >

void NAV::KeyedKalmanFilter< Scalar, StateKeyType, MeasKeyType >::showKalmanFilterMatrixViews	(	const char *	id,
		int	nRows = -2 )

inline

Shows ImGui Tree nodes for all matrices.

Parameters

[in]	id	Unique id for ImGui
[in]	nRows	Amount of rows to show

The documentation for this class was generated from the following file:

/home/runner/work/INSTINCT/INSTINCT/src/Navigation/Math/KeyedKalmanFilter.hpp

Classes

Public Member Functions

Public Attributes

Detailed Description

Constructor & Destructor Documentation

◆ KeyedKalmanFilter()

Member Function Documentation

◆ addState()

◆ addStates()

◆ calcPhiAndQWithVanLoanMethod()

◆ calcTransitionMatrix_Phi_exp()

◆ calcTransitionMatrix_Phi_Taylor()

◆ checkForOutliersNIS()

◆ correct()

◆ correctWithMeasurementInnovation()

◆ hasAnyStates()

◆ hasState()

◆ hasStates()

◆ predict()

◆ removeMeasurement()

◆ removeMeasurements()

◆ removeState()

◆ removeStates()

◆ replaceState()

◆ setMeasurements()

◆ showKalmanFilterGUI()

◆ showKalmanFilterMatrixViews()