#include <kalman_filter.h>

Public Member Functions
	KalmanFilter (LinearSystem< STATES, INPUTS, OUTPUTS > &plant, const StateVector &state_stddevs, const OutputVector &measurement_stddevs)

	KalmanFilter (const StateMatrix &A, const InputMatrix &B, const EMat< OUTPUTS, STATES > &C, const EMat< OUTPUTS, INPUTS > &D, const StateVector &state_stddevs, const OutputVector &measurement_stddevs)

StateMatrix	P () const

void	set_P (const StateMatrix &P)

const StateVector &	xhat () const

double	xhat (int i) const

void	set_xhat (const StateVector &xhat)

void	set_xhat (int i, double value)

void	reset ()

void	predict (const InputVector &u, const double &dt)

void	correct (const OutputVector &y, const InputVector &u)

void	correct (const OutputVector &y, const InputVector &u, const EMat< OUTPUTS, OUTPUTS > &R)

template<int ROWS>
void	correct (const EVec< ROWS > &y, const InputVector &u, const EMat< ROWS, STATES > &C, const EMat< ROWS, INPUTS > &D, const EMat< ROWS, ROWS > &R)

Detailed Description

template<int STATES, int INPUTS, int OUTPUTS>
class KalmanFilter< STATES, INPUTS, OUTPUTS >

Kalman filters combine predictions from a model and measurements to estimate a system's true state.

Each call of predict moves the state forward in time according to the matrix A, and the covariance has white noise Q added.

Each call of correct applies a measurement which moves the state more toward the true state, and it reduces the state covariance.

To read more about Kalman filters read: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

Template Parameters

STATES	Dimension of the state vector.
INPUTS	Dimension of the control input vector.
OUTPUTS	Dimension of the measurement vector.

Constructor & Destructor Documentation

◆ KalmanFilter() [1/2]

template<int STATES, int INPUTS, int OUTPUTS>

KalmanFilter< STATES, INPUTS, OUTPUTS >::KalmanFilter	(	LinearSystem< STATES, INPUTS, OUTPUTS > &	plant,
		const StateVector &	state_stddevs,
		const OutputVector &	measurement_stddevs )

inline

Constructs a Kalman filter.

Parameters

plant	The linear system the filter tracks.
state_stddevs	The standard deviations of the states.
measurement_stddevs	The standard deviations of the measurements.

◆ KalmanFilter() [2/2]

template<int STATES, int INPUTS, int OUTPUTS>

KalmanFilter< STATES, INPUTS, OUTPUTS >::KalmanFilter	(	const StateMatrix &	A,
		const InputMatrix &	B,
		const EMat< OUTPUTS, STATES > &	C,
		const EMat< OUTPUTS, INPUTS > &	D,
		const StateVector &	state_stddevs,
		const OutputVector &	measurement_stddevs )

inline

Constructs a Kalman filter.

Parameters

A	The state matrix.
B	The input matrix.
C	The measurement matrix.
D	The feedthrough matrix.
state_stddevs	The standard deviations of the states.
measurement_stddevs	The standard deviations of the measurements.

Member Function Documentation

◆ correct() [1/3]

template<int STATES, int INPUTS, int OUTPUTS>

template<int ROWS>

void KalmanFilter< STATES, INPUTS, OUTPUTS >::correct	(	const EVec< ROWS > &	y,
		const InputVector &	u,
		const EMat< ROWS, STATES > &	C,
		const EMat< ROWS, INPUTS > &	D,
		const EMat< ROWS, ROWS > &	R )

inline

Correct the state estimate using the measurements in y, custom measurement and feedthrough matrices, and custom measurement measurement noise. This is useful for when a different set of measurements are being applied than what the plant defines.

Parameters

y	The vector of measurements.
u	The control input used in the last predict step.
C	The measurement matrix to use for this step.
D	The feedthrough matrix to use for this step.
R	The measurement noise matrix to use for this step.

◆ correct() [2/3]

template<int STATES, int INPUTS, int OUTPUTS>

void KalmanFilter< STATES, INPUTS, OUTPUTS >::correct	(	const OutputVector &	y,
		const InputVector &	u )

inline

Correct the state estimate using the measurements in y.

Parameters

y	The vector of measurements.
u	The control input used in the last predict step.

◆ correct() [3/3]

template<int STATES, int INPUTS, int OUTPUTS>

void KalmanFilter< STATES, INPUTS, OUTPUTS >::correct	(	const OutputVector &	y,
		const InputVector &	u,
		const EMat< OUTPUTS, OUTPUTS > &	R )

inline

Correct the state estimate using the measurements in y, and custom measurement noise matrix. This is useful for when the noise in the measurements vary.

Parameters

y	The vector of measurements.
u	The control input used in the last predict step.
R	The measurement noise matrix to use for this step.

◆ P()

template<int STATES, int INPUTS, int OUTPUTS>

StateMatrix KalmanFilter< STATES, INPUTS, OUTPUTS >::P ( ) const

inline

Returns the covariance matrix P.

◆ predict()

template<int STATES, int INPUTS, int OUTPUTS>

void KalmanFilter< STATES, INPUTS, OUTPUTS >::predict	(	const InputVector &	u,
		const double &	dt )

inline

Projects the state into the future by dt seconds with control input u.

Parameters

u	The control input.
dt	The timestep in seconds.

◆ reset()

template<int STATES, int INPUTS, int OUTPUTS>

void KalmanFilter< STATES, INPUTS, OUTPUTS >::reset ( )

inline

Resets the filter.

◆ set_P()

template<int STATES, int INPUTS, int OUTPUTS>

void KalmanFilter< STATES, INPUTS, OUTPUTS >::set_P ( const StateMatrix & P )

inline

Set the current covariance matrix P.

Parameters

P	The covariance matrix P.

◆ set_xhat() [1/2]

template<int STATES, int INPUTS, int OUTPUTS>

void KalmanFilter< STATES, INPUTS, OUTPUTS >::set_xhat ( const StateVector & xhat )

inline

Set the current state estimate x-hat.

◆ set_xhat() [2/2]

template<int STATES, int INPUTS, int OUTPUTS>

void KalmanFilter< STATES, INPUTS, OUTPUTS >::set_xhat	(	int	i,
		double	value )

inline

Set one element of the current state estimate x-hat.

Parameters

i	Row of x-hat.

◆ xhat() [1/2]

template<int STATES, int INPUTS, int OUTPUTS>

const StateVector & KalmanFilter< STATES, INPUTS, OUTPUTS >::xhat ( ) const

inline

Returns the current state estimate x-hat.

◆ xhat() [2/2]

template<int STATES, int INPUTS, int OUTPUTS>

double KalmanFilter< STATES, INPUTS, OUTPUTS >::xhat ( int i ) const

inline

Returns one element of the current state estimate x-hat.

Parameters

i	Row of x-hat.

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

kalman_filter.h

Public Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ KalmanFilter() [1/2]

◆ KalmanFilter() [2/2]

Member Function Documentation

◆ correct() [1/3]

◆ correct() [2/3]

◆ correct() [3/3]

◆ P()

◆ predict()

◆ reset()

◆ set_P()

◆ set_xhat() [1/2]

◆ set_xhat() [2/2]

◆ xhat() [1/2]

◆ xhat() [2/2]