[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(

wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp

@@ -7,14 +7,136 @@
 #include <Eigen/Core>
 #include <gtest/gtest.h>
 
-#include "wpi/math/system/DCMotor.hpp"
-#include "wpi/math/system/Models.hpp"
-#include "wpi/units/moment_of_inertia.hpp"
+#include "wpi/math/random/Normal.hpp"
+#include "wpi/math/system/LinearSystem.hpp"
+#include "wpi/math/trajectory/TrajectoryConfig.hpp"
+#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
 #include "wpi/units/time.hpp"
 
-TEST(KalmanFilterTest, Flywheel) {
-  auto motor = wpi::math::DCMotor::NEO();
-  auto flywheel =
-      wpi::math::Models::FlywheelFromPhysicalConstants(motor, 1_kg_sq_m, 1.0);
-  wpi::math::KalmanFilter<1, 1, 1> kf{flywheel, {1}, {1}, 5_ms};
+TEST(KalmanFilterTest, SwerveStationary) {
+  constexpr wpi::units::second_t dt = 20_ms;
+
+  // Swerve drive with x = [x, y, θ, v_x, v_y, ω]ᵀ, u = [a_x, a_y, α]ᵀ,
+  // y = [x, y, θ]ᵀ
+  wpi::math::LinearSystem<6, 3, 3> plant{
+      Eigen::Matrix<double, 6, 6>{{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}},
+      Eigen::Matrix<double, 6, 3>{
+          {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}},
+      Eigen::Matrix<double, 3, 6>{
+          {1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}},
+      Eigen::Matrix<double, 3, 3>::Zero()};
+
+  wpi::math::KalmanFilter<6, 3, 3> filter{
+      plant, {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}, dt};
+
+  constexpr Eigen::Vector<double, 3> u{{0.0}, {0.0}, {0.0}};
+
+  for (int i = 0; i < 100; ++i) {
+    auto y = wpi::math::Normal(1.0, 1.0, 1.0);
+
+    filter.Correct(u, y);
+    filter.Predict(u, dt);
+  }
+
+  EXPECT_NEAR(0.0, filter.Xhat(0), 0.3);
+  EXPECT_NEAR(0.0, filter.Xhat(1), 0.3);
+}
+
+TEST(KalmanFilterTest, SwerveBadInitialPose) {
+  constexpr wpi::units::second_t dt = 20_ms;
+
+  // Swerve drive with x = [x, y, θ, v_x, v_y, ω]ᵀ, u = [a_x, a_y, α]ᵀ,
+  // y = [x, y, θ]ᵀ
+  wpi::math::LinearSystem<6, 3, 3> plant{
+      Eigen::Matrix<double, 6, 6>{{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}},
+      Eigen::Matrix<double, 6, 3>{
+          {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}},
+      Eigen::Matrix<double, 3, 6>{
+          {1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}},
+      Eigen::Matrix<double, 3, 3>::Zero()};
+
+  wpi::math::KalmanFilter<6, 3, 3> filter{
+      plant, {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, {0.1, 0.1, 0.25}, dt};
+
+  // Set nonzero position
+  filter.SetXhat(0, 0.5);
+  filter.SetXhat(1, 0.5);
+
+  constexpr Eigen::Vector<double, 3> u{{0.0}, {0.0}, {0.0}};
+
+  // Let filter converge to zero position
+  for (int i = 0; i < 300; ++i) {
+    auto y = wpi::math::Normal(0.1, 0.1, 0.25);
+
+    filter.Correct(u, y);
+    filter.Predict(u, dt);
+  }
+
+  EXPECT_NEAR(0.0, filter.Xhat(0), 0.2);
+  EXPECT_NEAR(0.0, filter.Xhat(1), 0.2);
+}
+
+TEST(KalmanFilterTest, SwerveMovingOverTrajectory) {
+  constexpr wpi::units::second_t dt = 20_ms;
+
+  // Swerve drive with x = [x, y, θ, v_x, v_y, ω]ᵀ, u = [a_x, a_y, α]ᵀ,
+  // y = [x, y, θ]ᵀ
+  wpi::math::LinearSystem<6, 3, 3> plant{
+      Eigen::Matrix<double, 6, 6>{{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}},
+      Eigen::Matrix<double, 6, 3>{
+          {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}},
+      Eigen::Matrix<double, 3, 6>{
+          {1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}},
+      Eigen::Matrix<double, 3, 3>::Zero()};
+
+  wpi::math::KalmanFilter<6, 3, 3> filter{
+      plant, {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, {0.2, 0.2, 1.0 / 3.0}, dt};
+
+  auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
+      {wpi::math::Pose2d{0_m, 0_m, 0_rad}, wpi::math::Pose2d{5_m, 5_m, 0_rad}},
+      wpi::math::TrajectoryConfig{2_mps, 2_mps_sq});
+
+  Eigen::Vector3d lastVelocity{{0.0}, {0.0}, {0.0}};
+
+  for (wpi::units::second_t t = 0_s; t < trajectory.TotalTime(); t += dt) {
+    auto sample = trajectory.Sample(t);
+
+    Eigen::Vector3d y{sample.pose.Translation().X().value(),
+                      sample.pose.Translation().Y().value(),
+                      sample.pose.Rotation().Radians().value()};
+    y += wpi::math::Normal(0.2, 0.2, 1.0 / 3.0);
+
+    Eigen::Vector3d velocity{
+        (sample.velocity * sample.pose.Rotation().Cos()).value(),
+        (sample.velocity * sample.pose.Rotation().Sin()).value(),
+        (sample.curvature * sample.velocity).value()};
+    Eigen::Vector3d u = (velocity - lastVelocity) / dt.value();
+
+    filter.Correct(u, y);
+    filter.Predict(u, dt);
+
+    lastVelocity = velocity;
+  }
+
+  EXPECT_NEAR(
+      trajectory.Sample(trajectory.TotalTime()).pose.Translation().X().value(),
+      filter.Xhat(0), 0.2);
+  EXPECT_NEAR(
+      trajectory.Sample(trajectory.TotalTime()).pose.Translation().Y().value(),
+      filter.Xhat(1), 0.2);
 }