Class ExtendedKalmanFilter<States extends Num,Inputs extends Num,Outputs extends Num>
public class ExtendedKalmanFilter<States extends Num,Inputs extends Num,Outputs extends Num> extends Object
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.<Rows extends Num>
voidcorrect(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>
voidcorrect(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.
-
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.- 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.- 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
Returns the error covariance matrix P.- Returns:
- the error covariance matrix P.
-
getP
Returns an element of the error covariance matrix P.- Parameters:
row
- Row of P.col
- Column of P.- Returns:
- the value of the error covariance matrix P at (i, j).
-
setP
Sets the entire error covariance matrix P.- Parameters:
newP
- The new value of P to use.
-
getXhat
Returns the state estimate x-hat.- Returns:
- the state estimate x-hat.
-
getXhat
Returns an element of the state estimate x-hat.- Parameters:
row
- Row of x-hat.- Returns:
- the value of the state estimate x-hat at i.
-
setXhat
Set initial state estimate x-hat.- Parameters:
xHat
- The state estimate x-hat.
-
setXhat
Set an element of the initial state estimate x-hat.- Parameters:
row
- Row of x-hat.value
- Value for element of x-hat.
-
reset
-
predict
Project the model into the future with a new control input u.- 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 linearlize the model.dtSeconds
- Timestep for prediction.
-
correct
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
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
- Discrete 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
- Discrete 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.
-