[wpimath] Add RKF45 integration (#3047)

This is more stable than Runge-Kutta for systems with large elements in their A or B matrices.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java

@@ -10,8 +10,8 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.math.StateSpaceUtil;
+import edu.wpi.first.wpilibj.system.NumericalIntegration;
 import edu.wpi.first.wpilibj.system.NumericalJacobian;
-import edu.wpi.first.wpilibj.system.RungeKutta;
 import edu.wpi.first.wpilibj.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
@@ -177,7 +177,8 @@ public class ExtendedKalmanFilterTest {
       observer.predict(u, dtSeconds);
 
       groundTruthX =
-          RungeKutta.rungeKutta(ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dtSeconds);
+          NumericalIntegration.rk4(
+              ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dtSeconds);
 
       r = nextR;
 

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

@@ -12,8 +12,8 @@ import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.math.Discretization;
 import edu.wpi.first.wpilibj.math.StateSpaceUtil;
+import edu.wpi.first.wpilibj.system.NumericalIntegration;
 import edu.wpi.first.wpilibj.system.NumericalJacobian;
-import edu.wpi.first.wpilibj.system.RungeKutta;
 import edu.wpi.first.wpilibj.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
@@ -217,7 +217,7 @@ public class UnscentedKalmanFilterTest {
       r = nextR;
       observer.predict(u, dtSeconds);
       trueXhat =
-          RungeKutta.rungeKutta(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds);
+          NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds);
     }
 
     var localY = getLocalMeasurementModel(trueXhat, u);

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

@@ -12,7 +12,7 @@ import edu.wpi.first.wpiutil.math.VecBuilder;
 import edu.wpi.first.wpiutil.math.numbers.N1;
 import org.junit.jupiter.api.Test;
 
-public class RungeKuttaTest {
+public class NumericalIntegrationTest {
   @Test
   @SuppressWarnings({"ParameterName", "LocalVariableName"})
   public void testExponential() {
@@ -21,7 +21,7 @@ public class RungeKuttaTest {
 
     //noinspection SuspiciousNameCombination
     var y1 =
-        RungeKutta.rungeKutta(
+        NumericalIntegration.rk4(
             (Matrix<N1, N1> x) -> {
               var y = new Matrix<>(Nat.N1(), Nat.N1());
               y.set(0, 0, Math.exp(x.get(0, 0)));
@@ -32,4 +32,25 @@ public class RungeKuttaTest {
 
     assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
   }
+
+  @Test
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public void testExponentialAdaptive() {
+
+    Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
+
+    //noinspection SuspiciousNameCombination
+    var y1 =
+        NumericalIntegration.rkf45(
+            (x, u) -> {
+              var y = new Matrix<>(Nat.N1(), Nat.N1());
+              y.set(0, 0, Math.exp(x.get(0, 0)));
+              return y;
+            },
+            y0,
+            VecBuilder.fill(0),
+            0.1);
+
+    assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
+  }
 }

wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp

@@ -8,7 +8,7 @@
 
 #include "Eigen/Core"
 #include "frc/StateSpaceUtil.h"
-#include "frc/system/RungeKutta.h"
+#include "frc/system/NumericalIntegration.h"
 
 TEST(StateSpaceUtilTest, MakeMatrix) {
   // Column vector

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

@@ -12,8 +12,8 @@
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/AngleStatistics.h"
 #include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/system/NumericalIntegration.h"
 #include "frc/system/NumericalJacobian.h"
-#include "frc/system/RungeKutta.h"
 #include "frc/system/plant/DCMotor.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
 #include "units/moment_of_inertia.h"
@@ -154,7 +154,7 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
     observer.Predict(u, dt);
 
     r = nextR;
-    trueXhat = frc::RungeKutta(Dynamics, trueXhat, u, dt);
+    trueXhat = frc::RK4(Dynamics, trueXhat, u, dt);
   }
 
   auto localY = LocalMeasurementModel(trueXhat, u);

wpimath/src/test/native/cpp/system/DiscretizationTest.cpp

@@ -9,7 +9,7 @@
 #include "Eigen/Core"
 #include "Eigen/Eigenvalues"
 #include "frc/system/Discretization.h"
-#include "frc/system/RungeKutta.h"
+#include "frc/system/NumericalIntegration.h"
 
 // Check that for a simple second-order system that we can easily analyze
 // analytically,

wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp

@@ -6,14 +6,14 @@
 
 #include <cmath>
 
-#include "frc/system/RungeKutta.h"
+#include "frc/system/NumericalIntegration.h"
 
 // Tests that integrating dx/dt = e^x works.
-TEST(RungeKuttaTest, Exponential) {
+TEST(NumericalIntegrationTest, Exponential) {
   Eigen::Matrix<double, 1, 1> y0;
   y0(0) = 0.0;
 
-  Eigen::Matrix<double, 1, 1> y1 = frc::RungeKutta(
+  Eigen::Matrix<double, 1, 1> y1 = frc::RK4(
       [](Eigen::Matrix<double, 1, 1> x) {
         Eigen::Matrix<double, 1, 1> y;
         y(0) = std::exp(x(0));
@@ -24,11 +24,11 @@ TEST(RungeKuttaTest, Exponential) {
 }
 
 // Tests that integrating dx/dt = e^x works when we provide a U.
-TEST(RungeKuttaTest, ExponentialWithU) {
+TEST(NumericalIntegrationTest, ExponentialWithU) {
   Eigen::Matrix<double, 1, 1> y0;
   y0(0) = 0.0;
 
-  Eigen::Matrix<double, 1, 1> y1 = frc::RungeKutta(
+  Eigen::Matrix<double, 1, 1> y1 = frc::RK4(
       [](Eigen::Matrix<double, 1, 1> x, Eigen::Matrix<double, 1, 1> u) {
         Eigen::Matrix<double, 1, 1> y;
         y(0) = std::exp(u(0) * x(0));
@@ -38,6 +38,21 @@ TEST(RungeKuttaTest, ExponentialWithU) {
   EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
 }
 
+// Tests that integrating dx/dt = e^x works when we provide a U.
+TEST(NumericalIntegrationTest, ExponentialWithUAdaptive) {
+  Eigen::Matrix<double, 1, 1> y0;
+  y0(0) = 0.0;
+
+  Eigen::Matrix<double, 1, 1> y1 = frc::RKF45(
+      [](Eigen::Matrix<double, 1, 1> x, Eigen::Matrix<double, 1, 1> u) {
+        Eigen::Matrix<double, 1, 1> y;
+        y(0) = std::exp(x(0));
+        return y;
+      },
+      y0, (Eigen::Matrix<double, 1, 1>() << 0.0).finished(), 0.1_s);
+  EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
+}
+
 namespace {
 Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
   return (Eigen::Matrix<double, 1, 1>()
@@ -54,7 +69,7 @@ Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
 // The true (analytical) solution is:
 //
 // x(t) = 12 * e^t / ((e^t + 1)^2)
-TEST(RungeKuttaTest, RungeKuttaTimeVarying) {
+TEST(NumericalIntegrationTest, RungeKuttaTimeVarying) {
   Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
 
   Eigen::Matrix<double, 1, 1> y1 = frc::RungeKuttaTimeVarying(