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}