Package edu.wpi.first.math.estimator

Class MerweUKF<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.MerweUKF<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 MerweUKF<States extends Num,Inputs extends Num,Outputs extends Num>
extends UnscentedKalmanFilter<States,Inputs,Outputs>

An Unscented Kalman Filter using sigma points and weights from Van der Merwe's 2004 dissertation. S3UKF is generally preferred due to its greater performance with nearly identical accuracy.

Constructor Summary

Constructors

Constructor	Description
MerweUKF(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 Merwe Unscented Kalman Filter.
MerweUKF(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 Merwe 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

MerweUKF

public MerweUKF(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 Merwe 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.

MerweUKF

public MerweUKF(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 Merwe 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.