frc::KalmanFilter< States, Inputs, Outputs > Class Template Reference

A Kalman filter combines predictions from a model and measurements to give an estimate of the true system state. More...

#include <frc/estimator/KalmanFilter.h>

Public Types
using	StateVector = Vectord< States >
using	InputVector = Vectord< Inputs >
using	OutputVector = Vectord< Outputs >
using	StateArray = wpi::array< double, States >
using	OutputArray = wpi::array< double, Outputs >
using	StateMatrix = Matrixd< States, States >

Public Member Functions
	KalmanFilter (LinearSystem< States, Inputs, Outputs > &plant, const StateArray &stateStdDevs, const OutputArray &measurementStdDevs, units::second_t dt)
	Constructs a Kalman filter with the given plant. More...
const StateMatrix &	P () const
	Returns the error covariance matrix P. More...
double	P (int i, int j) const
	Returns an element of the error covariance matrix P. More...
void	SetP (const StateMatrix &P)
	Set the current error covariance matrix P. More...
const StateVector &	Xhat () const
	Returns the state estimate x-hat. More...
double	Xhat (int i) const
	Returns an element of the state estimate x-hat. More...
void	SetXhat (const StateVector &xHat)
	Set initial state estimate x-hat. More...
void	SetXhat (int i, double value)
	Set an element of the initial state estimate x-hat. More...
void	Reset ()
	Resets the observer. More...
void	Predict (const InputVector &u, units::second_t dt)
	Project the model into the future with a new control input u. More...
void	Correct (const InputVector &u, const OutputVector &y)
	Correct the state estimate x-hat using the measurements in y. More...
void	Correct (const InputVector &u, const OutputVector &y, const Matrixd< Outputs, Outputs > &R)
	Correct the state estimate x-hat using the measurements in y. More...

Detailed Description

template<int States, int Inputs, int Outputs>
class frc::KalmanFilter< States, Inputs, Outputs >

A Kalman filter combines predictions from a model and measurements to give an estimate of the true system state.

This is useful because many states cannot be measured directly as a result of sensor noise, or because the state is "hidden".

Kalman filters use a K gain matrix to determine whether to trust the model or measurements more. Kalman filter theory uses statistics to compute an optimal K gain which minimizes the sum of squares error in the state estimate. This K gain is used to correct the state estimate by some amount of the difference between the actual measurements and the measurements predicted by the model.

For more on the underlying math, read https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control theory".

Template Parameters

States	Number of states.
Inputs	Number of inputs.
Outputs	Number of outputs.

Member Typedef Documentation

InputVector

template<int States, int Inputs, int Outputs>

using frc::KalmanFilter< States, Inputs, Outputs >::InputVector = Vectord<Inputs>

OutputArray

template<int States, int Inputs, int Outputs>

using frc::KalmanFilter< States, Inputs, Outputs >::OutputArray = wpi::array<double, Outputs>

OutputVector

template<int States, int Inputs, int Outputs>

using frc::KalmanFilter< States, Inputs, Outputs >::OutputVector = Vectord<Outputs>

StateArray

template<int States, int Inputs, int Outputs>

using frc::KalmanFilter< States, Inputs, Outputs >::StateArray = wpi::array<double, States>

StateMatrix

template<int States, int Inputs, int Outputs>

using frc::KalmanFilter< States, Inputs, Outputs >::StateMatrix = Matrixd<States, States>

StateVector

template<int States, int Inputs, int Outputs>

using frc::KalmanFilter< States, Inputs, Outputs >::StateVector = Vectord<States>

Constructor & Destructor Documentation

KalmanFilter()

template<int States, int Inputs, int Outputs>

frc::KalmanFilter< States, Inputs, Outputs >::KalmanFilter	(	LinearSystem< States, Inputs, Outputs > &	plant,
		const StateArray &	stateStdDevs,
		const OutputArray &	measurementStdDevs,
		units::second_t	dt
	)

Constructs a Kalman filter with the given plant.

See https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices for how to select the standard deviations.

Parameters

plant	The plant used for the prediction step.
stateStdDevs	Standard deviations of model states.
measurementStdDevs	Standard deviations of measurements.
dt	Nominal discretization timestep.

Exceptions

std::invalid_argument

If the system is unobservable.

Member Function Documentation

Correct() [1/2]

template<int States, int Inputs, int Outputs>

void frc::KalmanFilter< States, Inputs, Outputs >::Correct ( const InputVector &  u, const OutputVector &  y  )

inline

Correct the state estimate x-hat using the measurements in y.

Parameters

u	Same control input used in the predict step.
y	Measurement vector.

Correct() [2/2]

template<int States, int Inputs, int Outputs>

void frc::KalmanFilter< States, Inputs, Outputs >::Correct	(	const InputVector &	u,
		const OutputVector &	y,
		const Matrixd< Outputs, Outputs > &	R
	)

Correct the state estimate x-hat using the measurements in y.

This is useful for when the measurement noise covariances vary.

Parameters

u	Same control input used in the predict step.
y	Measurement vector.
R	Continuous measurement noise covariance matrix.

P() [1/2]

template<int States, int Inputs, int Outputs>

const StateMatrix & frc::KalmanFilter< States, Inputs, Outputs >::P ( ) const

inline

Returns the error covariance matrix P.

P() [2/2]

template<int States, int Inputs, int Outputs>

double frc::KalmanFilter< States, Inputs, Outputs >::P ( int  i, int  j  ) const

inline

Returns an element of the error covariance matrix P.

Parameters

i	Row of P.
j	Column of P.

Predict()

template<int States, int Inputs, int Outputs>

void frc::KalmanFilter< States, Inputs, Outputs >::Predict	(	const InputVector &	u,
		units::second_t	dt
	)

Project the model into the future with a new control input u.

Parameters

u	New control input from controller.
dt	Timestep for prediction.

Reset()

template<int States, int Inputs, int Outputs>

void frc::KalmanFilter< States, Inputs, Outputs >::Reset ( )

inline

Resets the observer.

SetP()

template<int States, int Inputs, int Outputs>

void frc::KalmanFilter< States, Inputs, Outputs >::SetP ( const StateMatrix &  P )

inline

Set the current error covariance matrix P.

Parameters

P	The error covariance matrix P.

SetXhat() [1/2]

template<int States, int Inputs, int Outputs>

void frc::KalmanFilter< States, Inputs, Outputs >::SetXhat ( const StateVector &  xHat )

inline

Set initial state estimate x-hat.

Parameters

xHat	The state estimate x-hat.

SetXhat() [2/2]

template<int States, int Inputs, int Outputs>

void frc::KalmanFilter< States, Inputs, Outputs >::SetXhat ( int  i, double  value  )

inline

Set an element of the initial state estimate x-hat.

Parameters

i	Row of x-hat.
value	Value for element of x-hat.

Xhat() [1/2]

template<int States, int Inputs, int Outputs>

const StateVector & frc::KalmanFilter< States, Inputs, Outputs >::Xhat ( ) const

inline

Returns the state estimate x-hat.

Xhat() [2/2]

template<int States, int Inputs, int Outputs>

double frc::KalmanFilter< States, Inputs, Outputs >::Xhat ( int  i ) const

inline

Returns an element of the state estimate x-hat.

Parameters

i	Row of x-hat.

---

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

• frc/estimator/KalmanFilter.h
• frc/estimator/KalmanFilter.inc