[wpimath] Implement Scaled Spherical Simplex Filter (S3F) (#8091)

Adds S3SigmaPoints based on MerweScaledSigmaPoints. In addition, restructures UnscentedKalmanFilter to support different sigma point generators and provides MerweUKF and S3UKF for convenience when working with either kind of filter.

S3UKFTest is copied from MerweUKFTest (which is a rename of UnscentedKalmanFilterTest). Curiously, however, in Java the original tolerance used in MerweUKFTest.testDriveConvergence() for the final rotation was too low for S3UKFTest, so the tolerance is increased from 0.000005 (5e-6) radians to 0.00015 (1.5e-4) radians. However, the C++ version still uses the original tolerance. (This difference is probably because Java uses a final rotation of 5.846 degrees while C++ uses a final rotation of 5.846 radians)

Closes #8072.

Breaking changes:

- (C++) UnscentedKalmanFilter has a new template parameter for the sigma point generator type.
- (Java) UnscentedKalmanFilter has an additional parameter to every constructor providing an instance of a sigma point generator.
- (C++) int MerweScaledSigmaPoints.NumSigmas() has been replaced with constexpr int MerweScaledSigmaPoints::NumSigmas.
- (C++) The second parameter of SquareRootUnscentedTransform has been changed from States to NumSigmas.

wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h

@@ -15,8 +15,8 @@ namespace frc {
  * 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.
+ * version seen in most publications. S3SigmaPoints is generally preferred due
+ * to its greater performance with nearly identical accuracy.
  *
  * [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilistic
  *     Inference in Dynamic State-Space Models" (Doctoral dissertation)
@@ -27,6 +27,8 @@ namespace frc {
 template <int States>
 class MerweScaledSigmaPoints {
  public:
+  static constexpr int NumSigmas = 2 * States + 1;
+
   /**
    * Constructs a generator for Van der Merwe scaled sigma points.
    *
@@ -44,11 +46,6 @@ class MerweScaledSigmaPoints {
     ComputeWeights(beta);
   }
 
-  /**
-   * Returns number of sigma points for each variable in the state x.
-   */
-  int NumSigmas() { return 2 * States + 1; }
-
   /**
    * Computes the sigma points for an unscented Kalman filter given the mean
    * (x) and square-root covariance (S) of the filter.

wpimath/src/main/native/include/frc/estimator/MerweUKF.h

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/estimator/MerweScaledSigmaPoints.h"
+#include "frc/estimator/UnscentedKalmanFilter.h"
+
+namespace frc {
+
+template <int States, int Inputs, int Outputs>
+using MerweUKF = UnscentedKalmanFilter<States, Inputs, Outputs,
+                                       MerweScaledSigmaPoints<States>>;
+
+// Because MerweUKF is a type alias and not a class, we have to use
+// UnscentedKalmanFilter instead
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    UnscentedKalmanFilter<3, 3, 1, MerweScaledSigmaPoints<3>>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    UnscentedKalmanFilter<5, 3, 3, MerweScaledSigmaPoints<5>>;
+
+}  // namespace frc

wpimath/src/main/native/include/frc/estimator/S3SigmaPoints.h

@@ -0,0 +1,135 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/array.h>
+
+#include "frc/EigenCore.h"
+#include "frc/estimator/SigmaPoints.h"
+
+namespace frc {
+
+/**
+ * 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"
+ *
+ * @tparam States The dimenstionality of the state. States + 2 weights will be
+ * generated.
+ */
+template <int States>
+class S3SigmaPoints {
+ public:
+  static constexpr int NumSigmas = States + 2;
+
+  /**
+   * Constructs a generator for Papakonstantinou sigma points.
+   *
+   * @param alpha Determines the spread of the sigma points around the mean.
+   * Usually a small positive value (1e-3).
+   * @param beta Incorporates prior knowledge of the distribution of the mean.
+   * For Gaussian distributions, beta = 2 is optimal.
+   */
+  explicit S3SigmaPoints(double alpha = 1e-3, double beta = 2)
+      : m_alpha{alpha} {
+    ComputeWeights(beta);
+  }
+
+  /**
+   * Computes the sigma points for an unscented Kalman filter given the mean (x)
+   * and square-root covariance (S) of the filter.
+   *
+   * @param x An array of the means.
+   * @param S Square-root covariance of the filter.
+   *
+   * @return 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+1}.
+   */
+  Matrixd<States, NumSigmas> SquareRootSigmaPoints(
+      const Vectord<States>& x, const Matrixd<States, States>& S) const {
+    // table (1), equation (12)
+    wpi::array<double, States> q(wpi::empty_array);
+    for (size_t t = 1; t <= States; ++t) {
+      q[t - 1] = m_alpha * std::sqrt(static_cast<double>(t * (States + 1)) /
+                                     static_cast<double>(t + 1));
+    }
+
+    Matrixd<States, NumSigmas> C;
+    C.template block<States, 1>(0, 0) = Vectord<States>::Constant(0.0);
+    for (int col = 1; col < NumSigmas; ++col) {
+      for (int row = 0; row < States; ++row) {
+        if (row < col - 2) {
+          C(row, col) = 0.0;
+        } else if (row == col - 2) {
+          C(row, col) = q[row];
+        } else {
+          C(row, col) = -q[row] / (row + 1);
+        }
+      }
+    }
+
+    Matrixd<States, NumSigmas> sigmas;
+    for (int col = 0; col < NumSigmas; ++col) {
+      sigmas.col(col) = x + S * C.col(col);
+    }
+
+    return sigmas;
+  }
+
+  /**
+   * Returns the weight for each sigma point for the mean.
+   */
+  const Vectord<NumSigmas>& Wm() const { return m_Wm; }
+
+  /**
+   * Returns an element of the weight for each sigma point for the mean.
+   *
+   * @param i Element of vector to return.
+   */
+  double Wm(int i) const { return m_Wm(i, 0); }
+
+  /**
+   * Returns the weight for each sigma point for the covariance.
+   */
+  const Vectord<NumSigmas>& Wc() const { return m_Wc; }
+
+  /**
+   * Returns an element of the weight for each sigma point for the covariance.
+   *
+   * @param i Element of vector to return.
+   */
+  double Wc(int i) const { return m_Wc(i, 0); }
+
+ private:
+  Vectord<NumSigmas> m_Wm;
+  Vectord<NumSigmas> m_Wc;
+  double m_alpha;
+
+  /**
+   * Computes the weights for the scaled unscented Kalman filter.
+   *
+   * @param beta Incorporates prior knowledge of the distribution of the mean.
+   */
+  void ComputeWeights(double beta) {
+    double alpha_sq = m_alpha * m_alpha;
+
+    double c = 1.0 / (alpha_sq * (States + 1));
+    m_Wm = Vectord<NumSigmas>::Constant(c);
+    m_Wc = Vectord<NumSigmas>::Constant(c);
+
+    m_Wm(0) = 1.0 - 1.0 / alpha_sq;
+    m_Wc(0) = 1.0 - 1.0 / alpha_sq + (1 - alpha_sq + beta);
+  }
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/estimator/S3UKF.h

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/estimator/S3SigmaPoints.h"
+#include "frc/estimator/UnscentedKalmanFilter.h"
+
+namespace frc {
+
+template <int States, int Inputs, int Outputs>
+using S3UKF =
+    UnscentedKalmanFilter<States, Inputs, Outputs, S3SigmaPoints<States>>;
+
+// Because S3UKF is a type alias and not a class, we have to use
+// UnscentedKalmanFilter instead
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    UnscentedKalmanFilter<3, 3, 1, S3SigmaPoints<3>>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    UnscentedKalmanFilter<5, 3, 3, S3SigmaPoints<5>>;
+
+}  // namespace frc

wpimath/src/main/native/include/frc/estimator/SigmaPoints.h

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <concepts>
+
+#include "frc/EigenCore.h"
+
+namespace frc {
+
+template <typename T, int States>
+concept SigmaPoints =
+    requires(T t, Vectord<States> x, Matrixd<States, States> S, int i) {
+      { T::NumSigmas } -> std::convertible_to<const int>;
+      {
+        t.SquareRootSigmaPoints(x, S)
+      } -> std::same_as<Matrixd<States, T::NumSigmas>>;
+      { t.Wm() } -> std::convertible_to<Vectord<T::NumSigmas>>;
+      { t.Wm(i) } -> std::same_as<double>;
+      { t.Wc() } -> std::convertible_to<Vectord<T::NumSigmas>>;
+      { t.Wc(i) } -> std::same_as<double>;
+    } && std::default_initializable<T>;
+
+}  // namespace frc

wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h

@@ -13,7 +13,7 @@
 
 #include "frc/EigenCore.h"
 #include "frc/StateSpaceUtil.h"
-#include "frc/estimator/MerweScaledSigmaPoints.h"
+#include "frc/estimator/SigmaPoints.h"
 #include "frc/estimator/UnscentedTransform.h"
 #include "frc/system/Discretization.h"
 #include "frc/system/NumericalIntegration.h"
@@ -28,6 +28,11 @@ namespace frc {
  * be measured directly as a result of sensor noise, or because the state is
  * "hidden".
  *
+ * This class requires a SigmaPoints template parameter. For convenience, S3UKF
+ * and MerweUKF type aliases are provided to specify a suitable generator for
+ * you. S3UKF is generally preferred over MerweUKF because of its greater
+ * performance while maintaining nearly identical accuracy.
+ *
  * Kalman filters use a K gain matrix to determine whether to trust the model or
  * measurements more. Kalman filter theory uses statistics to compute an optimal
  * K gain which minimizes the sum of squares error in the state estimate. This K
@@ -51,10 +56,13 @@ namespace frc {
  * @tparam States Number of states.
  * @tparam Inputs Number of inputs.
  * @tparam Outputs Number of outputs.
+ * @tparam SigmaPoints Type used to generate sigma points and weights.
  */
-template <int States, int Inputs, int Outputs>
+template <int States, int Inputs, int Outputs, SigmaPoints<States> SigmaPoints>
 class UnscentedKalmanFilter {
  public:
+  static constexpr int NumSigmas = SigmaPoints::NumSigmas;
+
   using StateVector = Vectord<States>;
   using InputVector = Vectord<Inputs>;
   using OutputVector = Vectord<Outputs>;
@@ -87,12 +95,12 @@ class UnscentedKalmanFilter {
       : m_f(std::move(f)), m_h(std::move(h)) {
     m_contQ = MakeCovMatrix(stateStdDevs);
     m_contR = MakeCovMatrix(measurementStdDevs);
-    m_meanFuncX = [](const Matrixd<States, 2 * States + 1>& sigmas,
-                     const Vectord<2 * States + 1>& Wm) -> StateVector {
+    m_meanFuncX = [](const Matrixd<States, NumSigmas>& sigmas,
+                     const Vectord<NumSigmas>& Wm) -> StateVector {
       return sigmas * Wm;
     };
-    m_meanFuncY = [](const Matrixd<Outputs, 2 * States + 1>& sigmas,
-                     const Vectord<2 * States + 1>& Wc) -> OutputVector {
+    m_meanFuncY = [](const Matrixd<Outputs, NumSigmas>& sigmas,
+                     const Vectord<NumSigmas>& Wc) -> OutputVector {
       return sigmas * Wc;
     };
     m_residualFuncX = [](const StateVector& a,
@@ -141,11 +149,11 @@ class UnscentedKalmanFilter {
       std::function<StateVector(const StateVector&, const InputVector&)> f,
       std::function<OutputVector(const StateVector&, const InputVector&)> h,
       const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
-      std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
-                                const Vectord<2 * States + 1>&)>
+      std::function<StateVector(const Matrixd<States, NumSigmas>&,
+                                const Vectord<NumSigmas>&)>
           meanFuncX,
-      std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
-                                 const Vectord<2 * States + 1>&)>
+      std::function<OutputVector(const Matrixd<Outputs, NumSigmas>&,
+                                 const Vectord<NumSigmas>&)>
           meanFuncY,
       std::function<StateVector(const StateVector&, const StateVector&)>
           residualFuncX,
@@ -257,7 +265,7 @@ class UnscentedKalmanFilter {
     // Generate sigma points around the state mean
     //
     // equation (17)
-    Matrixd<States, 2 * States + 1> sigmas =
+    Matrixd<States, NumSigmas> sigmas =
         m_pts.SquareRootSigmaPoints(m_xHat, m_S);
 
     // Project each sigma point forward in time according to the
@@ -267,7 +275,7 @@ class UnscentedKalmanFilter {
     //   sigmasF = 𝒳ₖ,ₖ₋₁ or just 𝒳 for readability
     //
     // equation (18)
-    for (int i = 0; i < m_pts.NumSigmas(); ++i) {
+    for (int i = 0; i < NumSigmas; ++i) {
       StateVector x = sigmas.template block<States, 1>(0, i);
       m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
     }
@@ -276,7 +284,7 @@ class UnscentedKalmanFilter {
     // to compute the prior state mean and covariance
     //
     // equations (18) (19) and (20)
-    auto [xHat, S] = SquareRootUnscentedTransform<States, States>(
+    auto [xHat, S] = SquareRootUnscentedTransform<States, NumSigmas>(
         m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX,
         discQ.template triangularView<Eigen::Lower>());
     m_xHat = xHat;
@@ -327,8 +335,8 @@ class UnscentedKalmanFilter {
       const InputVector& u, const Vectord<Rows>& y,
       std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
       const Matrixd<Rows, Rows>& R) {
-    auto meanFuncY = [](const Matrixd<Outputs, 2 * States + 1>& sigmas,
-                        const Vectord<2 * States + 1>& Wc) -> Vectord<Rows> {
+    auto meanFuncY = [](const Matrixd<Outputs, NumSigmas>& sigmas,
+                        const Vectord<NumSigmas>& Wc) -> Vectord<Rows> {
       return sigmas * Wc;
     };
     auto residualFuncX = [](const StateVector& a,
@@ -358,7 +366,7 @@ class UnscentedKalmanFilter {
    * @param h             A vector-valued function of x and u that returns the
    *                      measurement vector.
    * @param R             Continuous measurement noise covariance matrix.
-   * @param meanFuncY     A function that computes the mean of 2 * States + 1
+   * @param meanFuncY     A function that computes the mean of NumSigmas
    *                      measurement vectors using a given set of weights.
    * @param residualFuncY A function that computes the residual of two
    *                      measurement vectors (i.e. it subtracts them.)
@@ -371,8 +379,8 @@ class UnscentedKalmanFilter {
       const InputVector& u, const Vectord<Rows>& y,
       std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
       const Matrixd<Rows, Rows>& R,
-      std::function<Vectord<Rows>(const Matrixd<Rows, 2 * States + 1>&,
-                                  const Vectord<2 * States + 1>&)>
+      std::function<Vectord<Rows>(const Matrixd<Rows, NumSigmas>&,
+                                  const Vectord<NumSigmas>&)>
           meanFuncY,
       std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
           residualFuncY,
@@ -392,10 +400,10 @@ class UnscentedKalmanFilter {
     // This differs from equation (22) which uses
     // the prior sigma points, regenerating them allows
     // multiple measurement updates per time update
-    Matrixd<Rows, 2 * States + 1> sigmasH;
-    Matrixd<States, 2 * States + 1> sigmas =
+    Matrixd<Rows, NumSigmas> sigmasH;
+    Matrixd<States, NumSigmas> sigmas =
         m_pts.SquareRootSigmaPoints(m_xHat, m_S);
-    for (int i = 0; i < m_pts.NumSigmas(); ++i) {
+    for (int i = 0; i < NumSigmas; ++i) {
       sigmasH.template block<Rows, 1>(0, i) =
           h(sigmas.template block<States, 1>(0, i), u);
     }
@@ -405,7 +413,7 @@ class UnscentedKalmanFilter {
     // covariance.
     //
     // equations (23) (24) and (25)
-    auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, States>(
+    auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, NumSigmas>(
         sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY,
         discR.template triangularView<Eigen::Lower>());
 
@@ -419,7 +427,7 @@ class UnscentedKalmanFilter {
     // equation (26)
     Matrixd<States, Rows> Pxy;
     Pxy.setZero();
-    for (int i = 0; i < m_pts.NumSigmas(); ++i) {
+    for (int i = 0; i < NumSigmas; ++i) {
       Pxy +=
           m_pts.Wc(i) *
           (residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
@@ -467,11 +475,11 @@ class UnscentedKalmanFilter {
  private:
   std::function<StateVector(const StateVector&, const InputVector&)> m_f;
   std::function<OutputVector(const StateVector&, const InputVector&)> m_h;
-  std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
-                            const Vectord<2 * States + 1>&)>
+  std::function<StateVector(const Matrixd<States, NumSigmas>&,
+                            const Vectord<NumSigmas>&)>
       m_meanFuncX;
-  std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
-                             const Vectord<2 * States + 1>&)>
+  std::function<OutputVector(const Matrixd<Outputs, NumSigmas>&,
+                             const Vectord<NumSigmas>&)>
       m_meanFuncY;
   std::function<StateVector(const StateVector&, const StateVector&)>
       m_residualFuncX;
@@ -482,15 +490,10 @@ class UnscentedKalmanFilter {
   StateMatrix m_S;
   StateMatrix m_contQ;
   Matrixd<Outputs, Outputs> m_contR;
-  Matrixd<States, 2 * States + 1> m_sigmasF;
+  Matrixd<States, NumSigmas> m_sigmasF;
   units::second_t m_dt;
 
-  MerweScaledSigmaPoints<States> m_pts;
+  SigmaPoints m_pts;
 };
 
-extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
-    UnscentedKalmanFilter<3, 3, 1>;
-extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
-    UnscentedKalmanFilter<5, 3, 3>;
-
 }  // namespace frc

wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h

@@ -22,11 +22,11 @@ namespace frc {
  *
  * @tparam CovDim      Dimension of covariance of sigma points after passing
  *                     through the transform.
- * @tparam States      Number of states.
+ * @tparam NumSigmas   Number of sigma points.
  * @param sigmas       List of sigma points.
  * @param Wm           Weights for the mean.
  * @param Wc           Weights for the covariance.
- * @param meanFunc     A function that computes the mean of 2 * States + 1 state
+ * @param meanFunc     A function that computes the mean of NumSigmas state
  *                     vectors using a given set of weights.
  * @param residualFunc A function that computes the residual of two state
  *                     vectors (i.e. it subtracts them.)
@@ -35,13 +35,13 @@ namespace frc {
  * @return Tuple of x, mean of sigma points; S, square-root covariance of
  * sigmas.
  */
-template <int CovDim, int States>
+template <int CovDim, int NumSigmas>
 std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>>
 SquareRootUnscentedTransform(
-    const Matrixd<CovDim, 2 * States + 1>& sigmas,
-    const Vectord<2 * States + 1>& Wm, const Vectord<2 * States + 1>& Wc,
-    std::function<Vectord<CovDim>(const Matrixd<CovDim, 2 * States + 1>&,
-                                  const Vectord<2 * States + 1>&)>
+    const Matrixd<CovDim, NumSigmas>& sigmas, const Vectord<NumSigmas>& Wm,
+    const Vectord<NumSigmas>& Wc,
+    std::function<Vectord<CovDim>(const Matrixd<CovDim, NumSigmas>&,
+                                  const Vectord<NumSigmas>&)>
         meanFunc,
     std::function<Vectord<CovDim>(const Vectord<CovDim>&,
                                   const Vectord<CovDim>&)>
@@ -62,13 +62,18 @@ SquareRootUnscentedTransform(
   //   [√{W₁⁽ᶜ⁾}(𝒳_{1:2L} - x̂) √{Rᵛ}]
   //
   // the part of equations (20) and (24) within the "qr{}"
-  Matrixd<CovDim, States * 2 + CovDim> Sbar;
-  for (int i = 0; i < States * 2; i++) {
+  //
+  // Note that we allow a custom function instead of the difference to allow
+  // angle wrapping. Furthermore, we allow an arbitrary number of sigma points
+  // to support similar methods such as the Scaled Spherical Simplex Filter
+  // (S3F).
+  Matrixd<CovDim, NumSigmas - 1 + CovDim> Sbar;
+  for (int i = 0; i < NumSigmas - 1; i++) {
     Sbar.template block<CovDim, 1>(0, i) =
         std::sqrt(Wc[1]) *
         residualFunc(sigmas.template block<CovDim, 1>(0, 1 + i), x);
   }
-  Sbar.template block<CovDim, CovDim>(0, States * 2) = squareRootR;
+  Sbar.template block<CovDim, CovDim>(0, NumSigmas - 1) = squareRootR;
 
   // Compute the square-root covariance of the sigma points.
   //