[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/cpp/estimator/MerweUKF.cpp

@@ -0,0 +1,14 @@
+// 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.
+
+#include "frc/estimator/MerweUKF.h"
+
+namespace frc {
+
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+    UnscentedKalmanFilter<3, 3, 1, MerweScaledSigmaPoints<3>>;
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+    UnscentedKalmanFilter<5, 3, 3, MerweScaledSigmaPoints<5>>;
+
+}  // namespace frc

wpimath/src/main/native/cpp/estimator/S3UKF.cpp

@@ -2,13 +2,13 @@
 // 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.
 
-#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/estimator/S3UKF.h"
 
 namespace frc {
 
 template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
-    UnscentedKalmanFilter<3, 3, 1>;
+    UnscentedKalmanFilter<3, 3, 1, S3SigmaPoints<3>>;
 template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
-    UnscentedKalmanFilter<5, 3, 3>;
+    UnscentedKalmanFilter<5, 3, 3, S3SigmaPoints<5>>;
 
 }  // namespace frc