[wpimath] Clean up NumericalIntegration and add Discretization tests (#3489)

* Rename Butcher tableau sections in NumericalIntegration such that
  top-left is c, top-right is A, and bottom-right is b
* Move edu.wpi.first.math.Discretization to
  edu.wpi.first.math.system.Discretization
* Sort Java Discretization to match C++ function order
* Add tests for Java Discretization
  * Required adding Runge-Kutta time-varying impl to tests
* Move C++ Runge-Kutta time-varying impl to tests only
  * Users don't need it

wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java

@@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.Discretization;
 import java.util.ArrayList;
 import java.util.List;
 import org.ejml.dense.row.MatrixFeatures_DDRM;

wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java

@@ -8,7 +8,6 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
-import edu.wpi.first.math.Discretization;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.StateSpaceUtil;
@@ -19,6 +18,7 @@ import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.numbers.N4;
 import edu.wpi.first.math.numbers.N6;
+import edu.wpi.first.math.system.Discretization;
 import edu.wpi.first.math.system.NumericalIntegration;
 import edu.wpi.first.math.system.NumericalJacobian;
 import edu.wpi.first.math.system.plant.DCMotor;

wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java

@@ -0,0 +1,215 @@
+// 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.
+
+package edu.wpi.first.math.system;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N2;
+import org.junit.jupiter.api.Test;
+
+public class DiscretizationTest {
+  // Check that for a simple second-order system that we can easily analyze
+  // analytically,
+  @Test
+  public void testDiscretizeA() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
+    final var x0 = VecBuilder.fill(1, 1);
+
+    final var discA = Discretization.discretizeA(contA, 1.0);
+    final var x1Discrete = discA.times(x0);
+
+    // We now have pos = vel = 1 and accel = 0, which should give us:
+    final var x1Truth =
+        VecBuilder.fill(
+            1.0 * x0.get(0, 0) + 1.0 * x0.get(1, 0), 0.0 * x0.get(0, 0) + 1.0 * x0.get(1, 0));
+
+    assertEquals(x1Truth, x1Discrete);
+  }
+
+  // Check that for a simple second-order system that we can easily analyze
+  // analytically,
+  @Test
+  public void testDiscretizeAB() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
+    final var contB = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0, 1);
+
+    final var x0 = VecBuilder.fill(1, 1);
+    final var u = VecBuilder.fill(1);
+
+    var discABPair = Discretization.discretizeAB(contA, contB, 1.0);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    var x1Discrete = discA.times(x0).plus(discB.times(u));
+
+    // We now have pos = vel = accel = 1, which should give us:
+    final var x1Truth =
+        VecBuilder.fill(
+            1.0 * x0.get(0, 0) + 1.0 * x0.get(1, 0) + 0.5 * u.get(0, 0),
+            0.0 * x0.get(0, 0) + 1.0 * x0.get(1, 0) + 1.0 * u.get(0, 0));
+
+    assertEquals(x1Truth, x1Discrete);
+  }
+
+  // Test that the discrete approximation of Q is roughly equal to
+  // integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
+  @Test
+  public void testDiscretizeSlowModelAQ() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
+    final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+
+    final double dt = 1.0;
+
+    final var discQIntegrated =
+        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
+            (Double t, Matrix<N2, N2> x) ->
+                contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
+            0.0,
+            new Matrix<>(Nat.N2(), Nat.N2()),
+            dt);
+
+    var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
+    var discQ = discAQPair.getSecond();
+
+    assertTrue(
+        discQIntegrated.minus(discQ).normF() < 1e-10,
+        "Expected these to be nearly equal:\ndiscQ:\n"
+            + discQ
+            + "\ndiscQIntegrated:\n"
+            + discQIntegrated);
+  }
+
+  // Test that the discrete approximation of Q is roughly equal to
+  // integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
+  @Test
+  public void testDiscretizeFastModelAQ() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1406.29);
+    final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1);
+
+    final var dt = 0.005;
+
+    final var discQIntegrated =
+        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
+            (Double t, Matrix<N2, N2> x) ->
+                contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
+            0.0,
+            new Matrix<>(Nat.N2(), Nat.N2()),
+            dt);
+
+    var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
+    var discQ = discAQPair.getSecond();
+
+    assertTrue(
+        discQIntegrated.minus(discQ).normF() < 1e-3,
+        "Expected these to be nearly equal:\ndiscQ:\n"
+            + discQ
+            + "\ndiscQIntegrated:\n"
+            + discQIntegrated);
+  }
+
+  // Test that the Taylor series discretization produces nearly identical results.
+  @Test
+  public void testDiscretizeSlowModelAQTaylor() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
+    final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+
+    final var dt = 1.0;
+
+    // Continuous Q should be positive semidefinite
+    final var esCont = contQ.getStorage().eig();
+    for (int i = 0; i < contQ.getNumRows(); ++i) {
+      assertTrue(esCont.getEigenvalue(i).real >= 0);
+    }
+
+    final var discQIntegrated =
+        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
+            (Double t, Matrix<N2, N2> x) ->
+                contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
+            0.0,
+            new Matrix<>(Nat.N2(), Nat.N2()),
+            dt);
+
+    var discA = Discretization.discretizeA(contA, dt);
+
+    var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
+    var discATaylor = discAQPair.getFirst();
+    var discQTaylor = discAQPair.getSecond();
+
+    assertTrue(
+        discQIntegrated.minus(discQTaylor).normF() < 1e-10,
+        "Expected these to be nearly equal:\ndiscQTaylor:\n"
+            + discQTaylor
+            + "\ndiscQIntegrated:\n"
+            + discQIntegrated);
+    assertTrue(discA.minus(discATaylor).normF() < 1e-10);
+
+    // Discrete Q should be positive semidefinite
+    final var esDisc = discQTaylor.getStorage().eig();
+    for (int i = 0; i < discQTaylor.getNumRows(); ++i) {
+      assertTrue(esDisc.getEigenvalue(i).real >= 0);
+    }
+  }
+
+  // Test that the Taylor series discretization produces nearly identical results.
+  @Test
+  public void testDiscretizeFastModelAQTaylor() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1500);
+    final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1);
+
+    final var dt = 0.005;
+
+    // Continuous Q should be positive semidefinite
+    final var esCont = contQ.getStorage().eig();
+    for (int i = 0; i < contQ.getNumRows(); ++i) {
+      assertTrue(esCont.getEigenvalue(i).real >= 0);
+    }
+
+    final var discQIntegrated =
+        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
+            (Double t, Matrix<N2, N2> x) ->
+                contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
+            0.0,
+            new Matrix<>(Nat.N2(), Nat.N2()),
+            dt);
+
+    var discA = Discretization.discretizeA(contA, dt);
+
+    var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
+    var discATaylor = discAQPair.getFirst();
+    var discQTaylor = discAQPair.getSecond();
+
+    assertTrue(
+        discQIntegrated.minus(discQTaylor).normF() < 1e-3,
+        "Expected these to be nearly equal:\ndiscQTaylor:\n"
+            + discQTaylor
+            + "\ndiscQIntegrated:\n"
+            + discQIntegrated);
+    assertTrue(discA.minus(discATaylor).normF() < 1e-10);
+
+    // Discrete Q should be positive semidefinite
+    final var esDisc = discQTaylor.getStorage().eig();
+    for (int i = 0; i < discQTaylor.getNumRows(); ++i) {
+      assertTrue(esDisc.getEigenvalue(i).real >= 0);
+    }
+  }
+
+  // Test that DiscretizeR() works
+  @Test
+  public void testDiscretizeR() {
+    var contR = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 0.0, 0.0, 1.0);
+    var discRTruth = Matrix.mat(Nat.N2(), Nat.N2()).fill(4.0, 0.0, 0.0, 2.0);
+
+    var discR = Discretization.discretizeR(contR, 0.5);
+
+    assertTrue(
+        discRTruth.minus(discR).normF() < 1e-10,
+        "Expected these to be nearly equal:\ndiscR:\n" + discR + "\ndiscRTruth:\n" + discRTruth);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java

@@ -14,11 +14,9 @@ import org.junit.jupiter.api.Test;
 
 public class NumericalIntegrationTest {
   @Test
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
   public void testExponential() {
     Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
 
-    //noinspection SuspiciousNameCombination
     var y1 =
         NumericalIntegration.rk4(
             (Matrix<N1, N1> x) -> {
@@ -33,11 +31,9 @@ public class NumericalIntegrationTest {
   }
 
   @Test
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
   public void testExponentialRKF45() {
     Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
 
-    //noinspection SuspiciousNameCombination
     var y1 =
         NumericalIntegration.rkf45(
             (x, u) -> {
@@ -53,11 +49,9 @@ public class NumericalIntegrationTest {
   }
 
   @Test
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
   public void testExponentialRKDP() {
     Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
 
-    //noinspection SuspiciousNameCombination
     var y1 =
         NumericalIntegration.rkdp(
             (x, u) -> {

wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java

@@ -0,0 +1,41 @@
+// 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.
+
+package edu.wpi.first.math.system;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import java.util.function.BiFunction;
+
+public final class RungeKuttaTimeVarying {
+  private RungeKuttaTimeVarying() {
+    // Utility class
+  }
+
+  /**
+   * Performs 4th order Runge-Kutta integration of dx/dt = f(t, y) for dt.
+   *
+   * @param <Rows> Rows in y.
+   * @param <Cols> Columns in y.
+   * @param f The function to integrate. It must take two arguments t and y.
+   * @param t The initial value of t.
+   * @param y The initial value of y.
+   * @param dtSeconds The time over which to integrate.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> rungeKuttaTimeVarying(
+      BiFunction<Double, Matrix<Rows, Cols>, Matrix<Rows, Cols>> f,
+      double t,
+      Matrix<Rows, Cols> y,
+      double dtSeconds) {
+    final var h = dtSeconds;
+
+    Matrix<Rows, Cols> k1 = f.apply(t, y);
+    Matrix<Rows, Cols> k2 = f.apply(t + dtSeconds * 0.5, y.plus(k1.times(h * 0.5)));
+    Matrix<Rows, Cols> k3 = f.apply(t + dtSeconds * 0.5, y.plus(k2.times(h * 0.5)));
+    Matrix<Rows, Cols> k4 = f.apply(t + dtSeconds, y.plus(k3.times(h)));
+
+    return y.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0));
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java

@@ -0,0 +1,43 @@
+// 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.
+
+package edu.wpi.first.math.system;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.N1;
+import org.junit.jupiter.api.Test;
+
+public class RungeKuttaTimeVaryingTest {
+  private static Matrix<N1, N1> rungeKuttaTimeVaryingSolution(double t) {
+    return new MatBuilder<>(Nat.N1(), Nat.N1())
+        .fill(12.0 * Math.exp(t) / (Math.pow(Math.exp(t) + 1.0, 2.0)));
+  }
+
+  // Tests RK4 with a time varying solution. From
+  // http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
+  //   x' = x (2 / (e^t + 1) - 1)
+  //
+  // The true (analytical) solution is:
+  //
+  // x(t) = 12 * e^t / ((e^t + 1)^2)
+  @Test
+  public void rungeKuttaTimeVaryingTest() {
+    final var y0 = rungeKuttaTimeVaryingSolution(5.0);
+
+    final var y1 =
+        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
+            (Double t, Matrix<N1, N1> x) -> {
+              return new MatBuilder<>(Nat.N1(), Nat.N1())
+                  .fill(x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0));
+            },
+            5.0,
+            y0,
+            1.0);
+    assertEquals(rungeKuttaTimeVaryingSolution(6.0).get(0, 0), y1.get(0, 0), 1e-3);
+  }
+}