WPILibC++ 2024.3.2
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 StateMatrixP () 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 StateVectorXhat () 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
StatesNumber of states.
InputsNumber of inputs.
OutputsNumber 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
plantThe plant used for the prediction step.
stateStdDevsStandard deviations of model states.
measurementStdDevsStandard deviations of measurements.
dtNominal discretization timestep.
Exceptions
std::invalid_argumentIf 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
uSame control input used in the predict step.
yMeasurement 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
uSame control input used in the predict step.
yMeasurement vector.
RContinuous 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
iRow of P.
jColumn 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
uNew control input from controller.
dtTimestep 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
PThe 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
xHatThe 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
iRow of x-hat.
valueValue 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
iRow of x-hat.

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