Package edu.wpi.first.math.estimator

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

java.lang.Object

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

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

Type Parameters:

States - Number of states.

Inputs - Number of inputs.

Outputs - Number of outputs.

All Implemented Interfaces:

KalmanTypeFilter<States,Inputs,Outputs>

public class S3UKF<States extends Num,Inputs extends Num,Outputs extends Num>
extends UnscentedKalmanFilter<States,Inputs,Outputs>

An Unscented Kalman Filter using sigma points and weights from Papakonstantinou's paper. This is generally preferred over other UKF variants due to its high accuracy and performance.

Constructor Summary

Constructors

Constructor	Description
S3UKF(Nat<States> states, 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 nominalDt)	Constructs a Scaled Spherical Simplex (S3) Unscented Kalman Filter.
S3UKF(Nat<States> states, 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<States,?>,Matrix<?,N1>,Matrix<States,N1>> meanFuncX, BiFunction<Matrix<Outputs,?>,Matrix<?,N1>,Matrix<Outputs,N1>> meanFuncY, BiFunction<Matrix<States,N1>,Matrix<States,N1>,Matrix<States,N1>> residualFuncX, BiFunction<Matrix<Outputs,N1>,Matrix<Outputs,N1>,Matrix<Outputs,N1>> residualFuncY, BiFunction<Matrix<States,N1>,Matrix<States,N1>,Matrix<States,N1>> addFuncX, double nominalDt)	Constructs a Scaled Spherical Simplex (S3) Unscented Kalman filter with custom mean, residual, and addition functions.

Method Summary

Methods inherited from class edu.wpi.first.math.estimator.UnscentedKalmanFilter

correct, correct, correct, correct, getP, getP, getS, getS, getXhat, getXhat, predict, reset, setP, setS, setXhat, setXhat

Methods inherited from class java.lang.Object

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

Constructor Details

S3UKF

public S3UKF(Nat<States> states,
 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 nominalDt)

Constructs a Scaled Spherical Simplex (S3) Unscented 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.

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.

nominalDt - Nominal discretization timestep in seconds.

S3UKF

public S3UKF(Nat<States> states,
 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<States,?>,Matrix<?,N1>,Matrix<States,N1>> meanFuncX,
 BiFunction<Matrix<Outputs,?>,Matrix<?,N1>,Matrix<Outputs,N1>> meanFuncY,
 BiFunction<Matrix<States,N1>,Matrix<States,N1>,Matrix<States,N1>> residualFuncX,
 BiFunction<Matrix<Outputs,N1>,Matrix<Outputs,N1>,Matrix<Outputs,N1>> residualFuncY,
 BiFunction<Matrix<States,N1>,Matrix<States,N1>,Matrix<States,N1>> addFuncX,
 double nominalDt)

Constructs a Scaled Spherical Simplex (S3) Unscented Kalman filter with custom mean, residual, and addition functions. Using custom functions for arithmetic can be useful if you have angles in the state or measurements, because they allow you to correctly account for the modular nature of angle arithmetic.

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.

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.

meanFuncX - A function that computes the mean of NumSigmas state vectors using a given set of weights.

meanFuncY - A function that computes the mean of NumSigmas measurement vectors using a given set of weights.

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

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

addFuncX - A function that adds two state vectors.

nominalDt - Nominal discretization timestep in seconds.