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

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java

@@ -9,7 +9,7 @@ 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.LinearSystem;
-import edu.wpi.first.wpilibj.system.RungeKutta;
+import edu.wpi.first.wpilibj.system.NumericalIntegration;
 import edu.wpi.first.wpilibj.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
 import edu.wpi.first.wpilibj.util.Units;
@@ -151,7 +151,7 @@ public class DifferentialDrivetrainSim {
   public void update(double dtSeconds) {
 
     // Update state estimate with RK4
-    m_x = RungeKutta.rungeKutta(this::getDynamics, m_x, m_u, dtSeconds);
+    m_x = NumericalIntegration.rk4(this::getDynamics, m_x, m_u, dtSeconds);
     m_y = m_x;
     if (m_measurementStdDevs != null) {
       m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs));

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java

@@ -5,7 +5,7 @@
 package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.RungeKutta;
+import edu.wpi.first.wpilibj.system.NumericalIntegration;
 import edu.wpi.first.wpilibj.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
 import edu.wpi.first.wpiutil.math.Matrix;
@@ -202,7 +202,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
   protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
     // Calculate updated x-hat from Runge-Kutta.
     var updatedXhat =
-        RungeKutta.rungeKutta(
+        NumericalIntegration.rkf45(
             (x, u_) ->
                 (m_plant.getA().times(x))
                     .plus(m_plant.getB().times(u_))

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java

@@ -5,7 +5,7 @@
 package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.RungeKutta;
+import edu.wpi.first.wpilibj.system.NumericalIntegration;
 import edu.wpi.first.wpilibj.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
 import edu.wpi.first.wpiutil.math.Matrix;
@@ -263,7 +263,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
     // We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
     // cos(theta)]]
     Matrix<N2, N1> updatedXhat =
-        RungeKutta.rungeKutta(
+        NumericalIntegration.rkf45(
             (Matrix<N2, N1> x, Matrix<N1, N1> u_) -> {
               Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(u_));
               if (m_simulateGravity) {

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java

@@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj.controller.RamseteController;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.system.RungeKutta;
+import edu.wpi.first.wpilibj.system.NumericalIntegration;
 import edu.wpi.first.wpilibj.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
@@ -76,7 +76,7 @@ class DifferentialDrivetrainSimTest {
       sim.update(0.020);
 
       // Update our ground truth
-      groundTruthX = RungeKutta.rungeKutta(sim::getDynamics, groundTruthX, voltages, 0.020);
+      groundTruthX = NumericalIntegration.rk4(sim::getDynamics, groundTruthX, voltages, 0.020);
     }
 
     assertEquals(

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java

@@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.controller.PIDController;
 import edu.wpi.first.wpilibj.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.util.Units;
 import edu.wpi.first.wpiutil.math.VecBuilder;
 import org.junit.jupiter.api.Test;
 
@@ -87,4 +88,20 @@ public class ElevatorSimTest {
       assertTrue(height <= 1.05);
     }
   }
+
+  @Test
+  public void testStability() {
+    var sim = new ElevatorSim(DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10);
+
+    sim.setState(VecBuilder.fill(0, 0));
+    sim.setInput(12);
+    for (int i = 0; i < 50; i++) {
+      sim.update(0.02);
+    }
+
+    assertEquals(
+        sim.m_plant.calculateX(VecBuilder.fill(0, 0), VecBuilder.fill(12), 0.02 * 50.0).get(0, 0),
+        sim.getPositionMeters(),
+        0.1);
+  }
 }