001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.estimator;
006
007import edu.wpi.first.math.Matrix;
008import edu.wpi.first.math.Num;
009import edu.wpi.first.math.numbers.N1;
010
011/**
012 * A sigma points generator for the UnscentedKalmanFilter class.
013 *
014 * @param <States> The dimensionality of the state.
015 */
016public interface SigmaPoints<States extends Num> {
017  /**
018   * Returns number of sigma points for each variable in the state x.
019   *
020   * @return The number of sigma points for each variable in the state x.
021   */
022  int getNumSigmas();
023
024  /**
025   * Computes the sigma points for an unscented Kalman filter given the mean (x) and square-root
026   * covariance (s) of the filter.
027   *
028   * @param x An array of the means.
029   * @param s Square-root covariance of the filter.
030   * @return Two-dimensional array of sigma points. Each column contains all the sigmas for one
031   *     dimension in the problem space. Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
032   */
033  Matrix<States, ?> squareRootSigmaPoints(Matrix<States, N1> x, Matrix<States, States> s);
034
035  /**
036   * Returns the weight for each sigma point for the mean.
037   *
038   * @return the weight for each sigma point for the mean.
039   */
040  Matrix<?, N1> getWm();
041
042  /**
043   * Returns an element of the weight for each sigma point for the mean.
044   *
045   * @param element Element of vector to return.
046   * @return the element i's weight for the mean.
047   */
048  double getWm(int element);
049
050  /**
051   * Returns the weight for each sigma point for the covariance.
052   *
053   * @return the weight for each sigma point for the covariance.
054   */
055  Matrix<?, N1> getWc();
056
057  /**
058   * Returns an element of the weight for each sigma point for the covariance.
059   *
060   * @param element Element of vector to return.
061   * @return The element I's weight for the covariance.
062   */
063  double getWc(int element);
064}