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
All Methods Instance Methods Concrete Methods 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.
-
-
-
Constructor Detail
-
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 Detail
-
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.
-
-