diff --git a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
index 4e9121caeb..99b3efa410 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
@@ -491,9 +491,27 @@ public class Matrix<R extends Num, C extends Num> {
     return new Matrix<>(
         this.m_storage.extractMatrix(
             startingRow,
-            Objects.requireNonNull(height).getNum() + startingRow,
+            startingRow + Objects.requireNonNull(height).getNum(),
             startingCol,
-            Objects.requireNonNull(width).getNum() + startingCol));
+            startingCol + Objects.requireNonNull(width).getNum()));
+  }
+
+  /**
+   * Extracts a matrix of a given size and start position with new underlying storage.
+   *
+   * @param <R2> Number of rows to extract.
+   * @param <C2> Number of columns to extract.
+   * @param height The number of rows of the extracted matrix.
+   * @param width The number of columns of the extracted matrix.
+   * @param startingRow The starting row of the extracted matrix.
+   * @param startingCol The starting column of the extracted matrix.
+   * @return The extracted matrix.
+   */
+  public final <R2 extends Num, C2 extends Num> Matrix<R2, C2> block(
+      int height, int width, int startingRow, int startingCol) {
+    return new Matrix<R2, C2>(
+        this.m_storage.extractMatrix(
+            startingRow, startingRow + height, startingCol, startingCol + width));
   }
 
   /**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
index 0bf4f1fb32..627c27262d 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
@@ -4,10 +4,10 @@
 
 package edu.wpi.first.math.controller;
 
-import edu.wpi.first.math.Discretization;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
 import edu.wpi.first.math.system.LinearSystem;
 import org.ejml.simple.SimpleMatrix;
 
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
index c968701c75..97fa5faa28 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
@@ -4,7 +4,6 @@
 
 package edu.wpi.first.math.controller;
 
-import edu.wpi.first.math.Discretization;
 import edu.wpi.first.math.Drake;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -12,6 +11,7 @@ import edu.wpi.first.math.Num;
 import edu.wpi.first.math.StateSpaceUtil;
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
 import edu.wpi.first.math.system.LinearSystem;
 import org.ejml.simple.SimpleMatrix;
 
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
index a1760a503f..16ed9503fa 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
@@ -4,13 +4,13 @@
 
 package edu.wpi.first.math.estimator;
 
-import edu.wpi.first.math.Discretization;
 import edu.wpi.first.math.Drake;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.StateSpaceUtil;
 import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
 import edu.wpi.first.math.system.NumericalIntegration;
 import edu.wpi.first.math.system.NumericalJacobian;
 import java.util.function.BiFunction;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
index be666c6285..9643fc8379 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
@@ -4,7 +4,6 @@
 
 package edu.wpi.first.math.estimator;
 
-import edu.wpi.first.math.Discretization;
 import edu.wpi.first.math.Drake;
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.Matrix;
@@ -12,6 +11,7 @@ import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.StateSpaceUtil;
 import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
 import edu.wpi.first.math.system.LinearSystem;
 
 /**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
index 6b48be10a3..4eb0b816e7 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
@@ -4,13 +4,13 @@
 
 package edu.wpi.first.math.estimator;
 
-import edu.wpi.first.math.Discretization;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.StateSpaceUtil;
 import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
 import edu.wpi.first.math.system.NumericalIntegration;
 import edu.wpi.first.math.system.NumericalJacobian;
 import java.util.function.BiFunction;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/Discretization.java b/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
similarity index 63%
rename from wpimath/src/main/java/edu/wpi/first/math/Discretization.java
rename to wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
index 33a03ae7e4..0a8eb5d23a 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Discretization.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
@@ -2,8 +2,11 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package edu.wpi.first.math;
+package edu.wpi.first.math.system;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.Pair;
 import org.ejml.simple.SimpleMatrix;
 
 @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
@@ -28,46 +31,75 @@ public final class Discretization {
   /**
    * Discretizes the given continuous A and B matrices.
    *
-   * <p>Rather than solving a (States + Inputs) x (States + Inputs) matrix exponential like in
-   * DiscretizeAB(), we take advantage of the structure of the block matrix of A and B.
-   *
-   * <p>1) The exponential of A*t, which is only N x N, is relatively cheap. 2) The upper-right
-   * quarter of the (States + Inputs) x (States + Inputs) matrix, which we can approximate using a
-   * taylor series to several terms and still be substantially cheaper than taking the big
-   * exponential.
-   *
-   * @param <States> Number of states.
-   * @param <Inputs> Number of inputs.
-   * @param states Nat representing the states of the system.
+   * @param <States> Nat representing the states of the system.
+   * @param <Inputs> Nat representing the inputs to the system.
    * @param contA Continuous system matrix.
    * @param contB Continuous input matrix.
-   * @param dtseconds Discretization timestep.
-   * @return Pair containing discretized A and B matrices.
+   * @param dtSeconds Discretization timestep.
+   * @return a Pair representing discA and diskB.
    */
+  @SuppressWarnings("LocalVariableName")
   public static <States extends Num, Inputs extends Num>
-      Pair<Matrix<States, States>, Matrix<States, Inputs>> discretizeABTaylor(
-          Nat<States> states,
-          Matrix<States, States> contA,
-          Matrix<States, Inputs> contB,
-          double dtseconds) {
-    Matrix<States, States> lastTerm = Matrix.eye(states);
-    double lastCoeff = dtseconds;
+      Pair<Matrix<States, States>, Matrix<States, Inputs>> discretizeAB(
+          Matrix<States, States> contA, Matrix<States, Inputs> contB, double dtSeconds) {
+    var scaledA = contA.times(dtSeconds);
+    var scaledB = contB.times(dtSeconds);
 
-    var phi12 = lastTerm.times(lastCoeff);
+    int states = contA.getNumRows();
+    int inputs = contB.getNumCols();
+    var M = new Matrix<>(new SimpleMatrix(states + inputs, states + inputs));
+    M.assignBlock(0, 0, scaledA);
+    M.assignBlock(0, scaledA.getNumCols(), scaledB);
+    var phi = M.exp();
 
-    // i = 6 i.e. 5th order should be enough precision
-    for (int i = 2; i < 6; ++i) {
-      lastTerm = contA.times(lastTerm);
-      lastCoeff *= dtseconds / ((double) i);
+    var discA = new Matrix<States, States>(new SimpleMatrix(states, states));
+    var discB = new Matrix<States, Inputs>(new SimpleMatrix(states, inputs));
 
-      phi12 = phi12.plus(lastTerm.times(lastCoeff));
-    }
+    discA.extractFrom(0, 0, phi);
+    discB.extractFrom(0, contB.getNumRows(), phi);
 
-    var discB = phi12.times(contB);
+    return new Pair<>(discA, discB);
+  }
 
-    var discA = discretizeA(contA, dtseconds);
+  /**
+   * Discretizes the given continuous A and Q matrices.
+   *
+   * @param contA Continuous system matrix.
+   * @param contQ Continuous process noise covariance matrix.
+   * @param dtSeconds Discretization timestep.
+   * @return a pair representing the discrete system matrix and process noise covariance matrix.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public static <States extends Num>
+      Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQ(
+          Matrix<States, States> contA, Matrix<States, States> contQ, double dtSeconds) {
+    int states = contA.getNumRows();
 
-    return Pair.of(discA, discB);
+    // Make continuous Q symmetric if it isn't already
+    Matrix<States, States> Q = contQ.plus(contQ.transpose()).div(2.0);
+
+    // Set up the matrix M = [[-A, Q], [0, A.T]]
+    final var M = new Matrix<>(new SimpleMatrix(2 * states, 2 * states));
+    M.assignBlock(0, 0, contA.times(-1.0));
+    M.assignBlock(0, states, Q);
+    M.assignBlock(states, 0, new Matrix<>(new SimpleMatrix(states, states)));
+    M.assignBlock(states, states, contA.transpose());
+
+    final var phi = M.times(dtSeconds).exp();
+
+    // Phi12 = phi[0:States,        States:2*States]
+    // Phi22 = phi[States:2*States, States:2*States]
+    final Matrix<States, States> phi12 = phi.block(states, states, 0, states);
+    final Matrix<States, States> phi22 = phi.block(states, states, states, states);
+
+    final var discA = phi22.transpose();
+
+    Q = discA.times(phi12);
+
+    // Make discrete Q symmetric if it isn't already
+    final var discQ = Q.plus(Q.transpose()).div(2.0);
+
+    return new Pair<>(discA, discQ);
   }
 
   /**
@@ -90,7 +122,8 @@ public final class Discretization {
   public static <States extends Num>
       Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQTaylor(
           Matrix<States, States> contA, Matrix<States, States> contQ, double dtSeconds) {
-    Matrix<States, States> Q = (contQ.plus(contQ.transpose())).div(2.0);
+    // Make continuous Q symmetric if it isn't already
+    Matrix<States, States> Q = contQ.plus(contQ.transpose()).div(2.0);
 
     Matrix<States, States> lastTerm = Q.copy();
     double lastCoeff = dtSeconds;
@@ -130,38 +163,4 @@ public final class Discretization {
   public static <O extends Num> Matrix<O, O> discretizeR(Matrix<O, O> R, double dtSeconds) {
     return R.div(dtSeconds);
   }
-
-  /**
-   * Discretizes the given continuous A and B matrices.
-   *
-   * @param <States> Nat representing the states of the system.
-   * @param <Inputs> Nat representing the inputs to the system.
-   * @param contA Continuous system matrix.
-   * @param contB Continuous input matrix.
-   * @param dtSeconds Discretization timestep.
-   * @return a Pair representing discA and diskB.
-   */
-  @SuppressWarnings("LocalVariableName")
-  public static <States extends Num, Inputs extends Num>
-      Pair<Matrix<States, States>, Matrix<States, Inputs>> discretizeAB(
-          Matrix<States, States> contA, Matrix<States, Inputs> contB, double dtSeconds) {
-    var scaledA = contA.times(dtSeconds);
-    var scaledB = contB.times(dtSeconds);
-
-    var contSize = contB.getNumRows() + contB.getNumCols();
-    var Mcont = new Matrix<>(new SimpleMatrix(contSize, contSize));
-    Mcont.assignBlock(0, 0, scaledA);
-    Mcont.assignBlock(0, scaledA.getNumCols(), scaledB);
-    var Mdisc = Mcont.exp();
-
-    var discA =
-        new Matrix<States, States>(new SimpleMatrix(contB.getNumRows(), contB.getNumRows()));
-    var discB =
-        new Matrix<States, Inputs>(new SimpleMatrix(contB.getNumRows(), contB.getNumCols()));
-
-    discA.extractFrom(0, 0, Mdisc);
-    discB.extractFrom(0, contB.getNumRows(), Mdisc);
-
-    return new Pair<>(discA, discB);
-  }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
index 6089d1cb3e..b7b191a9bd 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
@@ -4,7 +4,6 @@
 
 package edu.wpi.first.math.system;
 
-import edu.wpi.first.math.Discretization;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.numbers.N1;
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 5035634594..274b10af54 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
@@ -26,12 +26,13 @@ public final class NumericalIntegration {
    */
   @SuppressWarnings("ParameterName")
   public static double rk4(DoubleFunction<Double> f, double x, double dtSeconds) {
-    final var halfDt = 0.5 * dtSeconds;
+    final var h = 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);
+    final var k2 = f.apply(x + h * k1 * 0.5);
+    final var k3 = f.apply(x + h * k2 * 0.5);
+    final var k4 = f.apply(x + h * k3);
+
+    return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
   }
 
   /**
@@ -46,12 +47,14 @@ public final class NumericalIntegration {
   @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 h = 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);
+    final var k2 = f.apply(x + h * k1 * 0.5, u);
+    final var k3 = f.apply(x + h * k2 * 0.5, u);
+    final var k4 = f.apply(x + h * k3, u);
+
+    return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
   }
 
   /**
@@ -71,12 +74,14 @@ public final class NumericalIntegration {
       Matrix<States, N1> x,
       Matrix<Inputs, N1> u,
       double dtSeconds) {
-    final var halfDt = 0.5 * dtSeconds;
+    final var h = 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));
+    Matrix<States, N1> k2 = f.apply(x.plus(k1.times(h * 0.5)), u);
+    Matrix<States, N1> k3 = f.apply(x.plus(k2.times(h * 0.5)), u);
+    Matrix<States, N1> k4 = f.apply(x.plus(k3.times(h)), u);
+
+    return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0));
   }
 
   /**
@@ -91,12 +96,14 @@ public final class NumericalIntegration {
   @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;
+    final var h = 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));
+    Matrix<States, N1> k2 = f.apply(x.plus(k1.times(h * 0.5)));
+    Matrix<States, N1> k3 = f.apply(x.plus(k2.times(h * 0.5)));
+    Matrix<States, N1> k4 = f.apply(x.plus(k3.times(h)));
+
+    return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0));
   }
 
   /**
@@ -145,13 +152,8 @@ public final class NumericalIntegration {
     // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
     // for the Butcher tableau the following arrays came from.
 
-    // This is used for time-varying integration
-    // // final double[5]
-    // final double[] A = {
-    //     1.0 / 4.0, 3.0 / 8.0, 12.0 / 13.0, 1.0, 1.0 / 2.0};
-
     // final double[5][5]
-    final double[][] B = {
+    final double[][] A = {
       {1.0 / 4.0},
       {3.0 / 32.0, 9.0 / 32.0},
       {1932.0 / 2197.0, -7200.0 / 2197.0, 7296.0 / 2197.0},
@@ -160,12 +162,12 @@ public final class NumericalIntegration {
     };
 
     // final double[6]
-    final double[] C1 = {
+    final double[] b1 = {
       16.0 / 135.0, 0.0, 6656.0 / 12825.0, 28561.0 / 56430.0, -9.0 / 50.0, 2.0 / 55.0
     };
 
     // final double[6]
-    final double[] C2 = {25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0};
+    final double[] b2 = {25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0};
 
     Matrix<States, N1> newX;
     double truncationError;
@@ -181,47 +183,53 @@ public final class NumericalIntegration {
 
         // Notice how the derivative in the Wikipedia notation is dy/dx.
         // That means their y is our x and their x is our t
-        var k1 = f.apply(x, u).times(h);
-        var k2 = f.apply(x.plus(k1.times(B[0][0])), u).times(h);
-        var k3 = f.apply(x.plus(k1.times(B[1][0])).plus(k2.times(B[1][1])), u).times(h);
+        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(B[2][0])).plus(k2.times(B[2][1])).plus(k3.times(B[2][2])), u)
-                .times(h);
+            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(B[3][0]))
-                        .plus(k2.times(B[3][1]))
-                        .plus(k3.times(B[3][2]))
-                        .plus(k4.times(B[3][3])),
-                    u)
-                .times(h);
+                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(B[4][0]))
-                        .plus(k2.times(B[4][1]))
-                        .plus(k3.times(B[4][2]))
-                        .plus(k4.times(B[4][3]))
-                        .plus(k5.times(B[4][4])),
-                    u)
-                .times(h);
+                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);
 
         newX =
-            x.plus(k1.times(C1[0]))
-                .plus(k2.times(C1[1]))
-                .plus(k3.times(C1[2]))
-                .plus(k4.times(C1[3]))
-                .plus(k5.times(C1[4]))
-                .plus(k6.times(C1[5]));
+            x.plus(
+                k1.times(b1[0])
+                    .plus(k2.times(b1[1]))
+                    .plus(k3.times(b1[2]))
+                    .plus(k4.times(b1[3]))
+                    .plus(k5.times(b1[4]))
+                    .plus(k6.times(b1[5]))
+                    .times(h));
         truncationError =
-            (k1.times(C1[0] - C2[0])
-                    .plus(k2.times(C1[1] - C2[1]))
-                    .plus(k3.times(C1[2] - C2[2]))
-                    .plus(k4.times(C1[3] - C2[3]))
-                    .plus(k5.times(C1[4] - C2[4]))
-                    .plus(k6.times(C1[5] - C2[5])))
+            (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]))
+                    .times(h))
                 .normF();
 
-        h = 0.9 * h * Math.pow(maxError / truncationError, 1.0 / 5.0);
+        h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
       } while (truncationError > maxError);
 
       dtElapsed += h;
@@ -274,13 +282,8 @@ public final class NumericalIntegration {
     // See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the
     // Butcher tableau the following arrays came from.
 
-    // This is used for time-varying integration
-    // // final double[6]
-    // final double[] A = {
-    //     1.0 / 5.0, 3.0 / 10.0, 4.0 / 5.0, 8.0 / 9.0, 1.0, 1.0};
-
     // final double[6][6]
-    final double[][] B = {
+    final double[][] A = {
       {1.0 / 5.0},
       {3.0 / 40.0, 9.0 / 40.0},
       {44.0 / 45.0, -56.0 / 15.0, 32.0 / 9.0},
@@ -290,12 +293,12 @@ public final class NumericalIntegration {
     };
 
     // final double[7]
-    final double[] C1 = {
+    final double[] b1 = {
       35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0, 0.0
     };
 
     // final double[7]
-    final double[] C2 = {
+    final double[] b2 = {
       5179.0 / 57600.0,
       0.0,
       7571.0 / 16695.0,
@@ -317,52 +320,58 @@ public final class NumericalIntegration {
         // Only allow us to advance up to the dt remaining
         h = Math.min(h, dtSeconds - dtElapsed);
 
-        var k1 = f.apply(x, u).times(h);
-        var k2 = f.apply(x.plus(k1.times(B[0][0])), u).times(h);
-        var k3 = f.apply(x.plus(k1.times(B[1][0])).plus(k2.times(B[1][1])), u).times(h);
+        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(B[2][0])).plus(k2.times(B[2][1])).plus(k3.times(B[2][2])), u)
-                .times(h);
+            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(B[3][0]))
-                        .plus(k2.times(B[3][1]))
-                        .plus(k3.times(B[3][2]))
-                        .plus(k4.times(B[3][3])),
-                    u)
-                .times(h);
+                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(B[4][0]))
-                        .plus(k2.times(B[4][1]))
-                        .plus(k3.times(B[4][2]))
-                        .plus(k4.times(B[4][3]))
-                        .plus(k5.times(B[4][4])),
-                    u)
-                .times(h);
+                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 B and the array C1 have the same coefficients
+        // 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(B[5][0]))
-                .plus(k2.times(B[5][1]))
-                .plus(k3.times(B[5][2]))
-                .plus(k4.times(B[5][3]))
-                .plus(k5.times(B[5][4]))
-                .plus(k6.times(B[5][5]));
-        var k7 = f.apply(newX, u).times(h);
+            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(C1[0] - C2[0])
-                    .plus(k2.times(C1[1] - C2[1]))
-                    .plus(k3.times(C1[2] - C2[2]))
-                    .plus(k4.times(C1[3] - C2[3]))
-                    .plus(k5.times(C1[4] - C2[4]))
-                    .plus(k6.times(C1[5] - C2[5]))
-                    .plus(k7.times(C1[6] - C2[6])))
+            (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();
 
-        h = 0.9 * h * Math.pow(maxError / truncationError, 1.0 / 5.0);
+        h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
       } while (truncationError > maxError);
 
       dtElapsed += h;
diff --git a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
index c731e464fe..0017be221e 100644
--- a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
+++ b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
@@ -23,12 +23,14 @@ namespace frc {
  */
 template <typename F, typename T>
 T RK4(F&& f, T x, units::second_t dt) {
-  const auto halfDt = 0.5 * dt;
+  const auto h = dt.to<double>();
+
   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);
+  T k2 = f(x + h * 0.5 * k1);
+  T k3 = f(x + h * 0.5 * k2);
+  T k4 = f(x + h * k3);
+
+  return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
 }
 
 /**
@@ -41,31 +43,14 @@ T RK4(F&& f, T x, units::second_t dt) {
  */
 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;
+  const auto h = dt.to<double>();
+
   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);
-}
+  T k2 = f(x + h * 0.5 * k1, u);
+  T k3 = f(x + h * 0.5 * k2, u);
+  T k4 = f(x + h * k3, u);
 
-/**
- * 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);
+  return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
 }
 
 /**
@@ -87,12 +72,8 @@ T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
   // for the Butcher tableau the following arrays came from.
   constexpr int kDim = 6;
 
-  // This is used for time-varying integration
-  // constexpr std::array<double, kDim - 1> A{
-  //     1.0 / 4.0, 3.0 / 8.0, 12.0 / 13.0, 1.0, 1.0 / 2.0};
-
   // clang-format off
-  constexpr double B[kDim - 1][kDim - 1]{
+  constexpr double A[kDim - 1][kDim - 1]{
       {     1.0 / 4.0},
       {     3.0 / 32.0,       9.0 / 32.0},
       {1932.0 / 2197.0, -7200.0 / 2197.0,  7296.0 / 2197.0},
@@ -100,10 +81,10 @@ T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
       {    -8.0 / 27.0,              2.0, -3544.0 / 2565.0, 1859.0 / 4104.0, -11.0 / 40.0}};
   // clang-format on
 
-  constexpr std::array<double, kDim> C1{16.0 / 135.0,     0.0,
+  constexpr std::array<double, kDim> b1{16.0 / 135.0,     0.0,
                                         6656.0 / 12825.0, 28561.0 / 56430.0,
                                         -9.0 / 50.0,      2.0 / 55.0};
-  constexpr std::array<double, kDim> C2{
+  constexpr std::array<double, kDim> b2{
       25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0};
 
   T newX;
@@ -121,22 +102,22 @@ T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
       // Notice how the derivative in the Wikipedia notation is dy/dx.
       // That means their y is our x and their x is our t
       // clang-format off
-      T k1 = f(x, u) * h;
-      T k2 = f(x + k1 * B[0][0], u) * h;
-      T k3 = f(x + k1 * B[1][0] + k2 * B[1][1], u) * h;
-      T k4 = f(x + k1 * B[2][0] + k2 * B[2][1] + k3 * B[2][2], u) * h;
-      T k5 = f(x + k1 * B[3][0] + k2 * B[3][1] + k3 * B[3][2] + k4 * B[3][3], u) * h;
-      T k6 = f(x + k1 * B[4][0] + k2 * B[4][1] + k3 * B[4][2] + k4 * B[4][3] + k5 * B[4][4], u) * h;
+      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
 
-      newX = x + k1 * C1[0] + k2 * C1[1] + k3 * C1[2] + k4 * C1[3] +
-             k5 * C1[4] + k6 * C1[5];
-      truncationError =
-          (k1 * (C1[0] - C2[0]) + k2 * (C1[1] - C2[1]) + k3 * (C1[2] - C2[2]) +
-           k4 * (C1[3] - C2[3]) + k5 * (C1[4] - C2[4]) + k6 * (C1[5] - C2[5]))
-              .norm();
+      newX = x + h * (b1[0] * k1 + b1[1] * k2 + b1[2] * k3 + b1[3] * k4 +
+                      b1[4] * k5 + b1[5] * k6);
+      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))
+                            .norm();
 
-      h = 0.9 * h * std::pow(maxError / truncationError, 1.0 / 5.0);
+      h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
     } while (truncationError > maxError);
 
     dtElapsed += h;
@@ -164,12 +145,8 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
 
   constexpr int kDim = 7;
 
-  // This is used for time-varying integration
-  // constexpr std::array<double, kDim - 1> A{
-  //     1.0 / 5.0, 3.0 / 10.0, 4.0 / 5.0, 8.0 / 9.0, 1.0, 1.0};
-
   // clang-format off
-  constexpr double B[kDim - 1][kDim - 1]{
+  constexpr double A[kDim - 1][kDim - 1]{
       {      1.0 / 5.0},
       {      3.0 / 40.0,        9.0 / 40.0},
       {     44.0 / 45.0,      -56.0 / 15.0,       32.0 / 9.0},
@@ -178,10 +155,10 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
       {    35.0 / 384.0,               0.0,   500.0 / 1113.0,  125.0 / 192.0,  -2187.0 / 6784.0, 11.0 / 84.0}};
   // clang-format on
 
-  constexpr std::array<double, kDim> C1{
+  constexpr std::array<double, kDim> b1{
       35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0,
       11.0 / 84.0,  0.0};
-  constexpr std::array<double, kDim> C2{5179.0 / 57600.0,    0.0,
+  constexpr std::array<double, kDim> b2{5179.0 / 57600.0,    0.0,
                                         7571.0 / 16695.0,    393.0 / 640.0,
                                         -92097.0 / 339200.0, 187.0 / 2100.0,
                                         1.0 / 40.0};
@@ -199,27 +176,27 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
       h = std::min(h, dt.to<double>() - dtElapsed);
 
       // clang-format off
-      T k1 = f(x, u) * h;
-      T k2 = f(x + k1 * B[0][0], u) * h;
-      T k3 = f(x + k1 * B[1][0] + k2 * B[1][1], u) * h;
-      T k4 = f(x + k1 * B[2][0] + k2 * B[2][1] + k3 * B[2][2], u) * h;
-      T k5 = f(x + k1 * B[3][0] + k2 * B[3][1] + k3 * B[3][2] + k4 * B[3][3], u) * h;
-      T k6 = f(x + k1 * B[4][0] + k2 * B[4][1] + k3 * B[4][2] + k4 * B[4][3] + k5 * B[4][4], u) * h;
+      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 B and the array C1 have the same coefficients
+      // 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 + k1 * B[5][0] + k2 * B[5][1] + k3 * B[5][2] + k4 * B[5][3] +
-             k5 * B[5][4] + k6 * B[5][5];
-      T k7 = f(newX, u) * h;
+      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 =
-          (k1 * (C1[0] - C2[0]) + k2 * (C1[1] - C2[1]) + k3 * (C1[2] - C2[2]) +
-           k4 * (C1[3] - C2[3]) + k5 * (C1[4] - C2[4]) + k6 * (C1[5] - C2[5]) +
-           k7 * (C1[6] - C2[6]))
-              .norm();
+      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 * h * std::pow(maxError / truncationError, 1.0 / 5.0);
+      h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
     } while (truncationError > maxError);
 
     dtElapsed += h;
diff --git a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
index 8ec37b6c2e..cf02636a83 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
@@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.Discretization;
 import java.util.ArrayList;
 import java.util.List;
 import org.ejml.dense.row.MatrixFeatures_DDRM;
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
index 00a6565c4a..586d5c48b1 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
@@ -8,7 +8,6 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
-import edu.wpi.first.math.Discretization;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.StateSpaceUtil;
@@ -19,6 +18,7 @@ import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.numbers.N4;
 import edu.wpi.first.math.numbers.N6;
+import edu.wpi.first.math.system.Discretization;
 import edu.wpi.first.math.system.NumericalIntegration;
 import edu.wpi.first.math.system.NumericalJacobian;
 import edu.wpi.first.math.system.plant.DCMotor;
diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
new file mode 100644
index 0000000000..54f9cd6347
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
@@ -0,0 +1,215 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N2;
+import org.junit.jupiter.api.Test;
+
+public class DiscretizationTest {
+  // Check that for a simple second-order system that we can easily analyze
+  // analytically,
+  @Test
+  public void testDiscretizeA() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
+    final var x0 = VecBuilder.fill(1, 1);
+
+    final var discA = Discretization.discretizeA(contA, 1.0);
+    final var x1Discrete = discA.times(x0);
+
+    // We now have pos = vel = 1 and accel = 0, which should give us:
+    final var x1Truth =
+        VecBuilder.fill(
+            1.0 * x0.get(0, 0) + 1.0 * x0.get(1, 0), 0.0 * x0.get(0, 0) + 1.0 * x0.get(1, 0));
+
+    assertEquals(x1Truth, x1Discrete);
+  }
+
+  // Check that for a simple second-order system that we can easily analyze
+  // analytically,
+  @Test
+  public void testDiscretizeAB() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
+    final var contB = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0, 1);
+
+    final var x0 = VecBuilder.fill(1, 1);
+    final var u = VecBuilder.fill(1);
+
+    var discABPair = Discretization.discretizeAB(contA, contB, 1.0);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    var x1Discrete = discA.times(x0).plus(discB.times(u));
+
+    // We now have pos = vel = accel = 1, which should give us:
+    final var x1Truth =
+        VecBuilder.fill(
+            1.0 * x0.get(0, 0) + 1.0 * x0.get(1, 0) + 0.5 * u.get(0, 0),
+            0.0 * x0.get(0, 0) + 1.0 * x0.get(1, 0) + 1.0 * u.get(0, 0));
+
+    assertEquals(x1Truth, x1Discrete);
+  }
+
+  // Test that the discrete approximation of Q is roughly equal to
+  // integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
+  @Test
+  public void testDiscretizeSlowModelAQ() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
+    final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+
+    final double dt = 1.0;
+
+    final var discQIntegrated =
+        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
+            (Double t, Matrix<N2, N2> x) ->
+                contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
+            0.0,
+            new Matrix<>(Nat.N2(), Nat.N2()),
+            dt);
+
+    var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
+    var discQ = discAQPair.getSecond();
+
+    assertTrue(
+        discQIntegrated.minus(discQ).normF() < 1e-10,
+        "Expected these to be nearly equal:\ndiscQ:\n"
+            + discQ
+            + "\ndiscQIntegrated:\n"
+            + discQIntegrated);
+  }
+
+  // Test that the discrete approximation of Q is roughly equal to
+  // integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
+  @Test
+  public void testDiscretizeFastModelAQ() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1406.29);
+    final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1);
+
+    final var dt = 0.005;
+
+    final var discQIntegrated =
+        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
+            (Double t, Matrix<N2, N2> x) ->
+                contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
+            0.0,
+            new Matrix<>(Nat.N2(), Nat.N2()),
+            dt);
+
+    var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
+    var discQ = discAQPair.getSecond();
+
+    assertTrue(
+        discQIntegrated.minus(discQ).normF() < 1e-3,
+        "Expected these to be nearly equal:\ndiscQ:\n"
+            + discQ
+            + "\ndiscQIntegrated:\n"
+            + discQIntegrated);
+  }
+
+  // Test that the Taylor series discretization produces nearly identical results.
+  @Test
+  public void testDiscretizeSlowModelAQTaylor() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
+    final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+
+    final var dt = 1.0;
+
+    // Continuous Q should be positive semidefinite
+    final var esCont = contQ.getStorage().eig();
+    for (int i = 0; i < contQ.getNumRows(); ++i) {
+      assertTrue(esCont.getEigenvalue(i).real >= 0);
+    }
+
+    final var discQIntegrated =
+        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
+            (Double t, Matrix<N2, N2> x) ->
+                contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
+            0.0,
+            new Matrix<>(Nat.N2(), Nat.N2()),
+            dt);
+
+    var discA = Discretization.discretizeA(contA, dt);
+
+    var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
+    var discATaylor = discAQPair.getFirst();
+    var discQTaylor = discAQPair.getSecond();
+
+    assertTrue(
+        discQIntegrated.minus(discQTaylor).normF() < 1e-10,
+        "Expected these to be nearly equal:\ndiscQTaylor:\n"
+            + discQTaylor
+            + "\ndiscQIntegrated:\n"
+            + discQIntegrated);
+    assertTrue(discA.minus(discATaylor).normF() < 1e-10);
+
+    // Discrete Q should be positive semidefinite
+    final var esDisc = discQTaylor.getStorage().eig();
+    for (int i = 0; i < discQTaylor.getNumRows(); ++i) {
+      assertTrue(esDisc.getEigenvalue(i).real >= 0);
+    }
+  }
+
+  // Test that the Taylor series discretization produces nearly identical results.
+  @Test
+  public void testDiscretizeFastModelAQTaylor() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1500);
+    final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1);
+
+    final var dt = 0.005;
+
+    // Continuous Q should be positive semidefinite
+    final var esCont = contQ.getStorage().eig();
+    for (int i = 0; i < contQ.getNumRows(); ++i) {
+      assertTrue(esCont.getEigenvalue(i).real >= 0);
+    }
+
+    final var discQIntegrated =
+        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
+            (Double t, Matrix<N2, N2> x) ->
+                contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
+            0.0,
+            new Matrix<>(Nat.N2(), Nat.N2()),
+            dt);
+
+    var discA = Discretization.discretizeA(contA, dt);
+
+    var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
+    var discATaylor = discAQPair.getFirst();
+    var discQTaylor = discAQPair.getSecond();
+
+    assertTrue(
+        discQIntegrated.minus(discQTaylor).normF() < 1e-3,
+        "Expected these to be nearly equal:\ndiscQTaylor:\n"
+            + discQTaylor
+            + "\ndiscQIntegrated:\n"
+            + discQIntegrated);
+    assertTrue(discA.minus(discATaylor).normF() < 1e-10);
+
+    // Discrete Q should be positive semidefinite
+    final var esDisc = discQTaylor.getStorage().eig();
+    for (int i = 0; i < discQTaylor.getNumRows(); ++i) {
+      assertTrue(esDisc.getEigenvalue(i).real >= 0);
+    }
+  }
+
+  // Test that DiscretizeR() works
+  @Test
+  public void testDiscretizeR() {
+    var contR = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 0.0, 0.0, 1.0);
+    var discRTruth = Matrix.mat(Nat.N2(), Nat.N2()).fill(4.0, 0.0, 0.0, 2.0);
+
+    var discR = Discretization.discretizeR(contR, 0.5);
+
+    assertTrue(
+        discRTruth.minus(discR).normF() < 1e-10,
+        "Expected these to be nearly equal:\ndiscR:\n" + discR + "\ndiscRTruth:\n" + discRTruth);
+  }
+}
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 93cedd443e..b3fe7e6479 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
@@ -14,11 +14,9 @@ import org.junit.jupiter.api.Test;
 
 public class NumericalIntegrationTest {
   @Test
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
   public void testExponential() {
     Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
 
-    //noinspection SuspiciousNameCombination
     var y1 =
         NumericalIntegration.rk4(
             (Matrix<N1, N1> x) -> {
@@ -33,11 +31,9 @@ public class NumericalIntegrationTest {
   }
 
   @Test
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
   public void testExponentialRKF45() {
     Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
 
-    //noinspection SuspiciousNameCombination
     var y1 =
         NumericalIntegration.rkf45(
             (x, u) -> {
@@ -53,11 +49,9 @@ public class NumericalIntegrationTest {
   }
 
   @Test
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
   public void testExponentialRKDP() {
     Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
 
-    //noinspection SuspiciousNameCombination
     var y1 =
         NumericalIntegration.rkdp(
             (x, u) -> {
diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java
new file mode 100644
index 0000000000..7b5e844401
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import java.util.function.BiFunction;
+
+public final class RungeKuttaTimeVarying {
+  private RungeKuttaTimeVarying() {
+    // Utility class
+  }
+
+  /**
+   * Performs 4th order Runge-Kutta integration of dx/dt = f(t, y) for dt.
+   *
+   * @param <Rows> Rows in y.
+   * @param <Cols> Columns in y.
+   * @param f The function to integrate. It must take two arguments t and y.
+   * @param t The initial value of t.
+   * @param y The initial value of y.
+   * @param dtSeconds The time over which to integrate.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> rungeKuttaTimeVarying(
+      BiFunction<Double, Matrix<Rows, Cols>, Matrix<Rows, Cols>> f,
+      double t,
+      Matrix<Rows, Cols> y,
+      double dtSeconds) {
+    final var h = dtSeconds;
+
+    Matrix<Rows, Cols> k1 = f.apply(t, y);
+    Matrix<Rows, Cols> k2 = f.apply(t + dtSeconds * 0.5, y.plus(k1.times(h * 0.5)));
+    Matrix<Rows, Cols> k3 = f.apply(t + dtSeconds * 0.5, y.plus(k2.times(h * 0.5)));
+    Matrix<Rows, Cols> k4 = f.apply(t + dtSeconds, y.plus(k3.times(h)));
+
+    return y.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0));
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
new file mode 100644
index 0000000000..ee843ab4fc
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.N1;
+import org.junit.jupiter.api.Test;
+
+public class RungeKuttaTimeVaryingTest {
+  private static Matrix<N1, N1> rungeKuttaTimeVaryingSolution(double t) {
+    return new MatBuilder<>(Nat.N1(), Nat.N1())
+        .fill(12.0 * Math.exp(t) / (Math.pow(Math.exp(t) + 1.0, 2.0)));
+  }
+
+  // Tests RK4 with a time varying solution. From
+  // http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
+  //   x' = x (2 / (e^t + 1) - 1)
+  //
+  // The true (analytical) solution is:
+  //
+  // x(t) = 12 * e^t / ((e^t + 1)^2)
+  @Test
+  public void rungeKuttaTimeVaryingTest() {
+    final var y0 = rungeKuttaTimeVaryingSolution(5.0);
+
+    final var y1 =
+        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
+            (Double t, Matrix<N1, N1> x) -> {
+              return new MatBuilder<>(Nat.N1(), Nat.N1())
+                  .fill(x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0));
+            },
+            5.0,
+            y0,
+            1.0);
+    assertEquals(rungeKuttaTimeVaryingSolution(6.0).get(0, 0), y1.get(0, 0), 1e-3);
+  }
+}
diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
index 4269f0afc4..70e4e4b1e3 100644
--- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
@@ -10,6 +10,7 @@
 #include "Eigen/Eigenvalues"
 #include "frc/system/Discretization.h"
 #include "frc/system/NumericalIntegration.h"
+#include "frc/system/RungeKuttaTimeVarying.h"
 
 // Check that for a simple second-order system that we can easily analyze
 // analytically,
@@ -26,8 +27,8 @@ TEST(DiscretizationTest, DiscretizeA) {
 
   // We now have pos = vel = 1 and accel = 0, which should give us:
   Eigen::Matrix<double, 2, 1> x1Truth;
-  x1Truth(1) = x0(1);
-  x1Truth(0) = x0(0) + 1.0 * x0(1);
+  x1Truth(0) = 1.0 * x0(0) + 1.0 * x0(1);
+  x1Truth(1) = 0.0 * x0(0) + 1.0 * x0(1);
 
   EXPECT_EQ(x1Truth, x1Discrete);
 }
@@ -53,8 +54,8 @@ TEST(DiscretizationTest, DiscretizeAB) {
 
   // We now have pos = vel = accel = 1, which should give us:
   Eigen::Matrix<double, 2, 1> x1Truth;
-  x1Truth(1) = x0(1) + 1.0 * u(0);
-  x1Truth(0) = x0(0) + 1.0 * x0(1) + 0.5 * u(0);
+  x1Truth(0) = 1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0);
+  x1Truth(1) = 0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0);
 
   EXPECT_EQ(x1Truth, x1Discrete);
 }
@@ -79,7 +80,7 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
             (contA * t.to<double>()).exp() * contQ *
             (contA.transpose() * t.to<double>()).exp());
       },
-      Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
+      0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
 
   Eigen::Matrix<double, 2, 2> discA;
   Eigen::Matrix<double, 2, 2> discQ;
@@ -100,7 +101,7 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
   Eigen::Matrix<double, 2, 2> contQ;
   contQ << 0.0025, 0, 0, 1;
 
-  constexpr auto dt = 5.05_ms;
+  constexpr auto dt = 5_ms;
 
   Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
       std::function<Eigen::Matrix<double, 2, 2>(
@@ -111,7 +112,7 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
             (contA * t.to<double>()).exp() * contQ *
             (contA.transpose() * t.to<double>()).exp());
       },
-      Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
+      0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
 
   Eigen::Matrix<double, 2, 2> discA;
   Eigen::Matrix<double, 2, 2> discQ;
@@ -128,9 +129,6 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
   Eigen::Matrix<double, 2, 2> contA;
   contA << 0, 1, 0, 0;
 
-  Eigen::Matrix<double, 2, 1> contB;
-  contB << 0, 1;
-
   Eigen::Matrix<double, 2, 2> contQ;
   contQ << 1, 0, 0, 1;
 
@@ -139,12 +137,11 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
   Eigen::Matrix<double, 2, 2> discQTaylor;
   Eigen::Matrix<double, 2, 2> discA;
   Eigen::Matrix<double, 2, 2> discATaylor;
-  Eigen::Matrix<double, 2, 1> discB;
 
   // Continuous Q should be positive semidefinite
   Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
-  for (int i = 0; i < contQ.rows(); i++) {
-    EXPECT_GT(esCont.eigenvalues()[i], 0);
+  for (int i = 0; i < contQ.rows(); ++i) {
+    EXPECT_GE(esCont.eigenvalues()[i], 0);
   }
 
   Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
@@ -156,9 +153,9 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
             (contA * t.to<double>()).exp() * contQ *
             (contA.transpose() * t.to<double>()).exp());
       },
-      Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
+      0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
 
-  frc::DiscretizeAB<2, 1>(contA, contB, dt, &discA, &discB);
+  frc::DiscretizeA<2>(contA, dt, &discA);
   frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
 
   EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-10)
@@ -169,8 +166,8 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
 
   // Discrete Q should be positive semidefinite
   Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
-  for (int i = 0; i < discQTaylor.rows(); i++) {
-    EXPECT_GT(esDisc.eigenvalues()[i], 0);
+  for (int i = 0; i < discQTaylor.rows(); ++i) {
+    EXPECT_GE(esDisc.eigenvalues()[i], 0);
   }
 }
 
@@ -179,23 +176,19 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
   Eigen::Matrix<double, 2, 2> contA;
   contA << 0, 1, 0, -1500;
 
-  Eigen::Matrix<double, 2, 1> contB;
-  contB << 0, 1;
-
   Eigen::Matrix<double, 2, 2> contQ;
   contQ << 0.0025, 0, 0, 1;
 
-  constexpr auto dt = 5.05_ms;
+  constexpr auto dt = 5_ms;
 
   Eigen::Matrix<double, 2, 2> discQTaylor;
   Eigen::Matrix<double, 2, 2> discA;
   Eigen::Matrix<double, 2, 2> discATaylor;
-  Eigen::Matrix<double, 2, 1> discB;
 
   // Continuous Q should be positive semidefinite
   Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
-  for (int i = 0; i < contQ.rows(); i++) {
-    EXPECT_GT(esCont.eigenvalues()[i], 0);
+  for (int i = 0; i < contQ.rows(); ++i) {
+    EXPECT_GE(esCont.eigenvalues()[i], 0);
   }
 
   Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
@@ -207,9 +200,9 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
             (contA * t.to<double>()).exp() * contQ *
             (contA.transpose() * t.to<double>()).exp());
       },
-      Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
+      0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
 
-  frc::DiscretizeAB<2, 1>(contA, contB, dt, &discA, &discB);
+  frc::DiscretizeA<2>(contA, dt, &discA);
   frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
 
   EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-3)
@@ -220,8 +213,8 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
 
   // Discrete Q should be positive semidefinite
   Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
-  for (int i = 0; i < discQTaylor.rows(); i++) {
-    EXPECT_GT(esDisc.eigenvalues()[i], 0);
+  for (int i = 0; i < discQTaylor.rows(); ++i) {
+    EXPECT_GE(esDisc.eigenvalues()[i], 0);
   }
 }
 
diff --git a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
index 208ab5d274..b644ddc25f 100644
--- a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
@@ -67,32 +67,3 @@ TEST(NumericalIntegrationTest, ExponentialRKDP) {
       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>()
-          << 12.0 * std::exp(t) / (std::pow(std::exp(t) + 1.0, 2.0)))
-      .finished();
-}
-}  // namespace
-
-// Tests RungeKutta with a time varying solution.
-// Now, lets test RK4 with a time varying solution.  From
-// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
-//   x' = x (2 / (e^t + 1) - 1)
-//
-// The true (analytical) solution is:
-//
-// x(t) = 12 * e^t / ((e^t + 1)^2)
-TEST(NumericalIntegrationTest, RungeKuttaTimeVarying) {
-  Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
-
-  Eigen::Matrix<double, 1, 1> y1 = frc::RungeKuttaTimeVarying(
-      [](units::second_t t, Eigen::Matrix<double, 1, 1> x) {
-        return (Eigen::Matrix<double, 1, 1>()
-                << x(0) * (2.0 / (std::exp(t.to<double>()) + 1.0) - 1.0))
-            .finished();
-      },
-      y0, 5_s, 1_s);
-  EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);
-}
diff --git a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
new file mode 100644
index 0000000000..334712901c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
@@ -0,0 +1,37 @@
+// 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.
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include "frc/system/RungeKuttaTimeVarying.h"
+
+namespace {
+Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
+  return (Eigen::Matrix<double, 1, 1>()
+          << 12.0 * std::exp(t) / (std::pow(std::exp(t) + 1.0, 2.0)))
+      .finished();
+}
+}  // namespace
+
+// Tests RK4 with a time varying solution. From
+// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
+//   x' = x (2 / (e^t + 1) - 1)
+//
+// The true (analytical) solution is:
+//
+// x(t) = 12 * e^t / ((e^t + 1)^2)
+TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) {
+  Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
+
+  Eigen::Matrix<double, 1, 1> y1 = frc::RungeKuttaTimeVarying(
+      [](units::second_t t, Eigen::Matrix<double, 1, 1> x) {
+        return (Eigen::Matrix<double, 1, 1>()
+                << x(0) * (2.0 / (std::exp(t.to<double>()) + 1.0) - 1.0))
+            .finished();
+      },
+      5_s, y0, 1_s);
+  EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);
+}
diff --git a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
new file mode 100644
index 0000000000..19cb9798d7
--- /dev/null
+++ b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
@@ -0,0 +1,34 @@
+// 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 <array>
+
+#include "Eigen/Core"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Performs 4th order Runge-Kutta 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.
+ */
+template <typename F, typename T>
+T RungeKuttaTimeVarying(F&& f, units::second_t t, T y, units::second_t dt) {
+  const auto h = dt.to<double>();
+
+  T k1 = f(t, y);
+  T k2 = f(t + dt * 0.5, y + h * k1 * 0.5);
+  T k3 = f(t + dt * 0.5, y + h * k2 * 0.5);
+  T k4 = f(t + dt, y + h * k3);
+
+  return y + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+}
+
+}  // namespace frc