diff --git a/sysid/src/main/native/cpp/analysis/ArmSim.cpp b/sysid/src/main/native/cpp/analysis/ArmSim.cpp
index a72f50569d1..2b3962b837f 100644
--- a/sysid/src/main/native/cpp/analysis/ArmSim.cpp
+++ b/sysid/src/main/native/cpp/analysis/ArmSim.cpp
@@ -42,7 +42,7 @@ void ArmSim::Update(units::volt_t voltage, units::second_t dt) {
   // small for ill-conditioned data (e.g., high velocities with sharp spikes in
   // acceleration).
   Eigen::Vector<double, 1> u{voltage.value()};
-  m_x = frc::RKDP(f, m_x, u, dt, 0.25);
+  m_x = frc::Tsit5(f, m_x, u, dt, 0.25);
 }
 
 double ArmSim::GetPosition() const {
diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
index 19f3ab6c7d3..c9dbddb7886 100644
--- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
@@ -59,7 +59,8 @@ void DifferentialDrivetrainSim::SetGearing(double newGearing) {
 }
 
 void DifferentialDrivetrainSim::Update(units::second_t dt) {
-  m_x = RKDP([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt);
+  m_x =
+      Tsit5([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 4553dc4d0a1..0e0bc4e885b 100644
--- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
@@ -104,7 +104,7 @@ void ElevatorSim::SetInputVoltage(units::volt_t voltage) {
 
 Vectord<2> ElevatorSim::UpdateX(const Vectord<2>& currentXhat,
                                 const Vectord<1>& u, units::second_t dt) {
-  auto updatedXhat = RKDP(
+  auto updatedXhat = Tsit5(
       [&](const Vectord<2>& x, const Vectord<1>& u_) -> Vectord<2> {
         Vectord<2> xdot = m_plant.A() * x + m_plant.B() * u;
 
diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
index f318d083000..23945eac774 100644
--- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
@@ -112,7 +112,7 @@ Vectord<2> SingleJointedArmSim::UpdateX(const Vectord<2>& currentXhat,
   //   f(x, u) = Ax + Bu + [0  α]ᵀ
   //   f(x, u) = Ax + Bu + [0  3/2⋅g⋅cos(θ)/L]ᵀ
 
-  Vectord<2> updatedXhat = RKDP(
+  Vectord<2> updatedXhat = Tsit5(
       [&](const auto& x, const auto& u) -> Vectord<2> {
         Vectord<2> xdot = m_plant.A() * x + m_plant.B() * u;
 
diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
index 9ed23624de3..43d4fee5c03 100644
--- a/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
@@ -55,7 +55,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
     sim.Update(20_ms);
 
     // Update ground truth.
-    groundTruthX = frc::RKDP(
+    groundTruthX = frc::Tsit5(
         [&sim](const auto& x, const auto& u) -> frc::Vectord<7> {
           return sim.Dynamics(x, u);
         },
@@ -63,7 +63,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
   }
 
   // 2 inch tolerance is OK since our ground truth is an approximation of the
-  // ODE solution using RKDP anyway
+  // ODE solution using Tsit5 anyway
   EXPECT_NEAR(groundTruthX(0, 0), sim.GetPose().X().value(), 0.05);
   EXPECT_NEAR(groundTruthX(1, 0), sim.GetPose().Y().value(), 0.05);
   EXPECT_NEAR(groundTruthX(2, 0), sim.GetHeading().Radians().value(), 0.01);
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 6e25b344393..6f78c77bd3e 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
@@ -148,7 +148,7 @@ public void setInputs(double leftVoltageVolts, double rightVoltageVolts) {
    * @param dtSeconds the time difference
    */
   public void update(double dtSeconds) {
-    m_x = NumericalIntegration.rkdp(this::getDynamics, m_x, m_u, dtSeconds);
+    m_x = NumericalIntegration.tsit5(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 2f7ce3e768d..d9cbda4f50a 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
@@ -235,7 +235,7 @@ public void setInputVoltage(double volts) {
   protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
     // Calculate updated x-hat from Runge-Kutta.
     var updatedXhat =
-        NumericalIntegration.rkdp(
+        NumericalIntegration.tsit5(
             (x, _u) -> {
               Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
               if (m_simulateGravity) {
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 7d7ae78284d..32eba10cb1a 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
@@ -244,7 +244,7 @@ protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, d
     //   f(x, u) = Ax + Bu + [0  3/2⋅g⋅cos(θ)/L]ᵀ
 
     Matrix<N2, N1> updatedXhat =
-        NumericalIntegration.rkdp(
+        NumericalIntegration.tsit5(
             (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 25bbdb59834..1889a606791 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
@@ -76,11 +76,11 @@ void testConvergence() {
       sim.update(0.020);
 
       // Update our ground truth
-      groundTruthX = NumericalIntegration.rkdp(sim::getDynamics, groundTruthX, voltages, 0.020);
+      groundTruthX = NumericalIntegration.tsit5(sim::getDynamics, groundTruthX, voltages, 0.020);
     }
 
     // 2 inch tolerance is OK since our ground truth is an approximation of the
-    // ODE solution using RKDP anyway
+    // ODE solution using tsit5 anyway
     assertEquals(
         groundTruthX.get(DifferentialDrivetrainSim.State.kX.value, 0),
         sim.getState(DifferentialDrivetrainSim.State.kX),
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
index 1dcddea8404..136e44ae5a2 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
@@ -144,8 +144,10 @@ public static <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> rk4(
    * @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.
+   * @deprecated Use tsit5() instead.
    */
   @SuppressWarnings("overloads")
+  @Deprecated(forRemoval = true, since = "2025")
   public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
       BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
       Matrix<States, N1> x,
@@ -165,8 +167,10 @@ public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
    * @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.
+   * @deprecated use tsit5() instead.
    */
   @SuppressWarnings("overloads")
+  @Deprecated(forRemoval = true, since = "2025")
   public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
       BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
       Matrix<States, N1> x,
@@ -290,8 +294,10 @@ public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
    * @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.
+   * @deprecated Use tsit5() instead
    */
   @SuppressWarnings("overloads")
+  @Deprecated(forRemoval = true, since = "2025")
   public static <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> rkdp(
       BiFunction<Double, Matrix<Rows, Cols>, Matrix<Rows, Cols>> f,
       double t,
@@ -406,4 +412,316 @@ public static <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> rkdp(
 
     return y;
   }
+
+  /**
+   * Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt. 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("overloads")
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> tsit5(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x,
+      Matrix<Inputs, N1> u,
+      double dtSeconds) {
+    return tsit5(f, x, u, dtSeconds, 1e-6);
+  }
+
+  /**
+   * Performs adaptive Tsitouras 5th Order 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.
+   * @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("overloads")
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> tsit5(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x,
+      Matrix<Inputs, N1> u,
+      double dtSeconds,
+      double maxError) {
+    // See http://users.uoa.gr/~tsitourasc/RK54_new_v2.pdf for the
+    // Butcher tableau the following arrays came from.
+
+    // final double[6][6]
+    final double[][] A = {
+      {0.161},
+      {-0.008480655492357, 0.3354806554923570},
+      {2.897153057105494, -6.359448489975075, 4.362295432869581},
+      {5.32586482843925895, -11.74888356406283, 7.495539342889836, -0.09249506636175525},
+      {
+        5.86145544294642038,
+        -12.92096931784711,
+        8.159367898576159,
+        -0.07158497328140100,
+        -0.02826905039406838
+      },
+      {
+        0.09646076681806523,
+        0.01,
+        0.4798896504144996,
+        1.379008574103742,
+        -3.290069515436081,
+        2.324710524099774
+      }
+    };
+
+    // final double[7]
+    final double[] b1 = {
+      0.09646076681806523,
+      0.01,
+      0.4798896504144996,
+      1.379008574103742,
+      -3.290069515436081,
+      2.324710524099774,
+      0.0
+    };
+
+    // final double[7]
+    final double[] b2 = {
+      0.001780011052226,
+      0.000816434459657,
+      -0.007880878010262,
+      0.144711007173263,
+      -0.582357165452555,
+      0.458082105929187,
+      1.0 / 66.0
+    };
+
+    Matrix<States, N1> newX;
+    double truncationError;
+
+    double dtElapsed = 0.0;
+    double h = dtSeconds;
+
+    // Loop until we've gotten to our desired dt
+    while (dtElapsed < dtSeconds) {
+      do {
+        // Only allow us to advance up to the dt remaining
+        h = Math.min(h, dtSeconds - dtElapsed);
+
+        var k1 = f.apply(x, u);
+        var k2 = f.apply(x.plus(k1.times(A[0][0]).times(h)), u);
+        var k3 = f.apply(x.plus(k1.times(A[1][0]).plus(k2.times(A[1][1])).times(h)), u);
+        var k4 =
+            f.apply(
+                x.plus(k1.times(A[2][0]).plus(k2.times(A[2][1])).plus(k3.times(A[2][2])).times(h)),
+                u);
+        var k5 =
+            f.apply(
+                x.plus(
+                    k1.times(A[3][0])
+                        .plus(k2.times(A[3][1]))
+                        .plus(k3.times(A[3][2]))
+                        .plus(k4.times(A[3][3]))
+                        .times(h)),
+                u);
+        var k6 =
+            f.apply(
+                x.plus(
+                    k1.times(A[4][0])
+                        .plus(k2.times(A[4][1]))
+                        .plus(k3.times(A[4][2]))
+                        .plus(k4.times(A[4][3]))
+                        .plus(k5.times(A[4][4]))
+                        .times(h)),
+                u);
+
+        // Since the final row of A and the array b1 have the same coefficients
+        // and k7 has no effect on newX, we can reuse the calculation.
+        newX =
+            x.plus(
+                k1.times(A[5][0])
+                    .plus(k2.times(A[5][1]))
+                    .plus(k3.times(A[5][2]))
+                    .plus(k4.times(A[5][3]))
+                    .plus(k5.times(A[5][4]))
+                    .plus(k6.times(A[5][5]))
+                    .times(h));
+        var k7 = f.apply(newX, u);
+
+        truncationError =
+            (k1.times(b1[0] - b2[0])
+                    .plus(k2.times(b1[1] - b2[1]))
+                    .plus(k3.times(b1[2] - b2[2]))
+                    .plus(k4.times(b1[3] - b2[3]))
+                    .plus(k5.times(b1[4] - b2[4]))
+                    .plus(k6.times(b1[5] - b2[5]))
+                    .plus(k7.times(b1[6] - b2[6]))
+                    .times(h))
+                .normF();
+
+        if (truncationError == 0.0) {
+          h = dtSeconds - dtElapsed;
+        } else {
+          h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
+        }
+      } while (truncationError > maxError);
+
+      dtElapsed += h;
+      x = newX;
+    }
+
+    return x;
+  }
+
+  /**
+   * Performs adaptive Tsitouras 5th Order 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.
+   * @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("overloads")
+  public static <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> tsit5(
+      BiFunction<Double, Matrix<Rows, Cols>, Matrix<Rows, Cols>> f,
+      double t,
+      Matrix<Rows, Cols> y,
+      double dtSeconds,
+      double maxError) {
+    // See http://users.uoa.gr/~tsitourasc/RK54_new_v2.pdf for the
+    // Butcher tableau the following arrays came from.
+
+    // final double[6][6]
+    final double[][] A = {
+      {0.161},
+      {-0.008480655492357, 0.3354806554923570},
+      {2.897153057105494, -6.359448489975075, 4.362295432869581},
+      {5.32586482843925895, -11.74888356406283, 7.495539342889836, -0.09249506636175525},
+      {
+        5.86145544294642038,
+        -12.92096931784711,
+        8.159367898576159,
+        -0.07158497328140100,
+        -0.02826905039406838
+      },
+      {
+        0.09646076681806523,
+        0.01,
+        0.4798896504144996,
+        1.379008574103742,
+        -3.290069515436081,
+        2.324710524099774
+      }
+    };
+
+    // final double[7]
+    final double[] b1 = {
+      0.09646076681806523,
+      0.01,
+      0.4798896504144996,
+      1.379008574103742,
+      -3.290069515436081,
+      2.324710524099774,
+      0.0
+    };
+
+    // final double[7]
+    final double[] b2 = {
+      0.001780011052226,
+      0.000816434459657,
+      -0.007880878010262,
+      0.144711007173263,
+      -0.582357165452555,
+      0.458082105929187,
+      1.0 / 66.0
+    };
+
+    // final double[6]
+    final double[] c = {0.161, 0.327, 0.9, 0.9800255409045097, 1.0, 1.0};
+
+    Matrix<Rows, Cols> newY;
+    double truncationError;
+
+    double dtElapsed = 0.0;
+    double h = dtSeconds;
+
+    // Loop until we've gotten to our desired dt
+    while (dtElapsed < dtSeconds) {
+      do {
+        // Only allow us to advance up to the dt remaining
+        h = Math.min(h, dtSeconds - dtElapsed);
+
+        var k1 = f.apply(t, y);
+        var k2 = f.apply(t + h * c[0], y.plus(k1.times(A[0][0]).times(h)));
+        var k3 = f.apply(t + h * c[1], y.plus(k1.times(A[1][0]).plus(k2.times(A[1][1])).times(h)));
+        var k4 =
+            f.apply(
+                t + h * c[2],
+                y.plus(k1.times(A[2][0]).plus(k2.times(A[2][1])).plus(k3.times(A[2][2])).times(h)));
+        var k5 =
+            f.apply(
+                t + h * c[3],
+                y.plus(
+                    k1.times(A[3][0])
+                        .plus(k2.times(A[3][1]))
+                        .plus(k3.times(A[3][2]))
+                        .plus(k4.times(A[3][3]))
+                        .times(h)));
+        var k6 =
+            f.apply(
+                t + h * c[4],
+                y.plus(
+                    k1.times(A[4][0])
+                        .plus(k2.times(A[4][1]))
+                        .plus(k3.times(A[4][2]))
+                        .plus(k4.times(A[4][3]))
+                        .plus(k5.times(A[4][4]))
+                        .times(h)));
+
+        // Since the final row of A and the array b1 have the same coefficients
+        // and k7 has no effect on newY, we can reuse the calculation.
+        newY =
+            y.plus(
+                k1.times(A[5][0])
+                    .plus(k2.times(A[5][1]))
+                    .plus(k3.times(A[5][2]))
+                    .plus(k4.times(A[5][3]))
+                    .plus(k5.times(A[5][4]))
+                    .plus(k6.times(A[5][5]))
+                    .times(h));
+        var k7 = f.apply(t + h * c[5], newY);
+
+        truncationError =
+            (k1.times(b1[0] - b2[0])
+                    .plus(k2.times(b1[1] - b2[1]))
+                    .plus(k3.times(b1[2] - b2[2]))
+                    .plus(k4.times(b1[3] - b2[3]))
+                    .plus(k5.times(b1[4] - b2[4]))
+                    .plus(k6.times(b1[5] - b2[5]))
+                    .plus(k7.times(b1[6] - b2[6]))
+                    .times(h))
+                .normF();
+
+        if (truncationError == 0.0) {
+          h = dtSeconds - dtElapsed;
+        } else {
+          h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
+        }
+      } while (truncationError > maxError);
+
+      dtElapsed += h;
+      y = newY;
+    }
+
+    return y;
+  }
 }
diff --git a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
index 2fe1946f0d3..d163068ce11 100644
--- a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
+++ b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
@@ -81,8 +81,10 @@ T RK4(F&& f, units::second_t t, T y, units::second_t dt) {
  * @param dt       The time over which to integrate.
  * @param maxError The maximum acceptable truncation error. Usually a small
  *                 number like 1e-6.
+ * @deprecated Use Tsit5() instead
  */
 template <typename F, typename T, typename U>
+[[deprecated("Use AprilTagFieldLayout::LoadField() instead")]]
 T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
   // See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the
   // Butcher tableau the following arrays came from.
@@ -164,8 +166,10 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
  * @param dt       The time over which to integrate.
  * @param maxError The maximum acceptable truncation error. Usually a small
  *                 number like 1e-6.
+ * @deprecated Use Tsit5() instead
  */
 template <typename F, typename T>
+[[deprecated("Use AprilTagFieldLayout::LoadField() instead")]]
 T RKDP(F&& f, units::second_t t, T y, units::second_t dt,
        double maxError = 1e-6) {
   // See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the
@@ -237,4 +241,178 @@ T RKDP(F&& f, units::second_t t, T y, units::second_t dt,
   return y;
 }
 
+/**
+ * Performs adaptive Tsitouras 5th Order 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.
+ * @param maxError The maximum acceptable truncation error. Usually a small
+ *                 number like 1e-6.
+ */
+template <typename F, typename T, typename U>
+T Tsit5(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
+  // See http://users.uoa.gr/~tsitourasc/RK54_new_v2.pdf for the
+  // Butcher tableau the following arrays came from.
+
+  constexpr int kDim = 7;
+
+  // clang-format off
+  constexpr double A[kDim - 1][kDim - 1]{
+      {              0.161},
+      {  -0.008480655492357,    0.3354806554923570},
+      {   2.897153057105494,     -6.359448489975075,   4.362295432869581},
+      { 5.32586482843925895,     -11.74888356406283,    7.495539342889836, -0.09249506636175525},
+      { 5.86145544294642038,     -12.92096931784711,    8.159367898576159,  -0.07158497328140100, -0.02826905039406838},
+      { 0.09646076681806523,                   0.01,   0.4798896504144996,     1.379008574103742,    -3.290069515436081, 2.324710524099774}};
+  // clang-format on
+
+  constexpr std::array<double, kDim> b1{0.09646076681806523,
+                                        0.01,
+                                        0.4798896504144996,
+                                        1.379008574103742,
+                                        -3.290069515436081,
+                                        2.324710524099774,
+                                        0.0};
+  constexpr std::array<double, kDim> b2{0.001780011052226,  0.000816434459657,
+                                        -0.007880878010262, 0.144711007173263,
+                                        -0.582357165452555, 0.458082105929187,
+                                        1.0 / 66.0};
+
+  T newX;
+  double truncationError;
+
+  double dtElapsed = 0.0;
+  double h = dt.value();
+
+  // Loop until we've gotten to our desired dt
+  while (dtElapsed < dt.value()) {
+    do {
+      // Only allow us to advance up to the dt remaining
+      h = (std::min)(h, dt.value() - dtElapsed);
+
+      // clang-format off
+      T k1 = f(x, u);
+      T k2 = f(x + h * (A[0][0] * k1), u);
+      T k3 = f(x + h * (A[1][0] * k1 + A[1][1] * k2), u);
+      T k4 = f(x + h * (A[2][0] * k1 + A[2][1] * k2 + A[2][2] * k3), u);
+      T k5 = f(x + h * (A[3][0] * k1 + A[3][1] * k2 + A[3][2] * k3 + A[3][3] * k4), u);
+      T k6 = f(x + h * (A[4][0] * k1 + A[4][1] * k2 + A[4][2] * k3 + A[4][3] * k4 + A[4][4] * k5), u);
+      // clang-format on
+
+      // Since the final row of A and the array b1 have the same coefficients
+      // and k7 has no effect on newX, we can reuse the calculation.
+      newX = x + h * (A[5][0] * k1 + A[5][1] * k2 + A[5][2] * k3 +
+                      A[5][3] * k4 + A[5][4] * k5 + A[5][5] * k6);
+      T k7 = f(newX, u);
+
+      truncationError = (h * ((b1[0] - b2[0]) * k1 + (b1[1] - b2[1]) * k2 +
+                              (b1[2] - b2[2]) * k3 + (b1[3] - b2[3]) * k4 +
+                              (b1[4] - b2[4]) * k5 + (b1[5] - b2[5]) * k6 +
+                              (b1[6] - b2[6]) * k7))
+                            .norm();
+
+      if (truncationError == 0.0) {
+        h = dt.value() - dtElapsed;
+      } else {
+        h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
+      }
+    } while (truncationError > maxError);
+
+    dtElapsed += h;
+    x = newX;
+  }
+
+  return x;
+}
+
+/**
+ * Performs adaptive Tsitouras 5th Order integration of dy/dt = f(t, y) for dt.
+ *
+ * @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 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>
+T Tsit5(F&& f, units::second_t t, T y, units::second_t dt,
+        double maxError = 1e-6) {
+  // See http://users.uoa.gr/~tsitourasc/RK54_new_v2.pdf for the
+  // Butcher tableau the following arrays came from.
+
+  constexpr int kDim = 7;
+
+  // clang-format off
+  constexpr double A[kDim - 1][kDim - 1]{
+        {              0.161},
+        {  -0.008480655492357,    0.3354806554923570},
+        {   2.897153057105494,     -6.359448489975075,   4.362295432869581},
+        { 5.32586482843925895,     -11.74888356406283,    7.495539342889836, -0.09249506636175525},
+        { 5.86145544294642038,     -12.92096931784711,    8.159367898576159,  -0.07158497328140100, -0.02826905039406838},
+        { 0.09646076681806523,                   0.01,   0.4798896504144996,     1.379008574103742,    -3.290069515436081, 2.324710524099774}};
+  // clang-format on
+
+  constexpr std::array<double, kDim> b1{0.09646076681806523,
+                                        0.01,
+                                        0.4798896504144996,
+                                        1.379008574103742,
+                                        -3.290069515436081,
+                                        2.324710524099774,
+                                        0.0};
+  constexpr std::array<double, kDim> b2{0.001780011052226,  0.000816434459657,
+                                        -0.007880878010262, 0.144711007173263,
+                                        -0.582357165452555, 0.458082105929187,
+                                        1.0 / 66.0};
+
+  constexpr std::array<double, kDim - 1> c{
+      0.161, 0.327, 0.9, 0.9800255409045097, 1.0, 1.0};
+
+  T newY;
+  double truncationError;
+
+  double dtElapsed = 0.0;
+  double h = dt.to<double>();
+
+  // Loop until we've gotten to our desired dt
+  while (dtElapsed < dt.to<double>()) {
+    do {
+      // Only allow us to advance up to the dt remaining
+      h = std::min(h, dt.to<double>() - dtElapsed);
+
+      // clang-format off
+      T k1 = f(t, y);
+      T k2 = f(t + units::second_t{h} * c[0], y + h * (A[0][0] * k1));
+      T k3 = f(t + units::second_t{h} * c[1], y + h * (A[1][0] * k1 + A[1][1] * k2));
+      T k4 = f(t + units::second_t{h} * c[2], y + h * (A[2][0] * k1 + A[2][1] * k2 + A[2][2] * k3));
+      T k5 = f(t + units::second_t{h} * c[3], y + h * (A[3][0] * k1 + A[3][1] * k2 + A[3][2] * k3 + A[3][3] * k4));
+      T k6 = f(t + units::second_t{h} * c[4], y + h * (A[4][0] * k1 + A[4][1] * k2 + A[4][2] * k3 + A[4][3] * k4 + A[4][4] * k5));
+      // clang-format on
+
+      // Since the final row of A and the array b1 have the same coefficients
+      // and k7 has no effect on newY, we can reuse the calculation.
+      newY = y + h * (A[5][0] * k1 + A[5][1] * k2 + A[5][2] * k3 +
+                      A[5][3] * k4 + A[5][4] * k5 + A[5][5] * k6);
+      T k7 = f(t + units::second_t{h} * c[5], newY);
+
+      truncationError = (h * ((b1[0] - b2[0]) * k1 + (b1[1] - b2[1]) * k2 +
+                              (b1[2] - b2[2]) * k3 + (b1[3] - b2[3]) * k4 +
+                              (b1[4] - b2[4]) * k5 + (b1[5] - b2[5]) * k6 +
+                              (b1[6] - b2[6]) * k7))
+                            .norm();
+
+      h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
+    } while (truncationError > maxError);
+
+    dtElapsed += h;
+    y = newY;
+  }
+
+  return y;
+}
+
 }  // namespace frc
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java
index 99bc544bd04..76cbcab810e 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java
@@ -107,7 +107,7 @@ void testReachesReference() {
               robotPose, x.get(State.kLeftVelocity, 0), x.get(State.kRightVelocity, 0), state);
 
       x =
-          NumericalIntegration.rkdp(
+          NumericalIntegration.tsit5(
               LTVDifferentialDriveControllerTest::dynamics,
               x,
               VecBuilder.fill(output.left, output.right),
diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
index db7cf5c6d1c..4ce116aaef8 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
@@ -54,6 +54,7 @@ void testRK4TimeVarying() {
   }
 
   @Test
+  @Deprecated(forRemoval = true, since = "2025")
   void testZeroRKDP() {
     var y1 =
         NumericalIntegration.rkdp(
@@ -63,6 +64,7 @@ void testZeroRKDP() {
   }
 
   @Test
+  @Deprecated(forRemoval = true, since = "2025")
   void testExponentialRKDP() {
     Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
 
@@ -89,6 +91,7 @@ void testExponentialRKDP() {
   //
   //   x(t) = 12eᵗ/(eᵗ + 1)²
   @Test
+  @Deprecated(forRemoval = true, since = "2025")
   void testRKDPTimeVarying() {
     final var y0 = VecBuilder.fill(12.0 * Math.exp(5.0) / Math.pow(Math.exp(5.0) + 1.0, 2.0));
 
@@ -103,4 +106,55 @@ void testRKDPTimeVarying() {
             1e-12);
     assertEquals(12.0 * Math.exp(6.0) / Math.pow(Math.exp(6.0) + 1.0, 2.0), y1.get(0, 0), 1e-3);
   }
+
+  @Test
+  void testZeroTsit5() {
+    var y1 =
+        NumericalIntegration.tsit5(
+            (x, u) -> VecBuilder.fill(0), VecBuilder.fill(0), VecBuilder.fill(0), 0.1);
+
+    assertEquals(0.0, y1.get(0, 0), 1e-3);
+  }
+
+  @Test
+  void testExponentialTsit5() {
+    Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
+
+    var y1 =
+        NumericalIntegration.tsit5(
+            (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);
+  }
+
+  // Tests Tsit5 with a time varying solution. From
+  // http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
+  //
+  //   dx/dt = x(2/(eᵗ + 1) - 1)
+  //
+  // The true (analytical) solution is:
+  //
+  //   x(t) = 12eᵗ/(eᵗ + 1)²
+  @Test
+  void testTsit5TimeVarying() {
+    final var y0 = VecBuilder.fill(12.0 * Math.exp(5.0) / Math.pow(Math.exp(5.0) + 1.0, 2.0));
+
+    final var y1 =
+        NumericalIntegration.tsit5(
+            (Double t, Matrix<N1, N1> y) ->
+                MatBuilder.fill(
+                    Nat.N1(), Nat.N1(), y.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0)),
+            5.0,
+            y0,
+            1.0,
+            1e-12);
+    assertEquals(12.0 * Math.exp(6.0) / Math.pow(Math.exp(6.0) + 1.0, 2.0), y1.get(0, 0), 1e-3);
+  }
 }
diff --git a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
index f602c702448..e1d1b6e85e4 100644
--- a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
@@ -88,9 +88,9 @@ TEST(LTVDifferentialDriveControllerTest, ReachesReference) {
         robotPose, units::meters_per_second_t{x(State::kLeftVelocity)},
         units::meters_per_second_t{x(State::kRightVelocity)}, state);
 
-    x = frc::RKDP(&Dynamics, x,
-                  frc::Vectord<2>{leftVoltage.value(), rightVoltage.value()},
-                  kDt);
+    x = frc::Tsit5(&Dynamics, x,
+                   frc::Vectord<2>{leftVoltage.value(), rightVoltage.value()},
+                   kDt);
   }
 
   auto& endPose = trajectory.States().back().pose;
diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
index 055ee49736b..38f2879cdf3 100644
--- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
@@ -62,9 +62,9 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
   // Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
   //       0
   frc::Matrixd<2, 2> discQIntegrated =
-      frc::RKDP<std::function<frc::Matrixd<2, 2>(units::second_t,
-                                                 const frc::Matrixd<2, 2>&)>,
-                frc::Matrixd<2, 2>>(
+      frc::Tsit5<std::function<frc::Matrixd<2, 2>(units::second_t,
+                                                  const frc::Matrixd<2, 2>&)>,
+                 frc::Matrixd<2, 2>>(
           [&](units::second_t t, const frc::Matrixd<2, 2>&) {
             return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
                                       (contA.transpose() * t.value()).exp());
@@ -94,9 +94,9 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
   // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
   //       0
   frc::Matrixd<2, 2> discQIntegrated =
-      frc::RKDP<std::function<frc::Matrixd<2, 2>(units::second_t,
-                                                 const frc::Matrixd<2, 2>&)>,
-                frc::Matrixd<2, 2>>(
+      frc::Tsit5<std::function<frc::Matrixd<2, 2>(units::second_t,
+                                                  const frc::Matrixd<2, 2>&)>,
+                 frc::Matrixd<2, 2>>(
           [&](units::second_t t, const frc::Matrixd<2, 2>&) {
             return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
                                       (contA.transpose() * t.value()).exp());
diff --git a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
index 05ec0a48265..11b20fca94d 100644
--- a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
@@ -53,6 +53,7 @@ TEST(NumericalIntegrationTest, RK4TimeVarying) {
 }
 
 // Tests that integrating dx/dt = 0 works with RKDP
+[[deprecated("Use Tsit5() instead")]]
 TEST(NumericalIntegrationTest, ZeroRKDP) {
   frc::Vectord<1> y1 = frc::RKDP(
       [](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
@@ -63,6 +64,7 @@ TEST(NumericalIntegrationTest, ZeroRKDP) {
 }
 
 // Tests that integrating dx/dt = eˣ works with RKDP
+[[deprecated("Use Tsit5() instead")]]
 TEST(NumericalIntegrationTest, ExponentialRKDP) {
   frc::Vectord<1> y0{0.0};
 
@@ -82,6 +84,7 @@ TEST(NumericalIntegrationTest, ExponentialRKDP) {
 // The true (analytical) solution is:
 //
 //   x(t) = 12eᵗ/(eᵗ + 1)²
+[[deprecated("Use Tsit5() instead")]]
 TEST(NumericalIntegrationTest, RKDPTimeVarying) {
   frc::Vectord<1> y0{12.0 * std::exp(5.0) / std::pow(std::exp(5.0) + 1.0, 2.0)};
 
@@ -94,3 +97,46 @@ TEST(NumericalIntegrationTest, RKDPTimeVarying) {
   EXPECT_NEAR(y1(0), 12.0 * std::exp(6.0) / std::pow(std::exp(6.0) + 1.0, 2.0),
               1e-3);
 }
+
+// Tests that integrating dx/dt = 0 works with Tsit5
+TEST(NumericalIntegrationTest, ZeroTsit5) {
+  frc::Vectord<1> y1 = frc::Tsit5(
+      [](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
+        return frc::Vectord<1>::Zero();
+      },
+      frc::Vectord<1>{0.0}, frc::Vectord<1>{0.0}, 0.1_s);
+  EXPECT_NEAR(y1(0), 0.0, 1e-3);
+}
+
+// Tests that integrating dx/dt = eˣ works with Tsit5
+TEST(NumericalIntegrationTest, ExponentialTsit5) {
+  frc::Vectord<1> y0{0.0};
+
+  frc::Vectord<1> y1 = frc::Tsit5(
+      [](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
+        return frc::Vectord<1>{std::exp(x(0))};
+      },
+      y0, frc::Vectord<1>{0.0}, 0.1_s);
+  EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
+}
+
+// Tests Tsit5 with a time varying solution. From
+// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
+//
+//   dx/dt = x(2/(eᵗ + 1) - 1)
+//
+// The true (analytical) solution is:
+//
+//   x(t) = 12eᵗ/(eᵗ + 1)²
+TEST(NumericalIntegrationTest, Tsit5TimeVarying) {
+  frc::Vectord<1> y0{12.0 * std::exp(5.0) / std::pow(std::exp(5.0) + 1.0, 2.0)};
+
+  frc::Vectord<1> y1 = frc::Tsit5(
+      [](units::second_t t, const frc::Vectord<1>& x) {
+        return frc::Vectord<1>{x(0) *
+                               (2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
+      },
+      5_s, y0, 1_s, 1e-12);
+  EXPECT_NEAR(y1(0), 12.0 * std::exp(6.0) / std::pow(std::exp(6.0) + 1.0, 2.0),
+              1e-3);
+}