diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
index 8224887ff1..0e22f6d434 100644
--- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
@@ -9,7 +9,7 @@
 #include <utility>
 
 #include "frc/RobotController.h"
-#include "frc/system/RungeKutta.h"
+#include "frc/system/NumericalIntegration.h"
 
 using namespace frc;
 using namespace frc::sim;
@@ -56,8 +56,7 @@ void DifferentialDrivetrainSim::SetGearing(double newGearing) {
 }
 
 void DifferentialDrivetrainSim::Update(units::second_t dt) {
-  m_x = RungeKutta([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x,
-                   m_u, dt);
+  m_x = RK4([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt);
   m_y = m_x + frc::MakeWhiteNoiseVector<7>(m_measurementStdDevs);
 }
 
diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
index dfc12a4460..3a165e591e 100644
--- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
@@ -7,7 +7,7 @@
 #include <wpi/MathExtras.h>
 
 #include "frc/StateSpaceUtil.h"
-#include "frc/system/RungeKutta.h"
+#include "frc/system/NumericalIntegration.h"
 #include "frc/system/plant/LinearSystemId.h"
 
 using namespace frc;
@@ -77,7 +77,7 @@ void ElevatorSim::SetInputVoltage(units::volt_t voltage) {
 Eigen::Matrix<double, 2, 1> ElevatorSim::UpdateX(
     const Eigen::Matrix<double, 2, 1>& currentXhat,
     const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
-  auto updatedXhat = RungeKutta(
+  auto updatedXhat = RKF45(
       [&](const Eigen::Matrix<double, 2, 1>& x,
           const Eigen::Matrix<double, 1, 1>& u_)
           -> Eigen::Matrix<double, 2, 1> {
diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
index de4cca225f..926a18a57f 100644
--- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
@@ -9,7 +9,7 @@
 #include <units/voltage.h>
 #include <wpi/MathExtras.h>
 
-#include "frc/system/RungeKutta.h"
+#include "frc/system/NumericalIntegration.h"
 #include "frc/system/plant/LinearSystemId.h"
 
 using namespace frc;
@@ -82,7 +82,7 @@ Eigen::Matrix<double, 2, 1> SingleJointedArmSim::UpdateX(
   // We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
   // std::cos(theta)]]
 
-  auto updatedXhat = RungeKutta(
+  Eigen::Matrix<double, 2, 1> updatedXhat = RKF45(
       [&](const auto& x, const auto& u) -> Eigen::Matrix<double, 2, 1> {
         Eigen::Matrix<double, 2, 1> xdot = m_plant.A() * x + m_plant.B() * u;
 
diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
index d24249bfae..a8c5f41646 100644
--- a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
@@ -10,7 +10,7 @@
 #include "frc/controller/RamseteController.h"
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/simulation/DifferentialDrivetrainSim.h"
-#include "frc/system/RungeKutta.h"
+#include "frc/system/NumericalIntegration.h"
 #include "frc/system/plant/DCMotor.h"
 #include "frc/system/plant/LinearSystemId.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
@@ -57,7 +57,7 @@ TEST(DifferentialDriveSim, Convergence) {
     sim.Update(20_ms);
 
     // Update ground truth.
-    groundTruthX = frc::RungeKutta(
+    groundTruthX = frc::RK4(
         [&sim](const auto& x, const auto& u) -> Eigen::Matrix<double, 7, 1> {
           return sim.Dynamics(x, u);
         },
diff --git a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
index 085789bb11..1083ad51a6 100644
--- a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
@@ -13,6 +13,7 @@
 #include "frc/controller/PIDController.h"
 #include "frc/simulation/ElevatorSim.h"
 #include "frc/simulation/EncoderSim.h"
+#include "frc/system/NumericalIntegration.h"
 #include "frc/system/plant/DCMotor.h"
 #include "frc/system/plant/LinearSystemId.h"
 #include "gtest/gtest.h"
@@ -64,3 +65,28 @@ TEST(ElevatorSim, MinMax) {
     EXPECT_TRUE(height < 1.05_m);
   }
 }
+
+TEST(ElevatorSim, Stability) {
+  static constexpr double kElevatorGearing = 100.0;
+  static constexpr units::meter_t kElevatorDrumRadius = 0.5_in;
+  static constexpr units::kilogram_t kCarriageMass = 4.0_kg;
+  frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4);
+
+  frc::LinearSystem<2, 1, 1> system = frc::LinearSystemId::ElevatorSystem(
+      m_elevatorGearbox, kCarriageMass, kElevatorDrumRadius, kElevatorGearing);
+
+  Eigen::Matrix<double, 2, 1> x0 = frc::MakeMatrix<2, 1>(0.0, 0.0);
+  Eigen::Matrix<double, 1, 1> u0 = frc::MakeMatrix<1, 1>(12.0);
+
+  Eigen::Matrix<double, 2, 1> x1 = frc::MakeMatrix<2, 1>(0.0, 0.0);
+  for (size_t i = 0; i < 50; i++) {
+    x1 = frc::RKF45(
+        [&](Eigen::Matrix<double, 2, 1> x,
+            Eigen::Matrix<double, 1, 1> u) -> Eigen::Matrix<double, 2, 1> {
+          return system.A() * x + system.B() * u;
+        },
+        x1, u0, 0.020_s);
+  }
+
+  EXPECT_NEAR(x1(0), system.CalculateX(x0, u0, 1_s)(0), 0.1);
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
index 37f9406af2..22fb40e7bd 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
+++ b/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));
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
index bfe9b201d8..7262cc8793 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
+++ b/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_))
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
index 41de9ee704..b9c5508c48 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
+++ b/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) {
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
index c77fb5ad6f..f9ecdfed69 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
+++ b/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(
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java
index 5a1498e8a1..1903c852f6 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java
+++ b/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);
+  }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java
index 154055094e..5f89e80f8f 100644
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java
@@ -7,8 +7,8 @@ package edu.wpi.first.wpilibj.estimator;
 import edu.wpi.first.math.Drake;
 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.wpiutil.math.Matrix;
 import edu.wpi.first.wpiutil.math.Nat;
 import edu.wpi.first.wpiutil.math.Num;
@@ -220,7 +220,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
     final var discA = discPair.getFirst();
     final var discQ = discPair.getSecond();
 
-    m_xHat = RungeKutta.rungeKutta(f, m_xHat, u, dtSeconds);
+    m_xHat = NumericalIntegration.rk4(f, m_xHat, u, dtSeconds);
     m_P = discA.times(m_P).times(discA.transpose()).plus(discQ);
     m_dtSeconds = dtSeconds;
   }
diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java
index 7c327e2e29..d4a142ea7f 100644
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java
@@ -6,8 +6,8 @@ package edu.wpi.first.wpilibj.estimator;
 
 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.wpiutil.math.Matrix;
 import edu.wpi.first.wpiutil.math.Nat;
 import edu.wpi.first.wpiutil.math.Num;
@@ -291,7 +291,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
     for (int i = 0; i < m_pts.getNumSigmas(); ++i) {
       Matrix<States, N1> x = sigmas.extractColumnVector(i);
 
-      m_sigmasF.setColumn(i, RungeKutta.rungeKutta(m_f, x, u, dtSeconds));
+      m_sigmasF.setColumn(i, NumericalIntegration.rk4(m_f, x, u, dtSeconds));
     }
 
     var ret =
diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/NumericalIntegration.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/NumericalIntegration.java
new file mode 100644
index 0000000000..5b52ee47fb
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/NumericalIntegration.java
@@ -0,0 +1,290 @@
+// 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.wpilibj.system;
+
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.Pair;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N5;
+import edu.wpi.first.wpiutil.math.numbers.N6;
+import java.util.function.BiFunction;
+import java.util.function.DoubleFunction;
+import java.util.function.Function;
+
+public final class NumericalIntegration {
+  private NumericalIntegration() {
+    // utility Class
+  }
+
+  /**
+   * Performs Runge Kutta integration (4th order).
+   *
+   * @param f The function to integrate, which takes one argument x.
+   * @param x The initial value of x.
+   * @param dtSeconds The time over which to integrate.
+   * @return the integration of dx/dt = f(x) for dt.
+   */
+  @SuppressWarnings("ParameterName")
+  public static double rk4(DoubleFunction<Double> f, double x, double dtSeconds) {
+    final var halfDt = 0.5 * dtSeconds;
+    final var k1 = f.apply(x);
+    final var k2 = f.apply(x + k1 * halfDt);
+    final var k3 = f.apply(x + k2 * halfDt);
+    final var k4 = f.apply(x + k3 * dtSeconds);
+    return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+  }
+
+  /**
+   * Performs Runge Kutta integration (4th order).
+   *
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dtSeconds The time over which to integrate.
+   * @return The result of Runge Kutta integration (4th order).
+   */
+  @SuppressWarnings("ParameterName")
+  public static double rk4(
+      BiFunction<Double, Double, Double> f, double x, Double u, double dtSeconds) {
+    final var halfDt = 0.5 * dtSeconds;
+    final var k1 = f.apply(x, u);
+    final var k2 = f.apply(x + k1 * halfDt, u);
+    final var k3 = f.apply(x + k2 * halfDt, u);
+    final var k4 = f.apply(x + k3 * dtSeconds, u);
+    return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+  }
+
+  /**
+   * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
+   *
+   * @param <States> A Num representing the states of the system to integrate.
+   * @param <Inputs> A Num representing the inputs of the system to integrate.
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dtSeconds The time over which to integrate.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rk4(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x,
+      Matrix<Inputs, N1> u,
+      double dtSeconds) {
+
+    final var halfDt = 0.5 * dtSeconds;
+    Matrix<States, N1> k1 = f.apply(x, u);
+    Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)), u);
+    Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)), u);
+    Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)), u);
+    return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
+  }
+
+  /**
+   * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
+   *
+   * @param <States> A Num prepresenting the states of the system.
+   * @param f The function to integrate. It must take one argument x.
+   * @param x The initial value of x.
+   * @param dtSeconds The time over which to integrate.
+   * @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public static <States extends Num> Matrix<States, N1> rk4(
+      Function<Matrix<States, N1>, Matrix<States, N1>> f, Matrix<States, N1> x, double dtSeconds) {
+
+    final var halfDt = 0.5 * dtSeconds;
+    Matrix<States, N1> k1 = f.apply(x);
+    Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)));
+    Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)));
+    Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)));
+    return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
+  }
+
+  /**
+   * Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described in
+   * https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method. By default, the max
+   * error is 1e-6.
+   *
+   * @param <States> A Num representing the states of the system to integrate.
+   * @param <Inputs> A Num representing the inputs of the system to integrate.
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dtSeconds The time over which to integrate.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkf45(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x,
+      Matrix<Inputs, N1> u,
+      double dtSeconds) {
+    return rkf45(f, x, u, dtSeconds, 1e-6);
+  }
+
+  /**
+   * Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described in
+   * https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
+   *
+   * @param <States> A Num representing the states of the system to integrate.
+   * @param <Inputs> A Num representing the inputs of the system to integrate.
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dtSeconds The time over which to integrate.
+   * @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkf45(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x,
+      Matrix<Inputs, N1> u,
+      double dtSeconds,
+      double maxError) {
+
+    double dtElapsed = 0;
+    double previousH = dtSeconds;
+    // Loop until we've gotten to our desired dt
+    while (dtElapsed < dtSeconds) {
+      // RKF45 will give us an updated x and a dt back.
+      // We use the new dt (h) as the initial dt for our next loop
+      var ret = rkf45Impl(f, x, u, previousH, maxError, dtSeconds - dtElapsed);
+      dtElapsed += ret.getSecond();
+      previousH = ret.getSecond();
+      x = ret.getFirst();
+    }
+    return x;
+  }
+
+  static final double[] ch = {47 / 450.0, 0, 12 / 25.0, 32 / 225.0, 1 / 30.0, 6 / 25.0};
+  static final double[] ct = {-1 / 150.0, 0, 3 / 100.0, -16 / 75.0, -1 / 20.0, 6 / 25.0};
+  static final Matrix<N6, N5> Bs =
+      Matrix.mat(Nat.N6(), Nat.N5())
+          .fill(
+              0,
+              0,
+              0,
+              0,
+              0,
+              2 / 9.0,
+              0,
+              0,
+              0,
+              0,
+              1 / 12.0,
+              1 / 4.0,
+              0,
+              0,
+              0,
+              69 / 128.0,
+              -243 / 128.0,
+              135 / 64.0,
+              0,
+              0,
+              -17 / 12.0,
+              27 / 4.0,
+              -27 / 5.0,
+              16 / 15.0,
+              0,
+              65 / 432.0,
+              -5 / 16.0,
+              13 / 16.0,
+              4 / 27.0,
+              5 / 144.0);
+
+  /**
+   * Implements one loop of RKF45. This takes an initial state, dt guess, and max truncation error,
+   * and returns a new x and the dt over which that x was updated. This should be called until there
+   * is no dt remaining.
+   *
+   * @param <States> Num representing the states of the system to integrate.
+   * @param <Inputs> A Num representing the inputs of the system to integrate.
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param initialH The initial dt guess. This is refined to clamp truncation error to the
+   *     specified max.
+   * @param maxTruncationError The max truncation error acceptable. Usually a small number like
+   *     1e-6.
+   * @param dtRemaining How much time is left to integrate over. Used to clamp h.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  private static <States extends Num, Inputs extends Num>
+      Pair<Matrix<States, N1>, Double> rkf45Impl(
+          BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+          Matrix<States, N1> x,
+          Matrix<Inputs, N1> u,
+          double initialH,
+          double maxTruncationError,
+          double dtRemaining) {
+
+    double truncationErr;
+    double h = initialH;
+    Matrix<States, N1> newX;
+
+    do {
+      // only allow us to advance up to the dt remaining
+      h = Math.min(h, dtRemaining);
+
+      // Notice how the derivative in the Wikipedia notation is dy/dx.
+      // That means their y is our x and their x is our t
+      Matrix<States, N1> k1 = f.apply(x, u).times(h);
+      Matrix<States, N1> k2 = f.apply(x.plus(k1.times(Bs.get(1, 0))), u).times(h);
+      Matrix<States, N1> k3 =
+          f.apply(x.plus(k1.times(Bs.get(2, 0))).plus(k2.times(Bs.get(2, 1))), u).times(h);
+      Matrix<States, N1> k4 =
+          f.apply(
+                  x.plus(k1.times(Bs.get(3, 0)))
+                      .plus(k2.times(Bs.get(3, 1)))
+                      .plus(k3.times(Bs.get(3, 2))),
+                  u)
+              .times(h);
+      Matrix<States, N1> k5 =
+          f.apply(
+                  x.plus(k1.times(Bs.get(4, 0)))
+                      .plus(k2.times(Bs.get(4, 1)))
+                      .plus(k3.times(Bs.get(4, 2)))
+                      .plus(k4.times(Bs.get(4, 3))),
+                  u)
+              .times(h);
+      Matrix<States, N1> k6 =
+          f.apply(
+                  x.plus(k1.times(Bs.get(5, 0)))
+                      .plus(k2.times(Bs.get(5, 1)))
+                      .plus(k3.times(Bs.get(5, 2)))
+                      .plus(k4.times(Bs.get(5, 3)))
+                      .plus(k5.times(Bs.get(5, 4))),
+                  u)
+              .times(h);
+
+      newX =
+          x.plus(k1.times(ch[0]))
+              .plus(k2.times(ch[1]))
+              .plus(k3.times(ch[2]))
+              .plus(k4.times(ch[3]))
+              .plus(k5.times(ch[4]))
+              .plus(k6.times(ch[5]));
+
+      truncationErr =
+          k1.times(ct[0])
+              .plus(k2.times(ct[1]))
+              .plus(k3.times(ct[2]))
+              .plus(k4.times(ct[3]))
+              .plus(k5.times(ct[4]))
+              .plus(k6.times(ct[5]))
+              .normF();
+
+      h = 0.9 * h * Math.pow(maxTruncationError / truncationErr, 1 / 5.0);
+    } while (truncationErr > maxTruncationError);
+
+    // Return the new x, and the timestep
+    return Pair.of(newX, h);
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/RungeKutta.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/RungeKutta.java
deleted file mode 100644
index 841d7c116b..0000000000
--- a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/RungeKutta.java
+++ /dev/null
@@ -1,103 +0,0 @@
-// 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.wpilibj.system;
-
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import java.util.function.BiFunction;
-import java.util.function.DoubleFunction;
-import java.util.function.Function;
-
-public final class RungeKutta {
-  private RungeKutta() {
-    // utility Class
-  }
-
-  /**
-   * Performs Runge Kutta integration (4th order).
-   *
-   * @param f The function to integrate, which takes one argument x.
-   * @param x The initial value of x.
-   * @param dtSeconds The time over which to integrate.
-   * @return the integration of dx/dt = f(x) for dt.
-   */
-  @SuppressWarnings("ParameterName")
-  public static double rungeKutta(DoubleFunction<Double> f, double x, double dtSeconds) {
-    final var halfDt = 0.5 * dtSeconds;
-    final var k1 = f.apply(x);
-    final var k2 = f.apply(x + k1 * halfDt);
-    final var k3 = f.apply(x + k2 * halfDt);
-    final var k4 = f.apply(x + k3 * dtSeconds);
-    return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
-  }
-
-  /**
-   * Performs Runge Kutta integration (4th order).
-   *
-   * @param f The function to integrate. It must take two arguments x and u.
-   * @param x The initial value of x.
-   * @param u The value u held constant over the integration period.
-   * @param dtSeconds The time over which to integrate.
-   * @return The result of Runge Kutta integration (4th order).
-   */
-  @SuppressWarnings("ParameterName")
-  public static double rungeKutta(
-      BiFunction<Double, Double, Double> f, double x, Double u, double dtSeconds) {
-    final var halfDt = 0.5 * dtSeconds;
-    final var k1 = f.apply(x, u);
-    final var k2 = f.apply(x + k1 * halfDt, u);
-    final var k3 = f.apply(x + k2 * halfDt, u);
-    final var k4 = f.apply(x + k3 * dtSeconds, u);
-    return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
-  }
-
-  /**
-   * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
-   *
-   * @param <States> A Num representing the states of the system to integrate.
-   * @param <Inputs> A Num representing the inputs of the system to integrate.
-   * @param f The function to integrate. It must take two arguments x and u.
-   * @param x The initial value of x.
-   * @param u The value u held constant over the integration period.
-   * @param dtSeconds The time over which to integrate.
-   * @return the integration of dx/dt = f(x, u) for dt.
-   */
-  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
-  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rungeKutta(
-      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
-      Matrix<States, N1> x,
-      Matrix<Inputs, N1> u,
-      double dtSeconds) {
-
-    final var halfDt = 0.5 * dtSeconds;
-    Matrix<States, N1> k1 = f.apply(x, u);
-    Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)), u);
-    Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)), u);
-    Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)), u);
-    return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
-  }
-
-  /**
-   * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
-   *
-   * @param <States> A Num prepresenting the states of the system.
-   * @param f The function to integrate. It must take one argument x.
-   * @param x The initial value of x.
-   * @param dtSeconds The time over which to integrate.
-   * @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
-   */
-  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
-  public static <States extends Num> Matrix<States, N1> rungeKutta(
-      Function<Matrix<States, N1>, Matrix<States, N1>> f, Matrix<States, N1> x, double dtSeconds) {
-
-    final var halfDt = 0.5 * dtSeconds;
-    Matrix<States, N1> k1 = f.apply(x);
-    Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)));
-    Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)));
-    Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)));
-    return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
-  }
-}
diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
index a2a11bd965..d56ca7e667 100644
--- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
@@ -12,8 +12,8 @@
 #include "drake/math/discrete_algebraic_riccati_equation.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/system/Discretization.h"
+#include "frc/system/NumericalIntegration.h"
 #include "frc/system/NumericalJacobian.h"
-#include "frc/system/RungeKutta.h"
 #include "units/time.h"
 
 namespace frc {
@@ -149,7 +149,7 @@ class ExtendedKalmanFilter {
     Eigen::Matrix<double, States, States> discQ;
     DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
 
-    m_xHat = RungeKutta(m_f, m_xHat, u, dt);
+    m_xHat = RK4(m_f, m_xHat, u, dt);
     m_P = discA * m_P * discA.transpose() + discQ;
   }
 
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
index ae138773f9..33a24c19bd 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
@@ -13,8 +13,8 @@
 #include "frc/estimator/MerweScaledSigmaPoints.h"
 #include "frc/estimator/UnscentedTransform.h"
 #include "frc/system/Discretization.h"
+#include "frc/system/NumericalIntegration.h"
 #include "frc/system/NumericalJacobian.h"
-#include "frc/system/RungeKutta.h"
 #include "units/time.h"
 
 namespace frc {
@@ -216,7 +216,7 @@ class UnscentedKalmanFilter {
     for (int i = 0; i < m_pts.NumSigmas(); ++i) {
       Eigen::Matrix<double, States, 1> x =
           sigmas.template block<States, 1>(0, i);
-      m_sigmasF.template block<States, 1>(0, i) = RungeKutta(m_f, x, u, dt);
+      m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
     }
 
     auto ret = UnscentedTransform<States, States>(
diff --git a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
new file mode 100644
index 0000000000..edc05255cc
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
@@ -0,0 +1,150 @@
+// 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.
+
+#pragma once
+
+#include <frc/StateSpaceUtil.h>
+
+#include <algorithm>
+
+#include "Eigen/Core"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
+ *
+ * @param f  The function to integrate. It must take one argument x.
+ * @param x  The initial value of x.
+ * @param dt The time over which to integrate.
+ */
+template <typename F, typename T>
+T RK4(F&& f, T x, units::second_t dt) {
+  const auto halfDt = 0.5 * dt;
+  T k1 = f(x);
+  T k2 = f(x + k1 * halfDt.to<double>());
+  T k3 = f(x + k2 * halfDt.to<double>());
+  T k4 = f(x + k3 * dt.to<double>());
+  return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+}
+
+/**
+ * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
+ *
+ * @param f  The function to integrate. It must take two arguments x and u.
+ * @param x  The initial value of x.
+ * @param u  The value u held constant over the integration period.
+ * @param dt The time over which to integrate.
+ */
+template <typename F, typename T, typename U>
+T RK4(F&& f, T x, U u, units::second_t dt) {
+  const auto halfDt = 0.5 * dt;
+  T k1 = f(x, u);
+  T k2 = f(x + k1 * halfDt.to<double>(), u);
+  T k3 = f(x + k2 * halfDt.to<double>(), u);
+  T k4 = f(x + k3 * dt.to<double>(), u);
+  return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+}
+
+/**
+ * Performs 4th order Runge-Kutta integration of dx/dt = f(t, x) for dt.
+ *
+ * @param f  The function to integrate. It must take two arguments x and t.
+ * @param x  The initial value of x.
+ * @param t  The initial value of t.
+ * @param dt The time over which to integrate.
+ */
+template <typename F, typename T>
+T RungeKuttaTimeVarying(F&& f, T x, units::second_t t, units::second_t dt) {
+  const auto halfDt = 0.5 * dt;
+  T k1 = f(t, x);
+  T k2 = f(t + halfDt, x + k1 / 2.0);
+  T k3 = f(t + halfDt, x + k2 / 2.0);
+  T k4 = f(t + dt, x + k3);
+
+  return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+}
+
+template <typename F, typename T, typename U>
+T RKF45Impl(F&& f, T x, U u, double& h, double maxTruncationError,
+            double dtRemaining) {
+  double truncationErr;
+  T newX;
+
+  static double ch[] = {47 / 450.0, 0,        12 / 25.0,
+                        32 / 225.0, 1 / 30.0, 6 / 25.0};
+  static double ct[] = {-1 / 150.0, 0,         3 / 100.0,
+                        -16 / 75.0, -1 / 20.0, 6 / 25.0};
+  static Eigen::Matrix<double, 6, 5> Bs = frc::MakeMatrix<6, 5>(
+      0.0, 0.0, 0.0, 0.0, 0.0, 2 / 9.0, 0.0, 0.0, 0.0, 0.0, 1 / 12.0, 1 / 4.0,
+      0.0, 0.0, 0.0, 69 / 128.0, -243 / 128.0, 135 / 64.0, 0.0, 0.0, -17 / 12.0,
+      27 / 4.0, -27 / 5.0, 16 / 15.0, 0.0, 65 / 432.0, -5 / 16.0, 13 / 16.0,
+      4 / 27.0, 5 / 144.0);
+
+  do {
+    // only allow us to advance up to the dt remaining
+    h = std::min(h, dtRemaining);
+
+    // Notice how the derivative in the Wikipedia notation is dy/dx.
+    // That means their y is our x and their x is our t
+    T k1 = f(x, u) * h;
+    T k2 = f(x + (k1 * Bs(1, 0)), u) * h;
+    T k3 = f(x + (k1 * (Bs(2, 0))) + (k2 * (Bs(2, 1))), u) * h;
+    T k4 =
+        f(x + (k1 * (Bs(3, 0))) + (k2 * (Bs(3, 1))) + (k3 * (Bs(3, 2))), u) * h;
+    T k5 = f(x + (k1 * (Bs(4, 0))) + (k2 * (Bs(4, 1))) + (k3 * (Bs(4, 2))) +
+                 (k4 * (Bs(4, 3))),
+             u) *
+           h;
+    T k6 = f(x + (k1 * (Bs(5, 0))) + (k2 * (Bs(5, 1))) + (k3 * (Bs(5, 2))) +
+                 (k4 * (Bs(5, 3))) + (k5 * (Bs(5, 4))),
+             u) *
+           h;
+
+    newX = x + (k1 * (ch[0])) + (k2 * (ch[1])) + (k3 * (ch[2])) +
+           (k4 * (ch[3])) + (k5 * (ch[4])) + (k6 * (ch[5]));
+
+    truncationErr = (k1 * (ct[0]) + (k2 * (ct[1])) + (k3 * (ct[2])) +
+                     (k4 * (ct[3])) + (k5 * (ct[4])) + (k6 * (ct[5])))
+                        .norm();
+
+    h = 0.9 * h * std::pow(maxTruncationError / truncationErr, 1 / 5.);
+  } while (truncationErr > maxTruncationError);
+
+  // Return the new x. Dt is changed by reference
+  return newX;
+}
+
+/**
+ * Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described
+ * in https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
+ *
+ * @param f        The function to integrate. It must take two arguments x and
+ *                 u.
+ * @param x        The initial value of x.
+ * @param u        The value u held constant over the integration period.
+ * @param dt       The time over which to integrate.
+ * @param maxError The maximum acceptable truncation error. Usually a small
+ *                 number like 1e-6.
+ */
+template <typename F, typename T, typename U>
+T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
+  double dtElapsed = 0;
+  double dtSeconds = dt.to<double>();
+  double previousH = dt.to<double>();
+
+  // Loop until we've gotten to our desired dt
+  while (dtElapsed < dtSeconds) {
+    // RKF45 will give us an updated x and a dt back.
+    // We use the new dt (h) as the initial dt for our next loop
+    // previousH is changed by-reference
+    T ret = RKF45Impl(f, x, u, previousH, maxError, dtSeconds - dtElapsed);
+    dtElapsed += previousH;
+    x = ret;
+  }
+  return x;
+}
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/system/RungeKutta.h b/wpimath/src/main/native/include/frc/system/RungeKutta.h
deleted file mode 100644
index 1ec2bfdfb4..0000000000
--- a/wpimath/src/main/native/include/frc/system/RungeKutta.h
+++ /dev/null
@@ -1,66 +0,0 @@
-// 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.
-
-#pragma once
-
-#include "Eigen/Core"
-#include "units/time.h"
-
-namespace frc {
-
-/**
- * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
- *
- * @param f  The function to integrate. It must take one argument x.
- * @param x  The initial value of x.
- * @param dt The time over which to integrate.
- */
-template <typename F, typename T>
-T RungeKutta(F&& f, T x, units::second_t dt) {
-  const auto halfDt = 0.5 * dt;
-  T k1 = f(x);
-  T k2 = f(x + k1 * halfDt.to<double>());
-  T k3 = f(x + k2 * halfDt.to<double>());
-  T k4 = f(x + k3 * dt.to<double>());
-  return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
-}
-
-/**
- * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
- *
- * @param f  The function to integrate. It must take two arguments x and u.
- * @param x  The initial value of x.
- * @param u  The value u held constant over the integration period.
- * @param dt The time over which to integrate.
- */
-template <typename F, typename T, typename U>
-T RungeKutta(F&& f, T x, U u, units::second_t dt) {
-  const auto halfDt = 0.5 * dt;
-  T k1 = f(x, u);
-  T k2 = f(x + k1 * halfDt.to<double>(), u);
-  T k3 = f(x + k2 * halfDt.to<double>(), u);
-  T k4 = f(x + k3 * dt.to<double>(), u);
-  return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
-}
-
-/**
- * Performs 4th order Runge-Kutta integration of dx/dt = f(t, x) for dt.
- *
- * @param f  The function to integrate. It must take two arguments x and t.
- * @param x  The initial value of x.
- * @param t  The initial value of t.
- * @param dt The time over which to integrate.
- */
-template <typename F, typename T>
-T RungeKuttaTimeVarying(F&& f, T x, units::second_t t, units::second_t dt) {
-  const auto halfDt = 0.5 * dt;
-  T k1 = f(t, x);
-  T k2 = f(t + halfDt, x + k1 / 2.0);
-  T k3 = f(t + halfDt, x + k2 / 2.0);
-  T k4 = f(t + dt, x + k3);
-
-  return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
-}
-
-}  // namespace frc
diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java
index 95b159cfff..79810c8bff 100644
--- a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java
+++ b/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;
 
diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java
index 135c85a26d..2a68283960 100644
--- a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java
+++ b/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);
diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/system/RungeKuttaTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/system/NumericalIntegrationTest.java
similarity index 61%
rename from wpimath/src/test/java/edu/wpi/first/wpilibj/system/RungeKuttaTest.java
rename to wpimath/src/test/java/edu/wpi/first/wpilibj/system/NumericalIntegrationTest.java
index 22418c09a7..1f67e26cab 100644
--- a/wpimath/src/test/java/edu/wpi/first/wpilibj/system/RungeKuttaTest.java
+++ b/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);
+  }
 }
diff --git a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
index 940c9c57ed..89d7c402cd 100644
--- a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
+++ b/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
diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
index 2ae76662a2..a659baede2 100644
--- a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
+++ b/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);
diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
index 3367bd2680..4269f0afc4 100644
--- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
+++ b/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,
diff --git a/wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
similarity index 70%
rename from wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp
rename to wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
index 4a098a4e5e..c2e6a3d05b 100644
--- a/wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp
+++ b/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(