Class KalmanFilter<States extends Num,Inputs extends Num,Outputs extends Num>
- All Implemented Interfaces:
KalmanTypeFilter<States,Inputs,Outputs>
public class KalmanFilter<States extends Num,Inputs extends Num,Outputs extends Num> extends Object implements KalmanTypeFilter<States,Inputs,Outputs>
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".
-
Constructor Summary
-
Method Summary
Modifier and Type Method Description void
correct(Matrix<Inputs,N1> u, Matrix<Outputs,N1> y)
Correct the state estimate x-hat using the measurements in y.void
correct(Matrix<Inputs,N1> u, Matrix<Outputs,N1> y, Matrix<Outputs,Outputs> R)
Correct the state estimate x-hat using the measurements in y.Matrix<States,States>
getP()
Returns the error covariance matrix P.double
getP(int row, int col)
Returns an element of the error covariance matrix P.Matrix<States,N1>
getXhat()
Returns the state estimate x-hat.double
getXhat(int row)
Returns an element of the state estimate x-hat.void
predict(Matrix<Inputs,N1> u, double dtSeconds)
Project the model into the future with a new control input u.void
reset()
void
setP(Matrix<States,States> newP)
Sets the entire error covariance matrix P.void
setXhat(int row, double value)
Set an element of the initial state estimate x-hat.void
setXhat(Matrix<States,N1> xHat)
Set initial state estimate x-hat.
-
Constructor Details
-
KalmanFilter
public KalmanFilter(Nat<States> states, Nat<Outputs> outputs, LinearSystem<States,Inputs,Outputs> plant, Matrix<States,N1> stateStdDevs, Matrix<Outputs,N1> measurementStdDevs, double dtSeconds)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:
states
- A Nat representing the states of the system.outputs
- A Nat representing the outputs of the system.plant
- The plant used for the prediction step.stateStdDevs
- Standard deviations of model states.measurementStdDevs
- Standard deviations of measurements.dtSeconds
- Nominal discretization timestep.- Throws:
IllegalArgumentException
- If the system is unobservable.
-
-
Method Details
-
getP
Returns the error covariance matrix P. -
getP
Returns an element of the error covariance matrix P. -
setP
Sets the entire error covariance matrix P. -
getXhat
Returns the state estimate x-hat. -
getXhat
Returns an element of the state estimate x-hat. -
setXhat
Set initial state estimate x-hat. -
setXhat
Set an element of the initial state estimate x-hat. -
reset
-
predict
Project the model into the future with a new control input u. -
correct
Correct the state estimate x-hat using the measurements in y. -
correct
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.
-