[wpimath] Clean up KalmanFilter tests (#8651)

Also synchronize C++ and Java versions of tests.

wpimath/src/test/java/org/wpilib/math/estimator/KalmanFilterTest.java

@@ -4,134 +4,145 @@
 
 package org.wpilib.math.estimator;
 
-import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
 import java.util.List;
-import java.util.Random;
 import org.junit.jupiter.api.Test;
 import org.wpilib.math.geometry.Pose2d;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.linalg.MatBuilder;
 import org.wpilib.math.linalg.Matrix;
 import org.wpilib.math.linalg.VecBuilder;
-import org.wpilib.math.numbers.N1;
-import org.wpilib.math.numbers.N2;
-import org.wpilib.math.numbers.N3;
-import org.wpilib.math.numbers.N6;
-import org.wpilib.math.system.DCMotor;
+import org.wpilib.math.random.Normal;
 import org.wpilib.math.system.LinearSystem;
-import org.wpilib.math.system.Models;
 import org.wpilib.math.trajectory.TrajectoryConfig;
 import org.wpilib.math.trajectory.TrajectoryGenerator;
 import org.wpilib.math.util.Nat;
 
 class KalmanFilterTest {
-  private static LinearSystem<N2, N1, N2> elevatorPlant;
-
-  private static final double kDt = 0.005;
-
-  static {
-    createElevator();
-  }
-
-  private static void createElevator() {
-    var motors = DCMotor.getVex775Pro(2);
-
-    var m = 5.0;
-    var r = 0.0181864;
-    var G = 1.0;
-    elevatorPlant = Models.elevatorFromPhysicalConstants(motors, m, r, G);
-  }
-
-  // A swerve drive system where the states are [x, y, theta, vx, vy, vTheta]ᵀ,
-  // Y is [x, y, theta]ᵀ and u is [ax, ay, alpha}ᵀ
-  LinearSystem<N6, N3, N3> m_swerveObserverSystem =
-      new LinearSystem<>(
-          MatBuilder.fill(
-              Nat.N6(), Nat.N6(), // A
-              0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-              0, 0, 0, 0, 0, 0, 0),
-          MatBuilder.fill(
-              Nat.N6(), Nat.N3(), // B
-              0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1),
-          MatBuilder.fill(
-              Nat.N3(), Nat.N6(), // C
-              1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0),
-          new Matrix<>(Nat.N3(), Nat.N3())); // D
-
   @Test
-  @SuppressWarnings("unchecked")
-  void testElevatorKalmanFilter() {
-    var Q = VecBuilder.fill(0.05, 1.0);
-    var R = VecBuilder.fill(0.0001);
+  void testSwerveStationary() {
+    final var dt = 0.02;
 
-    assertDoesNotThrow(
-        () ->
-            new KalmanFilter<>(
-                Nat.N2(), Nat.N1(), (LinearSystem<N2, N1, N1>) elevatorPlant.slice(0), Q, R, kDt));
-  }
-
-  @Test
-  void testSwerveKFStationary() {
-    var random = new Random();
+    // Swerve drive with x = [x, y, θ, v_x, v_y, ω]ᵀ, u = [a_x, a_y, α]ᵀ,
+    // y = [x, y, θ]ᵀ
+    var plant =
+        new LinearSystem<>(
+            MatBuilder.fill(
+                Nat.N6(),
+                Nat.N6(),
+                // spotless:off
+                0, 0, 0, 1, 0, 0,
+                0, 0, 0, 0, 1, 0,
+                0, 0, 0, 0, 0, 1,
+                0, 0, 0, 0, 0, 0,
+                0, 0, 0, 0, 0, 0,
+                0, 0, 0, 0, 0, 0),
+            // spotless:on
+            MatBuilder.fill(
+                Nat.N6(),
+                Nat.N3(),
+                // spotless:off
+                0, 0, 0,
+                0, 0, 0,
+                0, 0, 0,
+                1, 0, 0,
+                0, 1, 0,
+                0, 0, 1),
+            // spotless:on
+            MatBuilder.fill(
+                Nat.N3(),
+                Nat.N6(),
+                // spotless:off
+                1, 0, 0, 0, 0, 0,
+                0, 1, 0, 0, 0, 0,
+                0, 0, 1, 0, 0, 0),
+            // spotless:on
+            new Matrix<>(Nat.N3(), Nat.N3()));
 
     var filter =
         new KalmanFilter<>(
             Nat.N6(),
             Nat.N3(),
-            m_swerveObserverSystem,
-            VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
-            // weights
-            VecBuilder.fill(2, 2, 2), // measurement weights
-            0.020);
+            plant,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
+            VecBuilder.fill(1.0, 1.0, 1.0),
+            dt);
 
-    Matrix<N3, N1> measurement;
-    for (int i = 0; i < 100; i++) {
-      // the robot is at [0, 0, 0] so we just park here
-      measurement =
-          VecBuilder.fill(random.nextGaussian(), random.nextGaussian(), random.nextGaussian());
-      filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement);
+    var u = VecBuilder.fill(0.0, 0.0, 0.0);
 
-      // we continue to not accelerate
-      filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020);
+    for (int i = 0; i < 100; ++i) {
+      var y = Normal.normal(VecBuilder.fill(1.0, 1.0, 1.0));
+
+      filter.correct(u, y);
+      filter.predict(u, dt);
     }
 
     assertEquals(0.0, filter.getXhat(0), 0.3);
-    assertEquals(0.0, filter.getXhat(0), 0.3);
+    assertEquals(0.0, filter.getXhat(1), 0.3);
   }
 
   @Test
-  void testSwerveKFMovingWithoutAccelerating() {
-    var random = new Random();
+  void testSwerveBadInitialPose() {
+    final var dt = 0.02;
+
+    // Swerve drive with x = [x, y, θ, v_x, v_y, ω]ᵀ, u = [a_x, a_y, α]ᵀ,
+    // y = [x, y, θ]ᵀ
+    var plant =
+        new LinearSystem<>(
+            MatBuilder.fill(
+                Nat.N6(),
+                Nat.N6(),
+                // spotless:off
+                0, 0, 0, 1, 0, 0,
+                0, 0, 0, 0, 1, 0,
+                0, 0, 0, 0, 0, 1,
+                0, 0, 0, 0, 0, 0,
+                0, 0, 0, 0, 0, 0,
+                0, 0, 0, 0, 0, 0),
+            // spotless:on
+            MatBuilder.fill(
+                Nat.N6(),
+                Nat.N3(),
+                // spotless:off
+                0, 0, 0,
+                0, 0, 0,
+                0, 0, 0,
+                1, 0, 0,
+                0, 1, 0,
+                0, 0, 1),
+            // spotless:on
+            MatBuilder.fill(
+                Nat.N3(),
+                Nat.N6(),
+                // spotless:off
+                1, 0, 0, 0, 0, 0,
+                0, 1, 0, 0, 0, 0,
+                0, 0, 1, 0, 0, 0),
+            // spotless:on
+            new Matrix<>(Nat.N3(), Nat.N3()));
 
     var filter =
         new KalmanFilter<>(
             Nat.N6(),
             Nat.N3(),
-            m_swerveObserverSystem,
-            VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
-            // weights
-            VecBuilder.fill(4, 4, 4), // measurement weights
-            0.020);
+            plant,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
+            VecBuilder.fill(0.1, 0.1, 0.25),
+            dt);
 
-    // we set the velocity of the robot so that it's moving forward slowly
+    // Set nonzero position
     filter.setXhat(0, 0.5);
     filter.setXhat(1, 0.5);
 
-    for (int i = 0; i < 300; i++) {
-      // the robot is at [0, 0, 0] so we just park here
-      var measurement =
-          VecBuilder.fill(
-              random.nextGaussian() / 10d,
-              random.nextGaussian() / 10d,
-              random.nextGaussian() / 4d // std dev of [1, 1, 1]
-              );
+    var u = VecBuilder.fill(0.0, 0.0, 0.0);
 
-      filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement);
+    // Let filter converge to zero position
+    for (int i = 0; i < 300; ++i) {
+      var y = Normal.normal(VecBuilder.fill(0.1, 0.1, 0.25));
 
-      // we continue to not accelerate
-      filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020);
+      filter.correct(u, y);
+      filter.predict(u, dt);
     }
 
     assertEquals(0.0, filter.getXhat(0), 0.2);
@@ -139,46 +150,85 @@ class KalmanFilterTest {
   }
 
   @Test
-  void testSwerveKFMovingOverTrajectory() {
-    var random = new Random();
+  void testSwerveMovingOverTrajectory() {
+    final var dt = 0.02;
+
+    // Swerve drive with x = [x, y, θ, v_x, v_y, ω]ᵀ, u = [a_x, a_y, α]ᵀ,
+    // y = [x, y, θ]ᵀ
+    var plant =
+        new LinearSystem<>(
+            MatBuilder.fill(
+                Nat.N6(),
+                Nat.N6(),
+                // spotless:off
+                0, 0, 0, 1, 0, 0,
+                0, 0, 0, 0, 1, 0,
+                0, 0, 0, 0, 0, 1,
+                0, 0, 0, 0, 0, 0,
+                0, 0, 0, 0, 0, 0,
+                0, 0, 0, 0, 0, 0),
+            // spotless:on
+            MatBuilder.fill(
+                Nat.N6(),
+                Nat.N3(),
+                // spotless:off
+                0, 0, 0,
+                0, 0, 0,
+                0, 0, 0,
+                1, 0, 0,
+                0, 1, 0,
+                0, 0, 1),
+            // spotless:on
+            MatBuilder.fill(
+                Nat.N3(),
+                Nat.N6(),
+                // spotless:off
+                1, 0, 0, 0, 0, 0,
+                0, 1, 0, 0, 0, 0,
+                0, 0, 1, 0, 0, 0),
+            // spotless:on
+            new Matrix<>(Nat.N3(), Nat.N3()));
 
     var filter =
         new KalmanFilter<>(
             Nat.N6(),
             Nat.N3(),
-            m_swerveObserverSystem,
-            VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
-            // weights
-            VecBuilder.fill(4, 4, 4), // measurement weights
-            0.020);
+            plant,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
+            VecBuilder.fill(0.2, 0.2, 1.0 / 3.0),
+            dt);
 
     var trajectory =
         TrajectoryGenerator.generateTrajectory(
-            List.of(new Pose2d(0, 0, Rotation2d.kZero), new Pose2d(5, 5, Rotation2d.kZero)),
-            new TrajectoryConfig(2, 2));
-    var time = 0.0;
+            List.of(new Pose2d(0.0, 0.0, Rotation2d.kZero), new Pose2d(5.0, 5.0, Rotation2d.kZero)),
+            new TrajectoryConfig(2.0, 2.0));
+
     var lastVelocity = VecBuilder.fill(0.0, 0.0, 0.0);
 
-    while (time <= trajectory.getTotalTime()) {
-      var sample = trajectory.sample(time);
-      var measurement =
+    for (var t = 0.0; t < trajectory.getTotalTime(); t += dt) {
+      var sample = trajectory.sample(t);
+
+      var y =
           VecBuilder.fill(
-              sample.pose.getTranslation().getX() + random.nextGaussian() / 5d,
-              sample.pose.getTranslation().getY() + random.nextGaussian() / 5d,
-              sample.pose.getRotation().getRadians() + random.nextGaussian() / 3d);
+              sample.pose.getTranslation().getX(),
+              sample.pose.getTranslation().getY(),
+              sample.pose.getRotation().getRadians());
+      var noise = Normal.normal(VecBuilder.fill(0.2, 0.2, 1.0 / 3.0));
+      y.set(0, 0, y.get(0, 0) + noise.get(0, 0));
+      y.set(1, 0, y.get(1, 0) + noise.get(1, 0));
+      y.set(2, 0, y.get(2, 0) + noise.get(2, 0));
 
       var velocity =
           VecBuilder.fill(
               sample.velocity * sample.pose.getRotation().getCos(),
               sample.velocity * sample.pose.getRotation().getSin(),
               sample.curvature * sample.velocity);
-      var u = (velocity.minus(lastVelocity)).div(0.020);
+      var u = velocity.minus(lastVelocity).div(dt);
+
+      filter.correct(u, y);
+      filter.predict(u, dt);
+
       lastVelocity = velocity;
-
-      filter.correct(u, measurement);
-      filter.predict(u, 0.020);
-
-      time += 0.020;
     }
 
     assertEquals(