Package edu.wpi.first.math.estimator

Class MerweScaledSigmaPoints<S extends Num>

java.lang.Object
edu.wpi.first.math.estimator.MerweScaledSigmaPoints<S>
public class MerweScaledSigmaPoints<S extends Num>
extends Object
Generates sigma points and weights according to Van der Merwe's 2004 dissertation[1] for the UnscentedKalmanFilter class.

It parametrizes the sigma points using alpha, beta, kappa terms, and is the version seen in most publications. Unless you know better, this should be your default choice.

States is the dimensionality of the state. 2*States+1 weights will be generated.

[1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic Inference in Dynamic State-Space Models" (Doctoral dissertation)

  • Constructor Summary

    Constructors 
    Constructor Description
    MerweScaledSigmaPoints​(Nat<S> states)
    Constructs a generator for Van der Merwe scaled sigma points with default values for alpha, beta, and kappa.
    MerweScaledSigmaPoints​(Nat<S> states, double alpha, double beta, int kappa)
    Constructs a generator for Van der Merwe scaled 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<S,​?> sigmaPoints​(Matrix<S,​N1> x, Matrix<S,​S> P)
    Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P) of the filter.

    Methods inherited from class java.lang.Object

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

  • Constructor Details

    • MerweScaledSigmaPoints

      public MerweScaledSigmaPoints​(Nat<S> states, double alpha, double beta, int kappa)
      Constructs a generator for Van der Merwe scaled 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.
      kappa - Secondary scaling parameter usually set to 0 or 3 - States.

    • MerweScaledSigmaPoints

      public MerweScaledSigmaPoints​(Nat<S> states)
      Constructs a generator for Van der Merwe scaled 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.
      Returns:
      The number of sigma points for each variable in the state x.

    • sigmaPoints

      public Matrix<S,​?> sigmaPoints​(Matrix<S,​N1> x, Matrix<S,​S> P)
      Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P) of the filter.
      Parameters:
      x - An array of the means.
      P - Covariance of the filter.
      Returns:
      Two dimensional array of sigma points. Each column contains all of 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.
      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.
      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.
      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.
      Parameters:
      element - Element of vector to return.
      Returns:
      The element I's weight for the covariance.