Package edu.wpi.first.math.estimator

Class S3SigmaPoints<States extends Num>

java.lang.Object
edu.wpi.first.math.estimator.S3SigmaPoints<States>
Type Parameters:
States - The dimenstionality of the state. States + 2 weights will be generated.
All Implemented Interfaces:
SigmaPoints<States>
public class S3SigmaPoints<States extends Num> extends Object implements SigmaPoints<States>
Generates sigma points and weights according to Papakonstantinou's paper[1] for the UnscentedKalmanFilter class.

It parameterizes the sigma points using alpha and beta terms. Unless you know better, this should be your default choice due to its high accuracy and performance.

[1] K. Papakonstantinou "A Scaled Spherical Simplex Filter (S3F) with a decreased n + 2 sigma points set size and equivalent 2n + 1 Unscented Kalman Filter (UKF) accuracy"

  • Constructor Summary

    Constructors
    Constructor
    Description
    S3SigmaPoints(Nat<States> states)
    Constructs a generator for Papakonstantinou sigma points with default values for alpha, beta, and kappa.
    S3SigmaPoints(Nat<States> states, double alpha, double beta)
    Constructs a generator for Papakonstantinou sigma points.

  • Method Summary

    Modifier and Type
    Method
    Description
    int
    getNumSigmas()
    Returns number of sigma points for each variable in the state x.
    Matrix<?,N1>
    getWc()
    Returns the weight for each sigma point for the covariance.
    double
    getWc(int element)
    Returns an element of the weight for each sigma point for the covariance.
    Matrix<?,N1>
    getWm()
    Returns the weight for each sigma point for the mean.
    double
    getWm(int element)
    Returns an element of the weight for each sigma point for the mean.
    Matrix<States,?>
    squareRootSigmaPoints(Matrix<States,N1> x, Matrix<States,States> s)
    Computes the sigma points for an unscented Kalman filter given the mean (x) and square-root covariance (s) of the filter.

    Methods inherited from class java.lang.Object

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

  • Constructor Details

    • S3SigmaPoints

      public S3SigmaPoints(Nat<States> states, double alpha, double beta)
      Constructs a generator for Papakonstantinou sigma points.
      Parameters:
      states - an instance of Num that represents the number of states.
      alpha - Determines the spread of the sigma points around the mean. Usually a small positive value (1e-3).
      beta - Incorporates prior knowledge of the distribution of the mean. For Gaussian distributions, beta = 2 is optimal.

    • S3SigmaPoints

      public S3SigmaPoints(Nat<States> states)
      Constructs a generator for Papakonstantinou sigma points with default values for alpha, beta, and kappa.
      Parameters:
      states - an instance of Num that represents the number of states.

  • Method Details

    • getNumSigmas

      public int getNumSigmas()
      Returns number of sigma points for each variable in the state x.
      Specified by:
      getNumSigmas in interface SigmaPoints<States extends Num>
      Returns:
      The number of sigma points for each variable in the state x.

    • squareRootSigmaPoints

      public Matrix<States,?> squareRootSigmaPoints(Matrix<States,N1> x, Matrix<States,States> s)
      Computes the sigma points for an unscented Kalman filter given the mean (x) and square-root covariance (s) of the filter.
      Specified by:
      squareRootSigmaPoints in interface SigmaPoints<States extends Num>
      Parameters:
      x - An array of the means.
      s - Square-root covariance of the filter.
      Returns:
      Two-dimensional array of sigma points. Each column contains all the sigmas for one dimension in the problem space. Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}.

    • getWm

      public Matrix<?,N1> getWm()
      Returns the weight for each sigma point for the mean.
      Specified by:
      getWm in interface SigmaPoints<States extends Num>
      Returns:
      the weight for each sigma point for the mean.

    • getWm

      public double getWm(int element)
      Returns an element of the weight for each sigma point for the mean.
      Specified by:
      getWm in interface SigmaPoints<States extends Num>
      Parameters:
      element - Element of vector to return.
      Returns:
      the element i's weight for the mean.

    • getWc

      public Matrix<?,N1> getWc()
      Returns the weight for each sigma point for the covariance.
      Specified by:
      getWc in interface SigmaPoints<States extends Num>
      Returns:
      the weight for each sigma point for the covariance.

    • getWc

      public double getWc(int element)
      Returns an element of the weight for each sigma point for the covariance.
      Specified by:
      getWc in interface SigmaPoints<States extends Num>
      Parameters:
      element - Element of vector to return.
      Returns:
      The element I's weight for the covariance.