Package edu.wpi.first.math.estimator

Class ExtendedKalmanFilter<States extends Num,Inputs extends Num,Outputs extends Num>

java.lang.Object

edu.wpi.first.math.estimator.ExtendedKalmanFilter<States,Inputs,Outputs>

All Implemented Interfaces:

KalmanTypeFilter<States,Inputs,Outputs>

public class ExtendedKalmanFilter<States extends Num,Inputs extends Num,Outputs extends Num>
extends Object
implements KalmanTypeFilter<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.

An extended Kalman filter supports nonlinear state and measurement models. It propagates the error covariance by linearizing the models around the state estimate, then applying the linear Kalman filter equations.

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

Constructor Summary

Constructors

Constructor	Description
ExtendedKalmanFilter(Nat<States> states, Nat<Inputs> inputs, Nat<Outputs> outputs, BiFunction<Matrix<States,N1>,Matrix<Inputs,N1>,Matrix<States,N1>> f, BiFunction<Matrix<States,N1>,Matrix<Inputs,N1>,Matrix<Outputs,N1>> h, Matrix<States,N1> stateStdDevs, Matrix<Outputs,N1> measurementStdDevs, double dtSeconds)	Constructs an extended Kalman filter.
ExtendedKalmanFilter(Nat<States> states, Nat<Inputs> inputs, Nat<Outputs> outputs, BiFunction<Matrix<States,N1>,Matrix<Inputs,N1>,Matrix<States,N1>> f, BiFunction<Matrix<States,N1>,Matrix<Inputs,N1>,Matrix<Outputs,N1>> h, Matrix<States,N1> stateStdDevs, Matrix<Outputs,N1> measurementStdDevs, BiFunction<Matrix<Outputs,N1>,Matrix<Outputs,N1>,Matrix<Outputs,N1>> residualFuncY, BiFunction<Matrix<States,N1>,Matrix<States,N1>,Matrix<States,N1>> addFuncX, double dtSeconds)	Constructs an extended Kalman filter.

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.
<Rows extends Num> void	correct(Nat<Rows> rows, Matrix<Inputs,N1> u, Matrix<Rows,N1> y, BiFunction<Matrix<States,N1>,Matrix<Inputs,N1>,Matrix<Rows,N1>> h, Matrix<Rows,Rows> R)	Correct the state estimate x-hat using the measurements in y.
<Rows extends Num> void	correct(Nat<Rows> rows, Matrix<Inputs,N1> u, Matrix<Rows,N1> y, BiFunction<Matrix<States,N1>,Matrix<Inputs,N1>,Matrix<Rows,N1>> h, Matrix<Rows,Rows> R, BiFunction<Matrix<Rows,N1>,Matrix<Rows,N1>,Matrix<Rows,N1>> residualFuncY, BiFunction<Matrix<States,N1>,Matrix<States,N1>,Matrix<States,N1>> addFuncX)	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	predict(Matrix<Inputs,N1> u, BiFunction<Matrix<States,N1>,Matrix<Inputs,N1>,Matrix<States,N1>> f, 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.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Constructor Details

ExtendedKalmanFilter

public ExtendedKalmanFilter(Nat<States> states, Nat<Inputs> inputs, Nat<Outputs> outputs, BiFunction<Matrix<States,N1>,Matrix<Inputs,N1>,Matrix<States,N1>> f, BiFunction<Matrix<States,N1>,Matrix<Inputs,N1>,Matrix<Outputs,N1>> h, Matrix<States,N1> stateStdDevs, Matrix<Outputs,N1> measurementStdDevs, double dtSeconds)

Constructs an extended Kalman filter.

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 number of states.

inputs - a Nat representing the number of inputs.

outputs - a Nat representing the number of outputs.

f - A vector-valued function of x and u that returns the derivative of the state vector.

h - A vector-valued function of x and u that returns the measurement vector.

stateStdDevs - Standard deviations of model states.

measurementStdDevs - Standard deviations of measurements.

dtSeconds - Nominal discretization timestep.

ExtendedKalmanFilter

public ExtendedKalmanFilter(Nat<States> states, Nat<Inputs> inputs, Nat<Outputs> outputs, BiFunction<Matrix<States,N1>,Matrix<Inputs,N1>,Matrix<States,N1>> f, BiFunction<Matrix<States,N1>,Matrix<Inputs,N1>,Matrix<Outputs,N1>> h, Matrix<States,N1> stateStdDevs, Matrix<Outputs,N1> measurementStdDevs, BiFunction<Matrix<Outputs,N1>,Matrix<Outputs,N1>,Matrix<Outputs,N1>> residualFuncY, BiFunction<Matrix<States,N1>,Matrix<States,N1>,Matrix<States,N1>> addFuncX, double dtSeconds)

Constructs an extended Kalman filter.

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 number of states.

inputs - a Nat representing the number of inputs.

outputs - a Nat representing the number of outputs.

f - A vector-valued function of x and u that returns the derivative of the state vector.

h - A vector-valued function of x and u that returns the measurement vector.

stateStdDevs - Standard deviations of model states.

measurementStdDevs - Standard deviations of measurements.

residualFuncY - A function that computes the residual of two measurement vectors (i.e. it subtracts them.)

addFuncX - A function that adds two state vectors.

dtSeconds - Nominal discretization timestep.

Method Details

getP

public Matrix<States,States> getP()

Returns the error covariance matrix P.

Specified by:

getP in interface KalmanTypeFilter<States extends Num,Inputs extends Num,Outputs extends Num>

Returns:

the error covariance matrix P.

getP

public double getP(int row, int col)

Returns an element of the error covariance matrix P.

Specified by:

getP in interface KalmanTypeFilter<States extends Num,Inputs extends Num,Outputs extends Num>

Parameters:

row - Row of P.

col - Column of P.

Returns:

the value of the error covariance matrix P at (i, j).

setP

public void setP(Matrix<States,States> newP)

Sets the entire error covariance matrix P.

Specified by:

setP in interface KalmanTypeFilter<States extends Num,Inputs extends Num,Outputs extends Num>

Parameters:

newP - The new value of P to use.

getXhat

public Matrix<States,N1> getXhat()

Returns the state estimate x-hat.

Specified by:

getXhat in interface KalmanTypeFilter<States extends Num,Inputs extends Num,Outputs extends Num>

Returns:

the state estimate x-hat.

getXhat

public double getXhat(int row)

Returns an element of the state estimate x-hat.

Specified by:

getXhat in interface KalmanTypeFilter<States extends Num,Inputs extends Num,Outputs extends Num>

Parameters:

row - Row of x-hat.

Returns:

the value of the state estimate x-hat at that row.

setXhat

public void setXhat(Matrix<States,N1> xHat)

Set initial state estimate x-hat.

Specified by:

setXhat in interface KalmanTypeFilter<States extends Num,Inputs extends Num,Outputs extends Num>

Parameters:

xHat - The state estimate x-hat.

setXhat

public void setXhat(int row, double value)

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

Specified by:

setXhat in interface KalmanTypeFilter<States extends Num,Inputs extends Num,Outputs extends Num>

Parameters:

row - Row of x-hat.

value - Value for element of x-hat.

reset

public final void reset()

Specified by:

reset in interface KalmanTypeFilter<States extends Num,Inputs extends Num,Outputs extends Num>

predict

public void predict(Matrix<Inputs,N1> u, double dtSeconds)

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

Specified by:

predict in interface KalmanTypeFilter<States extends Num,Inputs extends Num,Outputs extends Num>

Parameters:

u - New control input from controller.

dtSeconds - Timestep for prediction.

predict

public void predict(Matrix<Inputs,N1> u, BiFunction<Matrix<States,N1>,Matrix<Inputs,N1>,Matrix<States,N1>> f, double dtSeconds)

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

Parameters:

u - New control input from controller.

f - The function used to linearize the model.

dtSeconds - Timestep for prediction.

correct

public void correct(Matrix<Inputs,N1> u, Matrix<Outputs,N1> y)

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

Specified by:

correct in interface KalmanTypeFilter<States extends Num,Inputs extends Num,Outputs extends Num>

Parameters:

u - Same control input used in the predict step.

y - Measurement vector.

correct

public 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.

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.

correct

public <Rows extends Num> void correct(Nat<Rows> rows, Matrix<Inputs,N1> u, Matrix<Rows,N1> y, BiFunction<Matrix<States,N1>,Matrix<Inputs,N1>,Matrix<Rows,N1>> h, Matrix<Rows,Rows> R)

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

This is useful for when the measurements available during a timestep's Correct() call vary. The h(x, u) passed to the constructor is used if one is not provided (the two-argument version of this function).

Type Parameters:

Rows - Number of rows in the result of f(x, u).

Parameters:

rows - Number of rows in the result of f(x, u).

u - Same control input used in the predict step.

y - Measurement vector.

h - A vector-valued function of x and u that returns the measurement vector.

R - Continuous measurement noise covariance matrix.

correct

public <Rows extends Num> void correct(Nat<Rows> rows, Matrix<Inputs,N1> u, Matrix<Rows,N1> y, BiFunction<Matrix<States,N1>,Matrix<Inputs,N1>,Matrix<Rows,N1>> h, Matrix<Rows,Rows> R, BiFunction<Matrix<Rows,N1>,Matrix<Rows,N1>,Matrix<Rows,N1>> residualFuncY, BiFunction<Matrix<States,N1>,Matrix<States,N1>,Matrix<States,N1>> addFuncX)

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

This is useful for when the measurements available during a timestep's Correct() call vary. The h(x, u) passed to the constructor is used if one is not provided (the two-argument version of this function).

Type Parameters:

Rows - Number of rows in the result of f(x, u).

Parameters:

rows - Number of rows in the result of f(x, u).

u - Same control input used in the predict step.

y - Measurement vector.

h - A vector-valued function of x and u that returns the measurement vector.

R - Continuous measurement noise covariance matrix.

residualFuncY - A function that computes the residual of two measurement vectors (i.e. it subtracts them.)

addFuncX - A function that adds two state vectors.