ExtendedKalmanFilter_2_2_2

class wpimath.estimator.ExtendedKalmanFilter_2_2_2(*args, **kwargs)

Bases: pybind11_object

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

@tparam States The number of states. @tparam Inputs The number of inputs. @tparam Outputs The number of outputs.

Overloaded function.

  1. __init__(self: wpimath._controls._controls.estimator.ExtendedKalmanFilter_2_2_2, f: Callable[[numpy.ndarray[numpy.float64[2, 1]], numpy.ndarray[numpy.float64[2, 1]]], numpy.ndarray[numpy.float64[2, 1]]], h: Callable[[numpy.ndarray[numpy.float64[2, 1]], numpy.ndarray[numpy.float64[2, 1]]], numpy.ndarray[numpy.float64[2, 1]]], stateStdDevs: Tuple[float, float], measurementStdDevs: Tuple[float, float], dt: seconds) -> None

Constructs an extended Kalman filter.

Parameters:
  • 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.

  • dt – Nominal discretization timestep.

  1. __init__(self: wpimath._controls._controls.estimator.ExtendedKalmanFilter_2_2_2, f: Callable[[numpy.ndarray[numpy.float64[2, 1]], numpy.ndarray[numpy.float64[2, 1]]], numpy.ndarray[numpy.float64[2, 1]]], h: Callable[[numpy.ndarray[numpy.float64[2, 1]], numpy.ndarray[numpy.float64[2, 1]]], numpy.ndarray[numpy.float64[2, 1]]], stateStdDevs: Tuple[float, float], measurementStdDevs: Tuple[float, float], residualFuncY: Callable[[numpy.ndarray[numpy.float64[2, 1]], numpy.ndarray[numpy.float64[2, 1]]], numpy.ndarray[numpy.float64[2, 1]]], addFuncX: Callable[[numpy.ndarray[numpy.float64[2, 1]], numpy.ndarray[numpy.float64[2, 1]]], numpy.ndarray[numpy.float64[2, 1]]], dt: seconds) -> None

Constructs an extended Kalman filter.

Parameters:
  • 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.

  • dt – Nominal discretization timestep.

P(*args, **kwargs)

Overloaded function.

  1. P(self: wpimath._controls._controls.estimator.ExtendedKalmanFilter_2_2_2) -> numpy.ndarray[numpy.float64[2, 2]]

Returns the error covariance matrix P.

  1. P(self: wpimath._controls._controls.estimator.ExtendedKalmanFilter_2_2_2, i: int, j: int) -> float

Returns an element of the error covariance matrix P.

Parameters:
  • i – Row of P.

  • j – Column of P.

correct(u: numpy.ndarray[numpy.float64[2, 1]], y: numpy.ndarray[numpy.float64[2, 1]]) None

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

Parameters:
  • u – Same control input used in the predict step.

  • y – Measurement vector.

predict(u: numpy.ndarray[numpy.float64[2, 1]], dt: seconds) None

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

Parameters:
  • u – New control input from controller.

  • dt – Timestep for prediction.

reset() None

Resets the observer.

setP(P: numpy.ndarray[numpy.float64[2, 2]]) None

Set the current error covariance matrix P.

Parameters:

P – The error covariance matrix P.

setXhat(*args, **kwargs)

Overloaded function.

  1. setXhat(self: wpimath._controls._controls.estimator.ExtendedKalmanFilter_2_2_2, xHat: numpy.ndarray[numpy.float64[2, 1]]) -> None

Set initial state estimate x-hat.

Parameters:

xHat – The state estimate x-hat.

  1. setXhat(self: wpimath._controls._controls.estimator.ExtendedKalmanFilter_2_2_2, i: int, value: float) -> None

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

Parameters:
  • i – Row of x-hat.

  • value – Value for element of x-hat.

xhat(*args, **kwargs)

Overloaded function.

  1. xhat(self: wpimath._controls._controls.estimator.ExtendedKalmanFilter_2_2_2) -> numpy.ndarray[numpy.float64[2, 1]]

Returns the state estimate x-hat.

  1. xhat(self: wpimath._controls._controls.estimator.ExtendedKalmanFilter_2_2_2, i: int) -> float

Returns an element of the state estimate x-hat.

Parameters:

i – Row of x-hat.