diff --git a/wpimath/src/main/java/edu/wpi/first/math/DARE.java b/wpimath/src/main/java/edu/wpi/first/math/DARE.java
new file mode 100644
index 0000000..ad07d05
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/DARE.java
@@ -0,0 +1,239 @@
+// 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;
+
+import org.ejml.simple.SimpleMatrix;
+
+public final class DARE {
+  private DARE() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+   *
+   * <p>This internal function skips expensive precondition checks for increased performance. The
+   * solver may hang if any of the following occur:
+   *
+   * <ul>
+   *   <li>Q isn't symmetric positive semidefinite
+   *   <li>R isn't symmetric positive definite
+   *   <li>The (A, B) pair isn't stabilizable
+   *   <li>The (A, C) pair where Q = CᵀC isn't detectable
+   * </ul>
+   *
+   * <p>Only use this function if you're sure the preconditions are met.
+   *
+   * @param <States> Number of states.
+   * @param <Inputs> Number of inputs.
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @param Q State cost matrix.
+   * @param R Input cost matrix.
+   * @return Solution of DARE.
+   */
+  public static <States extends Num, Inputs extends Num> Matrix<States, States> dareDetail(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R) {
+    var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
+    WPIMathJNI.dareDetailABQR(
+        A.getStorage().getDDRM().getData(),
+        B.getStorage().getDDRM().getData(),
+        Q.getStorage().getDDRM().getData(),
+        R.getStorage().getDDRM().getData(),
+        A.getNumCols(),
+        B.getNumCols(),
+        S.getStorage().getDDRM().getData());
+    return S;
+  }
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+   *
+   * <p>This is equivalent to solving the original DARE:
+   *
+   * <p>A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+   *
+   * <p>where A₂ and Q₂ are a change of variables:
+   *
+   * <p>A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+   *
+   * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
+   * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q  N][xₖ]
+   * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
+   * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
+   *
+   * <pre>
+   *     ∞
+   * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This can be refactored as:
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q 0][xₖ]
+   * J = Σ [uₖ] [0 R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This internal function skips expensive precondition checks for increased performance. The
+   * solver may hang if any of the following occur:
+   *
+   * <ul>
+   *   <li>Q₂ isn't symmetric positive semidefinite
+   *   <li>R isn't symmetric positive definite
+   *   <li>The (A₂, B) pair isn't stabilizable
+   *   <li>The (A₂, C) pair where Q₂ = CᵀC isn't detectable
+   * </ul>
+   *
+   * <p>Only use this function if you're sure the preconditions are met.
+   *
+   * @param <States> Number of states.
+   * @param <Inputs> Number of inputs.
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @param Q State cost matrix.
+   * @param R Input cost matrix.
+   * @param N State-input cross-term cost matrix.
+   * @return Solution of DARE.
+   */
+  public static <States extends Num, Inputs extends Num> Matrix<States, States> dareDetail(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R,
+      Matrix<States, Inputs> N) {
+    var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
+    WPIMathJNI.dareDetailABQRN(
+        A.getStorage().getDDRM().getData(),
+        B.getStorage().getDDRM().getData(),
+        Q.getStorage().getDDRM().getData(),
+        R.getStorage().getDDRM().getData(),
+        N.getStorage().getDDRM().getData(),
+        A.getNumCols(),
+        B.getNumCols(),
+        S.getStorage().getDDRM().getData());
+    return S;
+  }
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+   *
+   * @param <States> Number of states.
+   * @param <Inputs> Number of inputs.
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @param Q State cost matrix.
+   * @param R Input cost matrix.
+   * @return Solution of DARE.
+   * @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
+   * @throws IllegalArgumentException if R isn't symmetric positive definite.
+   * @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
+   * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
+   */
+  public static <States extends Num, Inputs extends Num> Matrix<States, States> dare(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R) {
+    var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
+    WPIMathJNI.dareABQR(
+        A.getStorage().getDDRM().getData(),
+        B.getStorage().getDDRM().getData(),
+        Q.getStorage().getDDRM().getData(),
+        R.getStorage().getDDRM().getData(),
+        A.getNumCols(),
+        B.getNumCols(),
+        S.getStorage().getDDRM().getData());
+    return S;
+  }
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+   *
+   * <p>This is equivalent to solving the original DARE:
+   *
+   * <p>A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+   *
+   * <p>where A₂ and Q₂ are a change of variables:
+   *
+   * <p>A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+   *
+   * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
+   * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q  N][xₖ]
+   * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
+   * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
+   *
+   * <pre>
+   *     ∞
+   * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This can be refactored as:
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q 0][xₖ]
+   * J = Σ [uₖ] [0 R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * @param <States> Number of states.
+   * @param <Inputs> Number of inputs.
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @param Q State cost matrix.
+   * @param R Input cost matrix.
+   * @param N State-input cross-term cost matrix.
+   * @return Solution of DARE.
+   * @throws IllegalArgumentException if Q₂ isn't symmetric positive semidefinite.
+   * @throws IllegalArgumentException if R isn't symmetric positive definite.
+   * @throws IllegalArgumentException if the (A₂, B) pair isn't stabilizable.
+   * @throws IllegalArgumentException if the (A₂, C) pair where Q₂ = CᵀC isn't detectable.
+   */
+  public static <States extends Num, Inputs extends Num> Matrix<States, States> dare(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R,
+      Matrix<States, Inputs> N) {
+    var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
+    WPIMathJNI.dareABQRN(
+        A.getStorage().getDDRM().getData(),
+        B.getStorage().getDDRM().getData(),
+        Q.getStorage().getDDRM().getData(),
+        R.getStorage().getDDRM().getData(),
+        N.getStorage().getDDRM().getData(),
+        A.getNumCols(),
+        B.getNumCols(),
+        S.getStorage().getDDRM().getData());
+    return S;
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/Drake.java b/wpimath/src/main/java/edu/wpi/first/math/Drake.java
deleted file mode 100644
index 55bc5b2..0000000
--- a/wpimath/src/main/java/edu/wpi/first/math/Drake.java
+++ /dev/null
@@ -1,116 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package edu.wpi.first.math;
-
-import org.ejml.simple.SimpleMatrix;
-
-public final class Drake {
-  private Drake() {}
-
-  /**
-   * Solves the discrete algebraic Riccati equation.
-   *
-   * @param A System matrix.
-   * @param B Input matrix.
-   * @param Q State cost matrix.
-   * @param R Input cost matrix.
-   * @return Solution of DARE.
-   */
-  public static SimpleMatrix discreteAlgebraicRiccatiEquation(
-      SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) {
-    var S = new SimpleMatrix(A.numRows(), A.numCols());
-    WPIMathJNI.discreteAlgebraicRiccatiEquation(
-        A.getDDRM().getData(),
-        B.getDDRM().getData(),
-        Q.getDDRM().getData(),
-        R.getDDRM().getData(),
-        A.numCols(),
-        B.numCols(),
-        S.getDDRM().getData());
-    return S;
-  }
-
-  /**
-   * Solves the discrete algebraic Riccati equation.
-   *
-   * @param <States> Number of states.
-   * @param <Inputs> Number of inputs.
-   * @param A System matrix.
-   * @param B Input matrix.
-   * @param Q State cost matrix.
-   * @param R Input cost matrix.
-   * @return Solution of DARE.
-   */
-  public static <States extends Num, Inputs extends Num>
-      Matrix<States, States> discreteAlgebraicRiccatiEquation(
-          Matrix<States, States> A,
-          Matrix<States, Inputs> B,
-          Matrix<States, States> Q,
-          Matrix<Inputs, Inputs> R) {
-    return new Matrix<>(
-        discreteAlgebraicRiccatiEquation(
-            A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage()));
-  }
-
-  /**
-   * Solves the discrete algebraic Riccati equation.
-   *
-   * @param A System matrix.
-   * @param B Input matrix.
-   * @param Q State cost matrix.
-   * @param R Input cost matrix.
-   * @param N State-input cross-term cost matrix.
-   * @return Solution of DARE.
-   */
-  public static SimpleMatrix discreteAlgebraicRiccatiEquation(
-      SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R, SimpleMatrix N) {
-    // See
-    // https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
-    // for the change of variables used here.
-    var scrA = A.minus(B.mult(R.solve(N.transpose())));
-    var scrQ = Q.minus(N.mult(R.solve(N.transpose())));
-
-    var S = new SimpleMatrix(A.numRows(), A.numCols());
-    WPIMathJNI.discreteAlgebraicRiccatiEquation(
-        scrA.getDDRM().getData(),
-        B.getDDRM().getData(),
-        scrQ.getDDRM().getData(),
-        R.getDDRM().getData(),
-        A.numCols(),
-        B.numCols(),
-        S.getDDRM().getData());
-    return S;
-  }
-
-  /**
-   * Solves the discrete algebraic Riccati equation.
-   *
-   * @param <States> Number of states.
-   * @param <Inputs> Number of inputs.
-   * @param A System matrix.
-   * @param B Input matrix.
-   * @param Q State cost matrix.
-   * @param R Input cost matrix.
-   * @param N State-input cross-term cost matrix.
-   * @return Solution of DARE.
-   */
-  public static <States extends Num, Inputs extends Num>
-      Matrix<States, States> discreteAlgebraicRiccatiEquation(
-          Matrix<States, States> A,
-          Matrix<States, Inputs> B,
-          Matrix<States, States> Q,
-          Matrix<Inputs, Inputs> R,
-          Matrix<States, Inputs> N) {
-    // See
-    // https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
-    // for the change of variables used here.
-    var scrA = A.minus(B.times(R.solve(N.transpose())));
-    var scrQ = Q.minus(N.times(R.solve(N.transpose())));
-
-    return new Matrix<>(
-        discreteAlgebraicRiccatiEquation(
-            scrA.getStorage(), B.getStorage(), scrQ.getStorage(), R.getStorage()));
-  }
-}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathShared.java b/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
index 483dad3..3c2b843 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
@@ -20,4 +20,11 @@
    * @param count the usage count
    */
   void reportUsage(MathUsageId id, int count);
+
+  /**
+   * Get the current time.
+   *
+   * @return Time in seconds
+   */
+  double getTimestamp();
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java b/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
index 0dbc03d..d6e3e96 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
@@ -4,6 +4,8 @@
 
 package edu.wpi.first.math;
 
+import edu.wpi.first.util.WPIUtilJNI;
+
 public final class MathSharedStore {
   private static MathShared mathShared;
 
@@ -23,6 +25,11 @@
 
             @Override
             public void reportUsage(MathUsageId id, int count) {}
+
+            @Override
+            public double getTimestamp() {
+              return WPIUtilJNI.now() * 1.0e-6;
+            }
           };
     }
     return mathShared;
@@ -56,4 +63,13 @@
   public static void reportUsage(MathUsageId id, int count) {
     getMathShared().reportUsage(id, count);
   }
+
+  /**
+   * Get the time.
+   *
+   * @return The time in seconds.
+   */
+  public static double getTimestamp() {
+    return getMathShared().getTimestamp();
+  }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
index 95ed5bf..3785780 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
@@ -145,4 +145,67 @@
   public static double interpolate(double startValue, double endValue, double t) {
     return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1);
   }
+
+  /**
+   * Return where within interpolation range [0, 1] q is between startValue and endValue.
+   *
+   * @param startValue Lower part of interpolation range.
+   * @param endValue Upper part of interpolation range.
+   * @param q Query.
+   * @return Interpolant in range [0, 1].
+   */
+  public static double inverseInterpolate(double startValue, double endValue, double q) {
+    double totalRange = endValue - startValue;
+    if (totalRange <= 0) {
+      return 0.0;
+    }
+    double queryToStart = q - startValue;
+    if (queryToStart <= 0) {
+      return 0.0;
+    }
+    return queryToStart / totalRange;
+  }
+
+  /**
+   * Checks if the given value matches an expected value within a certain tolerance.
+   *
+   * @param expected The expected value
+   * @param actual The actual value
+   * @param tolerance The allowed difference between the actual and the expected value
+   * @return Whether or not the actual value is within the allowed tolerance
+   */
+  public static boolean isNear(double expected, double actual, double tolerance) {
+    if (tolerance < 0) {
+      throw new IllegalArgumentException("Tolerance must be a non-negative number!");
+    }
+    return Math.abs(expected - actual) < tolerance;
+  }
+
+  /**
+   * Checks if the given value matches an expected value within a certain tolerance. Supports
+   * continuous input for cases like absolute encoders.
+   *
+   * <p>Continuous input means that the min and max value are considered to be the same point, and
+   * tolerances can be checked across them. A common example would be for absolute encoders: calling
+   * isNear(2, 359, 5, 0, 360) returns true because 359 is 1 away from 360 (which is treated as the
+   * same as 0) and 2 is 2 away from 0, adding up to an error of 3 degrees, which is within the
+   * given tolerance of 5.
+   *
+   * @param expected The expected value
+   * @param actual The actual value
+   * @param tolerance The allowed difference between the actual and the expected value
+   * @param min Smallest value before wrapping around to the largest value
+   * @param max Largest value before wrapping around to the smallest value
+   * @return Whether or not the actual value is within the allowed tolerance
+   */
+  public static boolean isNear(
+      double expected, double actual, double tolerance, double min, double max) {
+    if (tolerance < 0) {
+      throw new IllegalArgumentException("Tolerance must be a non-negative number!");
+    }
+    // Max error is exactly halfway between the min and max
+    double errorBound = (max - min) / 2.0;
+    double error = MathUtil.inputModulus(expected - actual, -errorBound, errorBound);
+    return Math.abs(error) < tolerance;
+  }
 }
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 b8e7c28..b7a86fa 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
@@ -42,6 +42,18 @@
    * Constructs a new {@link Matrix} with the given storage. Caller should make sure that the
    * provided generic bounds match the shape of the provided {@link Matrix}.
    *
+   * @param rows The number of rows of the matrix.
+   * @param columns The number of columns of the matrix.
+   * @param storage The double array to back this value.
+   */
+  public Matrix(Nat<R> rows, Nat<C> columns, double[] storage) {
+    this.m_storage = new SimpleMatrix(rows.getNum(), columns.getNum(), true, storage);
+  }
+
+  /**
+   * Constructs a new {@link Matrix} with the given storage. Caller should make sure that the
+   * provided generic bounds match the shape of the provided {@link Matrix}.
+   *
    * <p>NOTE:It is not recommend to use this constructor unless the {@link SimpleMatrix} API is
    * absolutely necessary due to the desired function not being accessible through the {@link
    * Matrix} wrapper.
@@ -80,7 +92,7 @@
    * @return The number of columns, according to the internal storage.
    */
   public final int getNumCols() {
-    return this.m_storage.numCols();
+    return this.m_storage.getNumCols();
   }
 
   /**
@@ -89,7 +101,7 @@
    * @return The number of rows, according to the internal storage.
    */
   public final int getNumRows() {
-    return this.m_storage.numRows();
+    return this.m_storage.getNumRows();
   }
 
   /**
@@ -222,7 +234,7 @@
    *
    * <p>c<sub>i,j</sub> = a<sub>i,j</sub>*other<sub>i,j</sub>
    *
-   * @param other The other {@link Matrix} to preform element multiplication on.
+   * @param other The other {@link Matrix} to perform element multiplication on.
    * @return The element by element multiplication of "this" and other.
    */
   public final Matrix<R, C> elementTimes(Matrix<R, C> other) {
@@ -582,7 +594,7 @@
     SimpleMatrix temp = m_storage.copy();
 
     CholeskyDecomposition_F64<DMatrixRMaj> chol =
-        DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
+        DecompositionFactory_DDRM.chol(temp.getNumRows(), lowerTriangular);
     if (!chol.decompose(temp.getMatrix())) {
       // check that the input is not all zeros -- if they are, we special case and return all
       // zeros.
@@ -592,7 +604,7 @@
         isZeros &= Math.abs(matDatum) < 1e-6;
       }
       if (isZeros) {
-        return new Matrix<>(new SimpleMatrix(temp.numRows(), temp.numCols()));
+        return new Matrix<>(new SimpleMatrix(temp.getNumRows(), temp.getNumCols()));
       }
 
       throw new RuntimeException(
diff --git a/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java b/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
index e1680eb..487faf4 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
@@ -68,7 +68,7 @@
 
   private static Pair<SimpleMatrix, SimpleMatrix> pade3(SimpleMatrix A) {
     double[] b = new double[] {120, 60, 12, 1};
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
 
     SimpleMatrix A2 = A.mult(A);
     SimpleMatrix U = A.mult(A2.mult(ident.scale(b[1]).plus(b[3])));
@@ -78,7 +78,7 @@
 
   private static Pair<SimpleMatrix, SimpleMatrix> pade5(SimpleMatrix A) {
     double[] b = new double[] {30240, 15120, 3360, 420, 30, 1};
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
     SimpleMatrix A2 = A.mult(A);
     SimpleMatrix A4 = A2.mult(A2);
 
@@ -90,7 +90,7 @@
 
   private static Pair<SimpleMatrix, SimpleMatrix> pade7(SimpleMatrix A) {
     double[] b = new double[] {17297280, 8648640, 1995840, 277200, 25200, 1512, 56, 1};
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
     SimpleMatrix A2 = A.mult(A);
     SimpleMatrix A4 = A2.mult(A2);
     SimpleMatrix A6 = A4.mult(A2);
@@ -108,7 +108,7 @@
         new double[] {
           17643225600.0, 8821612800.0, 2075673600, 302702400, 30270240, 2162160, 110880, 3960, 90, 1
         };
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
     SimpleMatrix A2 = A.mult(A);
     SimpleMatrix A4 = A2.mult(A2);
     SimpleMatrix A6 = A4.mult(A2);
@@ -149,7 +149,7 @@
           182,
           1
         };
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix ident = eye(A.getNumRows(), A.getNumCols());
 
     SimpleMatrix A2 = A.mult(A);
     SimpleMatrix A4 = A2.mult(A2);
@@ -213,7 +213,7 @@
     SimpleMatrix temp = src.copy();
 
     CholeskyDecomposition_F64<DMatrixRMaj> chol =
-        DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
+        DecompositionFactory_DDRM.chol(temp.getNumRows(), lowerTriangular);
     if (!chol.decompose(temp.getMatrix())) {
       // check that the input is not all zeros -- if they are, we special case and return all
       // zeros.
@@ -223,7 +223,7 @@
         isZeros &= Math.abs(matDatum) < 1e-6;
       }
       if (isZeros) {
-        return new SimpleMatrix(temp.numRows(), temp.numCols());
+        return new SimpleMatrix(temp.getNumRows(), temp.getNumCols());
       }
 
       throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n" + src.toString());
@@ -239,8 +239,8 @@
    * @return the exponential of A.
    */
   public static SimpleMatrix exp(SimpleMatrix A) {
-    SimpleMatrix toReturn = new SimpleMatrix(A.numRows(), A.numRows());
-    WPIMathJNI.exp(A.getDDRM().getData(), A.numRows(), toReturn.getDDRM().getData());
+    SimpleMatrix toReturn = new SimpleMatrix(A.getNumRows(), A.getNumRows());
+    WPIMathJNI.exp(A.getDDRM().getData(), A.getNumRows(), toReturn.getDDRM().getData());
     return toReturn;
   }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
index a041845..27ed9e0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
@@ -15,7 +15,7 @@
   private static Random rand = new Random();
 
   private StateSpaceUtil() {
-    // Utility class
+    throw new UnsupportedOperationException("This is a utility class!");
   }
 
   /**
@@ -90,7 +90,7 @@
    * Returns true if (A, B) is a stabilizable pair.
    *
    * <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
-   * absolute values less than one, where an eigenvalue is uncontrollable if rank(λI - A, B) %3C n
+   * absolute values less than one, where an eigenvalue is uncontrollable if rank([λI - A, B]) %3C n
    * where n is the number of states.
    *
    * @param <States> Num representing the size of A.
@@ -108,7 +108,7 @@
    * Returns true if (A, C) is a detectable pair.
    *
    * <p>(A, C) is detectable if and only if the unobservable eigenvalues of A, if any, have absolute
-   * values less than one, where an eigenvalue is unobservable if rank(λI - A; C) %3C n where n is
+   * values less than one, where an eigenvalue is unobservable if rank([λI - A; C]) %3C n where n is
    * the number of states.
    *
    * @param <States> Num representing the size of A.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java b/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java
index 670611a..9ba91b0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java
@@ -14,6 +14,8 @@
 import edu.wpi.first.math.numbers.N7;
 import edu.wpi.first.math.numbers.N8;
 import edu.wpi.first.math.numbers.N9;
+import java.util.Objects;
+import org.ejml.simple.SimpleMatrix;
 
 /**
  * A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices).
@@ -26,7 +28,15 @@
   }
 
   private Vector<N> fillVec(double... data) {
-    return new Vector<>(fill(data));
+    if (Objects.requireNonNull(data).length != this.m_rows.getNum()) {
+      throw new IllegalArgumentException(
+          "Invalid vector data provided. Wanted "
+              + this.m_rows.getNum()
+              + " vector, but got "
+              + data.length
+              + " elements");
+    }
+    return new Vector<>(new SimpleMatrix(this.m_rows.getNum(), 1, true, data));
   }
 
   /**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/Vector.java b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
index 1aebdde..bc46741 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Vector.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
@@ -5,6 +5,7 @@
 package edu.wpi.first.math;
 
 import edu.wpi.first.math.numbers.N1;
+import java.util.Objects;
 import org.ejml.simple.SimpleMatrix;
 
 /**
@@ -63,6 +64,26 @@
   }
 
   /**
+   * Adds the given vector to this vector.
+   *
+   * @param value The vector to add.
+   * @return The resultant vector.
+   */
+  public final Vector<R> plus(Vector<R> value) {
+    return new Vector<>(this.m_storage.plus(Objects.requireNonNull(value).m_storage));
+  }
+
+  /**
+   * Subtracts the given vector to this vector.
+   *
+   * @param value The vector to add.
+   * @return The resultant vector.
+   */
+  public final Vector<R> minus(Vector<R> value) {
+    return new Vector<>(this.m_storage.minus(Objects.requireNonNull(value).m_storage));
+  }
+
+  /**
    * Returns the dot product of this vector with another.
    *
    * @param other The other vector.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
index 40a5c63..ef47a3b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
@@ -43,8 +43,25 @@
     libraryLoaded = true;
   }
 
+  // DARE wrappers
+
   /**
-   * Solves the discrete alegebraic Riccati equation.
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+   *
+   * <p>This internal function skips expensive precondition checks for increased performance. The
+   * solver may hang if any of the following occur:
+   *
+   * <ul>
+   *   <li>Q isn't symmetric positive semidefinite
+   *   <li>R isn't symmetric positive definite
+   *   <li>The (A, B) pair isn't stabilizable
+   *   <li>The (A, C) pair where Q = CᵀC isn't detectable
+   * </ul>
+   *
+   * <p>Only use this function if you're sure the preconditions are met. Solves the discrete
+   * alegebraic Riccati equation.
    *
    * @param A Array containing elements of A in row-major order.
    * @param B Array containing elements of B in row-major order.
@@ -54,10 +71,148 @@
    * @param inputs Number of inputs in B matrix.
    * @param S Array storage for DARE solution.
    */
-  public static native void discreteAlgebraicRiccatiEquation(
+  public static native void dareDetailABQR(
       double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
 
   /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+   *
+   * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
+   * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q  N][xₖ]
+   * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
+   * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
+   *
+   * <pre>
+   *     ∞
+   * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This can be refactored as:
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q 0][xₖ]
+   * J = Σ [uₖ] [0 R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This internal function skips expensive precondition checks for increased performance. The
+   * solver may hang if any of the following occur:
+   *
+   * <ul>
+   *   <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
+   *   <li>R isn't symmetric positive definite
+   *   <li>The (A − BR⁻¹Nᵀ, B) pair isn't stabilizable
+   *   <li>The (A, C) pair where Q = CᵀC isn't detectable
+   * </ul>
+   *
+   * <p>Only use this function if you're sure the preconditions are met.
+   *
+   * @param A Array containing elements of A in row-major order.
+   * @param B Array containing elements of B in row-major order.
+   * @param Q Array containing elements of Q in row-major order.
+   * @param R Array containing elements of R in row-major order.
+   * @param N Array containing elements of N in row-major order.
+   * @param states Number of states in A matrix.
+   * @param inputs Number of inputs in B matrix.
+   * @param S Array storage for DARE solution.
+   */
+  public static native void dareDetailABQRN(
+      double[] A,
+      double[] B,
+      double[] Q,
+      double[] R,
+      double[] N,
+      int states,
+      int inputs,
+      double[] S);
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+   *
+   * @param A Array containing elements of A in row-major order.
+   * @param B Array containing elements of B in row-major order.
+   * @param Q Array containing elements of Q in row-major order.
+   * @param R Array containing elements of R in row-major order.
+   * @param states Number of states in A matrix.
+   * @param inputs Number of inputs in B matrix.
+   * @param S Array storage for DARE solution.
+   * @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
+   * @throws IllegalArgumentException if R isn't symmetric positive definite.
+   * @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
+   * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
+   */
+  public static native void dareABQR(
+      double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
+
+  /**
+   * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
+   *
+   * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+   *
+   * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
+   * following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q  N][xₖ]
+   * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
+   * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
+   *
+   * <pre>
+   *     ∞
+   * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   *    k=0
+   * </pre>
+   *
+   * <p>This can be refactored as:
+   *
+   * <pre>
+   *     ∞ [xₖ]ᵀ[Q 0][xₖ]
+   * J = Σ [uₖ] [0 R][uₖ] ΔT
+   *    k=0
+   * </pre>
+   *
+   * @param A Array containing elements of A in row-major order.
+   * @param B Array containing elements of B in row-major order.
+   * @param Q Array containing elements of Q in row-major order.
+   * @param R Array containing elements of R in row-major order.
+   * @param N Array containing elements of N in row-major order.
+   * @param states Number of states in A matrix.
+   * @param inputs Number of inputs in B matrix.
+   * @param S Array storage for DARE solution.
+   * @throws IllegalArgumentException if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite.
+   * @throws IllegalArgumentException if R isn't symmetric positive definite.
+   * @throws IllegalArgumentException if the (A − BR⁻¹Nᵀ, B) pair isn't stabilizable.
+   * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
+   */
+  public static native void dareABQRN(
+      double[] A,
+      double[] B,
+      double[] Q,
+      double[] R,
+      double[] N,
+      int states,
+      int inputs,
+      double[] S);
+
+  // Eigen wrappers
+
+  /**
    * Computes the matrix exp.
    *
    * @param src Array of elements of the matrix to be exponentiated.
@@ -77,6 +232,109 @@
   public static native void pow(double[] src, int rows, double exponent, double[] dst);
 
   /**
+   * Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition
+   * matrix.
+   *
+   * @param mat Array of elements of the matrix to be updated.
+   * @param lowerTriangular Whether mat is lower triangular.
+   * @param rows How many rows there are.
+   * @param vec Vector to use for the rank update.
+   * @param sigma Sigma value to use for the rank update.
+   */
+  public static native void rankUpdate(
+      double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular);
+
+  /**
+   * Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
+   *
+   * @param A Array of elements of the A matrix.
+   * @param Arows Number of rows of the A matrix.
+   * @param Acols Number of rows of the A matrix.
+   * @param B Array of elements of the B matrix.
+   * @param Brows Number of rows of the B matrix.
+   * @param Bcols Number of rows of the B matrix.
+   * @param dst Array to store solution in. If A is m-n and B is m-p, dst is n-p.
+   */
+  public static native void solveFullPivHouseholderQr(
+      double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst);
+
+  // Pose3d wrappers
+
+  /**
+   * Obtain a Pose3d from a (constant curvature) velocity.
+   *
+   * <p>The double array returned is of the form [dx, dy, dz, qx, qy, qz].
+   *
+   * @param poseX The pose's translational X component.
+   * @param poseY The pose's translational Y component.
+   * @param poseZ The pose's translational Z component.
+   * @param poseQw The pose quaternion's W component.
+   * @param poseQx The pose quaternion's X component.
+   * @param poseQy The pose quaternion's Y component.
+   * @param poseQz The pose quaternion's Z component.
+   * @param twistDx The twist's dx value.
+   * @param twistDy The twist's dy value.
+   * @param twistDz The twist's dz value.
+   * @param twistRx The twist's rx value.
+   * @param twistRy The twist's ry value.
+   * @param twistRz The twist's rz value.
+   * @return The new pose as a double array.
+   */
+  public static native double[] expPose3d(
+      double poseX,
+      double poseY,
+      double poseZ,
+      double poseQw,
+      double poseQx,
+      double poseQy,
+      double poseQz,
+      double twistDx,
+      double twistDy,
+      double twistDz,
+      double twistRx,
+      double twistRy,
+      double twistRz);
+
+  /**
+   * Returns a Twist3d that maps the starting pose to the end pose.
+   *
+   * <p>The double array returned is of the form [dx, dy, dz, rx, ry, rz].
+   *
+   * @param startX The starting pose's translational X component.
+   * @param startY The starting pose's translational Y component.
+   * @param startZ The starting pose's translational Z component.
+   * @param startQw The starting pose quaternion's W component.
+   * @param startQx The starting pose quaternion's X component.
+   * @param startQy The starting pose quaternion's Y component.
+   * @param startQz The starting pose quaternion's Z component.
+   * @param endX The ending pose's translational X component.
+   * @param endY The ending pose's translational Y component.
+   * @param endZ The ending pose's translational Z component.
+   * @param endQw The ending pose quaternion's W component.
+   * @param endQx The ending pose quaternion's X component.
+   * @param endQy The ending pose quaternion's Y component.
+   * @param endQz The ending pose quaternion's Z component.
+   * @return The twist that maps start to end as a double array.
+   */
+  public static native double[] logPose3d(
+      double startX,
+      double startY,
+      double startZ,
+      double startQw,
+      double startQx,
+      double startQy,
+      double startQz,
+      double endX,
+      double endY,
+      double endZ,
+      double endQw,
+      double endQx,
+      double endQy,
+      double endQz);
+
+  // StateSpaceUtil wrappers
+
+  /**
    * Returns true if (A, B) is a stabilizable pair.
    *
    * <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
@@ -91,6 +349,8 @@
    */
   public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);
 
+  // Trajectory wrappers
+
   /**
    * Loads a Pathweaver JSON.
    *
@@ -125,19 +385,6 @@
    */
   public static native String serializeTrajectory(double[] elements);
 
-  /**
-   * Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition
-   * matrix.
-   *
-   * @param mat Array of elements of the matrix to be updated.
-   * @param lowerTriangular Whether mat is lower triangular.
-   * @param rows How many rows there are.
-   * @param vec Vector to use for the rank update.
-   * @param sigma Sigma value to use for the rank update.
-   */
-  public static native void rankUpdate(
-      double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular);
-
   public static class Helper {
     private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
 
@@ -149,18 +396,4 @@
       extractOnStaticLoad.set(load);
     }
   }
-
-  /**
-   * Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
-   *
-   * @param A Array of elements of the A matrix.
-   * @param Arows Number of rows of the A matrix.
-   * @param Acols Number of rows of the A matrix.
-   * @param B Array of elements of the B matrix.
-   * @param Brows Number of rows of the B matrix.
-   * @param Bcols Number of rows of the B matrix.
-   * @param dst Array to store solution in. If A is m-n and B is m-p, dst is n-p.
-   */
-  public static native void solveFullPivHouseholderQr(
-      double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst);
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
index 385c5c0..a2a5944 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
@@ -181,6 +181,9 @@
   public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
     var rDot = (nextR.minus(r)).div(m_dt);
 
+    // ṙ = f(r) + Bu
+    // Bu = ṙ − f(r)
+    // u = B⁺(ṙ − f(r))
     m_uff = m_B.solve(rDot.minus(m_f.apply(r, new Matrix<>(m_inputs, Nat.N1()))));
 
     m_r = nextR;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
index 9f4dd86..cfe7c2b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
@@ -4,6 +4,10 @@
 
 package edu.wpi.first.math.controller;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+
 /**
  * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting
  * against the force of gravity).
@@ -54,6 +58,48 @@
   }
 
   /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * <p>Note this method is inaccurate when the velocity crosses 0.
+   *
+   * @param currentVelocity The current velocity setpoint.
+   * @param nextVelocity The next velocity setpoint.
+   * @param dtSeconds Time between velocity setpoints in seconds.
+   * @return The computed feedforward.
+   */
+  public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) {
+    // Discretize the affine model.
+    //
+    //   dx/dt = Ax + Bu + c
+    //   dx/dt = Ax + B(u + B⁺c)
+    //   xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
+    //   xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
+    //   xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ
+    //
+    // Solve for uₖ.
+    //
+    //   B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
+    //
+    // For an elevator with the model
+    // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x),
+    // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B
+    // assuming sgn(x) is a constant for the duration of the step.
+    //
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x)))
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x))
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x)
+    var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
+    var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
+
+    var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity);
+    var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity);
+
+    return kg + ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
+  }
+
+  /**
    * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
    * zero).
    *
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
index e43c6ff..87148a4 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
@@ -139,4 +139,31 @@
   public void setEnabled(boolean enabled) {
     m_enabled = enabled;
   }
+
+  /**
+   * Returns the heading controller.
+   *
+   * @return heading ProfiledPIDController
+   */
+  public ProfiledPIDController getThetaController() {
+    return m_thetaController;
+  }
+
+  /**
+   * Returns the x controller.
+   *
+   * @return X PIDController
+   */
+  public PIDController getXController() {
+    return m_xController;
+  }
+
+  /**
+   * Returns the y controller.
+   *
+   * @return Y PIDController
+   */
+  public PIDController getYController() {
+    return m_yController;
+  }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
index 46254a3..2be2a47 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math.controller;
 
+import edu.wpi.first.math.DARE;
 import edu.wpi.first.math.InterpolatingMatrixTreeMap;
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.MathUtil;
@@ -15,14 +16,22 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.system.Discretization;
 import edu.wpi.first.math.system.LinearSystem;
 import edu.wpi.first.math.trajectory.Trajectory;
 
 /**
  * The linear time-varying differential drive controller has a similar form to the LQR, but the
- * model used to compute the controller gain is the nonlinear model linearized around the
- * drivetrain's current state. We precomputed gains for important places in our state-space, then
- * interpolated between them with a LUT to save computational resources.
+ * model used to compute the controller gain is the nonlinear differential drive model linearized
+ * around the drivetrain's current state. We precompute gains for important places in our
+ * state-space, then interpolate between them with a lookup table to save computational resources.
+ *
+ * <p>This controller has a flat hierarchy with pose and wheel velocity references and voltage
+ * outputs. This is different from a Ramsete controller's nested hierarchy where the top-level
+ * controller has a pose reference and chassis velocity command outputs, and the low-level
+ * controller has wheel velocity references and voltage outputs. Flat hierarchies are easier to tune
+ * in one shot. Furthermore, this controller is more optimal in the "least-squares error" sense than
+ * a controller based on Ramsete.
  *
  * <p>See section 8.7 in Controls Engineering in FRC for a derivation of the control law we used
  * shown in theorem 8.7.4.
@@ -55,12 +64,18 @@
   /**
    * Constructs a linear time-varying differential drive controller.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param plant The differential drive velocity plant.
    * @param trackwidth The distance between the differential drive's left and right wheels in
    *     meters.
    * @param qelems The maximum desired error tolerance for each state.
    * @param relems The maximum desired control effort for each input.
    * @param dt Discretization timestep in seconds.
+   * @throws IllegalArgumentException if max velocity of plant with 12 V input &lt;= 0 m/s or &gt;=
+   *     15 m/s.
    */
   public LTVDifferentialDriveController(
       LinearSystem<N2, N2, N2> plant,
@@ -127,15 +142,39 @@
             .times(-1.0)
             .get(0, 0);
 
+    if (maxV <= 0.0) {
+      throw new IllegalArgumentException(
+          "Max velocity of plant with 12 V input must be greater than 0 m/s.");
+    }
+    if (maxV >= 15.0) {
+      throw new IllegalArgumentException(
+          "Max velocity of plant with 12 V input must be less than 15 m/s.");
+    }
+
     for (double velocity = -maxV; velocity < maxV; velocity += 0.01) {
       // The DARE is ill-conditioned if the velocity is close to zero, so don't
       // let the system stop.
       if (Math.abs(velocity) < 1e-4) {
-        m_table.put(velocity, new Matrix<>(Nat.N2(), Nat.N5()));
+        A.set(State.kY.value, State.kHeading.value, 1e-4);
       } else {
         A.set(State.kY.value, State.kHeading.value, velocity);
-        m_table.put(velocity, new LinearQuadraticRegulator<N5, N2, N5>(A, B, Q, R, dt).getK());
       }
+
+      var discABPair = Discretization.discretizeAB(A, B, dt);
+      var discA = discABPair.getFirst();
+      var discB = discABPair.getSecond();
+
+      var S = DARE.dareDetail(discA, discB, Q, R);
+
+      // K = (BᵀSB + R)⁻¹BᵀSA
+      m_table.put(
+          velocity,
+          discB
+              .transpose()
+              .times(S)
+              .times(discB)
+              .plus(R)
+              .solve(discB.transpose().times(S).times(discA)));
     }
   }
 
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
index 701f21b..7e3391f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math.controller;
 
+import edu.wpi.first.math.DARE;
 import edu.wpi.first.math.InterpolatingMatrixTreeMap;
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
@@ -15,12 +16,16 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.system.Discretization;
 import edu.wpi.first.math.trajectory.Trajectory;
 
 /**
  * The linear time-varying unicycle controller has a similar form to the LQR, but the model used to
- * compute the controller gain is the nonlinear model linearized around the drivetrain's current
- * state.
+ * compute the controller gain is the nonlinear unicycle model linearized around the drivetrain's
+ * current state.
+ *
+ * <p>This controller is a roughly drop-in replacement for {@link RamseteController} with more
+ * optimal feedback gains in the "least-squares error" sense.
  *
  * <p>See section 8.9 in Controls Engineering in FRC for a derivation of the control law we used
  * shown in theorem 8.9.1.
@@ -66,6 +71,7 @@
    * @param dt Discretization timestep in seconds.
    * @param maxVelocity The maximum velocity in meters per second for the controller gain lookup
    *     table. The default is 9 m/s.
+   * @throws IllegalArgumentException if maxVelocity &lt;= 0.
    */
   public LTVUnicycleController(double dt, double maxVelocity) {
     this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, maxVelocity);
@@ -74,6 +80,10 @@
   /**
    * Constructs a linear time-varying unicycle controller.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param qelems The maximum desired error tolerance for each state.
    * @param relems The maximum desired control effort for each input.
    * @param dt Discretization timestep in seconds.
@@ -85,14 +95,26 @@
   /**
    * Constructs a linear time-varying unicycle controller.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param qelems The maximum desired error tolerance for each state.
    * @param relems The maximum desired control effort for each input.
    * @param dt Discretization timestep in seconds.
    * @param maxVelocity The maximum velocity in meters per second for the controller gain lookup
    *     table. The default is 9 m/s.
+   * @throws IllegalArgumentException if maxVelocity &lt;= 0 m/s or &gt;= 15 m/s.
    */
   public LTVUnicycleController(
       Vector<N3> qelems, Vector<N2> relems, double dt, double maxVelocity) {
+    if (maxVelocity <= 0.0) {
+      throw new IllegalArgumentException("Max velocity must be greater than 0 m/s.");
+    }
+    if (maxVelocity >= 15.0) {
+      throw new IllegalArgumentException("Max velocity must be less than 15 m/s.");
+    }
+
     // The change in global pose for a unicycle is defined by the following
     // three equations.
     //
@@ -135,11 +157,26 @@
       // The DARE is ill-conditioned if the velocity is close to zero, so don't
       // let the system stop.
       if (Math.abs(velocity) < 1e-4) {
-        m_table.put(velocity, new Matrix<>(Nat.N2(), Nat.N3()));
+        A.set(State.kY.value, State.kHeading.value, 1e-4);
       } else {
         A.set(State.kY.value, State.kHeading.value, velocity);
-        m_table.put(velocity, new LinearQuadraticRegulator<N3, N2, N3>(A, B, Q, R, dt).getK());
       }
+
+      var discABPair = Discretization.discretizeAB(A, B, dt);
+      var discA = discABPair.getFirst();
+      var discB = discABPair.getSecond();
+
+      var S = DARE.dareDetail(discA, discB, Q, R);
+
+      // K = (BᵀSB + R)⁻¹BᵀSA
+      m_table.put(
+          velocity,
+          discB
+              .transpose()
+              .times(S)
+              .times(discB)
+              .plus(R)
+              .solve(discB.transpose().times(S).times(discA)));
     }
   }
 
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 a762851..b0f23a8 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
@@ -139,6 +139,9 @@
    * @return The calculated feedforward.
    */
   public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
+    // rₖ₊₁ = Arₖ + Buₖ
+    // Buₖ = rₖ₊₁ − Arₖ
+    // uₖ = B⁺(rₖ₊₁ − Arₖ)
     m_uff = new Matrix<>(m_B.solve(nextR.minus(m_A.times(r))));
 
     m_r = nextR;
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 dce1748..658be69 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,7 @@
 
 package edu.wpi.first.math.controller;
 
-import edu.wpi.first.math.Drake;
+import edu.wpi.first.math.DARE;
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Num;
@@ -35,6 +35,10 @@
   /**
    * Constructs a controller with the given coefficients and plant. Rho is defaulted to 1.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param plant The plant being controlled.
    * @param qelms The maximum desired error tolerance for each state.
    * @param relms The maximum desired control effort for each input.
@@ -57,6 +61,10 @@
   /**
    * Constructs a controller with the given coefficients and plant.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param A Continuous system matrix of the plant being controlled.
    * @param B Continuous input matrix of the plant being controlled.
    * @param qelms The maximum desired error tolerance for each state.
@@ -111,7 +119,7 @@
       throw new IllegalArgumentException(msg);
     }
 
-    var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R);
+    var S = DARE.dare(discA, discB, Q, R);
 
     // K = (BᵀSB + R)⁻¹BᵀSA
     m_K =
@@ -150,7 +158,7 @@
     var discA = discABPair.getFirst();
     var discB = discABPair.getSecond();
 
-    var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
+    var S = DARE.dare(discA, discB, Q, R, N);
 
     // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
     m_K =
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
index 5e82909..1d98900 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
@@ -24,6 +24,9 @@
   // Factor for "derivative" control
   private double m_kd;
 
+  // The error range where "integral" control applies
+  private double m_iZone = Double.POSITIVE_INFINITY;
+
   // The period (in seconds) of the loop that calls the controller
   private final double m_period;
 
@@ -55,6 +58,9 @@
   private double m_setpoint;
   private double m_measurement;
 
+  private boolean m_haveMeasurement;
+  private boolean m_haveSetpoint;
+
   /**
    * Allocates a PIDController with the given constants for kp, ki, and kd and a default period of
    * 0.02 seconds.
@@ -139,6 +145,22 @@
   }
 
   /**
+   * Sets the IZone range. When the absolute value of the position error is greater than IZone, the
+   * total accumulated error will reset to zero, disabling integral gain until the absolute value of
+   * the position error is less than IZone. This is used to prevent integral windup. Must be
+   * non-negative. Passing a value of zero will effectively disable integral gain. Passing a value
+   * of {@link Double#POSITIVE_INFINITY} disables IZone functionality.
+   *
+   * @param iZone Maximum magnitude of error to allow integral control.
+   */
+  public void setIZone(double iZone) {
+    if (iZone < 0) {
+      throw new IllegalArgumentException("IZone must be a non-negative number!");
+    }
+    m_iZone = iZone;
+  }
+
+  /**
    * Get the Proportional coefficient.
    *
    * @return proportional coefficient
@@ -166,6 +188,15 @@
   }
 
   /**
+   * Get the IZone range.
+   *
+   * @return Maximum magnitude of error to allow integral control.
+   */
+  public double getIZone() {
+    return m_iZone;
+  }
+
+  /**
    * Returns the period of this controller.
    *
    * @return the period of the controller.
@@ -199,6 +230,7 @@
    */
   public void setSetpoint(double setpoint) {
     m_setpoint = setpoint;
+    m_haveSetpoint = true;
 
     if (m_continuous) {
       double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
@@ -227,7 +259,9 @@
    * @return Whether the error is within the acceptable bounds.
    */
   public boolean atSetpoint() {
-    return Math.abs(m_positionError) < m_positionTolerance
+    return m_haveMeasurement
+        && m_haveSetpoint
+        && Math.abs(m_positionError) < m_positionTolerance
         && Math.abs(m_velocityError) < m_velocityTolerance;
   }
 
@@ -321,6 +355,7 @@
    */
   public double calculate(double measurement, double setpoint) {
     m_setpoint = setpoint;
+    m_haveSetpoint = true;
     return calculate(measurement);
   }
 
@@ -333,6 +368,7 @@
   public double calculate(double measurement) {
     m_measurement = measurement;
     m_prevError = m_positionError;
+    m_haveMeasurement = true;
 
     if (m_continuous) {
       double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
@@ -343,7 +379,10 @@
 
     m_velocityError = (m_positionError - m_prevError) / m_period;
 
-    if (m_ki != 0) {
+    // If the absolute value of the position error is greater than IZone, reset the total error
+    if (Math.abs(m_positionError) > m_iZone) {
+      m_totalError = 0;
+    } else if (m_ki != 0) {
       m_totalError =
           MathUtil.clamp(
               m_totalError + m_positionError * m_period,
@@ -360,6 +399,7 @@
     m_prevError = 0;
     m_totalError = 0;
     m_velocityError = 0;
+    m_haveMeasurement = false;
   }
 
   @Override
@@ -368,6 +408,7 @@
     builder.addDoubleProperty("p", this::getP, this::setP);
     builder.addDoubleProperty("i", this::getI, this::setI);
     builder.addDoubleProperty("d", this::getD, this::setD);
+    builder.addDoubleProperty("izone", this::getIZone, this::setIZone);
     builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
   }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
index 02cc17d..80311f5 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
@@ -22,9 +22,11 @@
   private PIDController m_controller;
   private double m_minimumInput;
   private double m_maximumInput;
+
+  private TrapezoidProfile.Constraints m_constraints;
+  private TrapezoidProfile m_profile;
   private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
   private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
-  private TrapezoidProfile.Constraints m_constraints;
 
   /**
    * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and Kd.
@@ -52,6 +54,7 @@
       double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints, double period) {
     m_controller = new PIDController(Kp, Ki, Kd, period);
     m_constraints = constraints;
+    m_profile = new TrapezoidProfile(m_constraints);
     instances++;
 
     SendableRegistry.add(this, "ProfiledPIDController", instances);
@@ -99,6 +102,19 @@
   }
 
   /**
+   * Sets the IZone range. When the absolute value of the position error is greater than IZone, the
+   * total accumulated error will reset to zero, disabling integral gain until the absolute value of
+   * the position error is less than IZone. This is used to prevent integral windup. Must be
+   * non-negative. Passing a value of zero will effectively disable integral gain. Passing a value
+   * of {@link Double#POSITIVE_INFINITY} disables IZone functionality.
+   *
+   * @param iZone Maximum magnitude of error to allow integral control.
+   */
+  public void setIZone(double iZone) {
+    m_controller.setIZone(iZone);
+  }
+
+  /**
    * Gets the proportional coefficient.
    *
    * @return proportional coefficient
@@ -126,6 +142,15 @@
   }
 
   /**
+   * Get the IZone range.
+   *
+   * @return Maximum magnitude of error to allow integral control.
+   */
+  public double getIZone() {
+    return m_controller.getIZone();
+  }
+
+  /**
    * Gets the period of this controller.
    *
    * @return The period of the controller.
@@ -197,6 +222,16 @@
    */
   public void setConstraints(TrapezoidProfile.Constraints constraints) {
     m_constraints = constraints;
+    m_profile = new TrapezoidProfile(m_constraints);
+  }
+
+  /**
+   * Get the velocity and acceleration constraints for this controller.
+   *
+   * @return Velocity and acceleration constraints.
+   */
+  public TrapezoidProfile.Constraints getConstraints() {
+    return m_constraints;
   }
 
   /**
@@ -312,8 +347,7 @@
       m_setpoint.position = setpointMinDistance + measurement;
     }
 
-    var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
-    m_setpoint = profile.calculate(getPeriod());
+    m_setpoint = m_profile.calculate(getPeriod(), m_goal, m_setpoint);
     return m_controller.calculate(measurement, m_setpoint.position);
   }
 
@@ -391,6 +425,7 @@
     builder.addDoubleProperty("p", this::getP, this::setP);
     builder.addDoubleProperty("i", this::getI, this::setI);
     builder.addDoubleProperty("d", this::getD, this::setD);
+    builder.addDoubleProperty("izone", this::getIZone, this::setIZone);
     builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal);
   }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
index b5fe591..7918f9f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
@@ -4,22 +4,15 @@
 
 package edu.wpi.first.math.estimator;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.util.WPIUtilJNI;
-import java.util.Map;
-import java.util.Objects;
 
 /**
  * This class wraps {@link DifferentialDriveOdometry Differential Drive Odometry} to fuse
@@ -34,15 +27,7 @@
  * <p>{@link DifferentialDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as
  * you want; if you never call it then this class will behave exactly like regular encoder odometry.
  */
-public class DifferentialDrivePoseEstimator {
-  private final DifferentialDriveKinematics m_kinematics;
-  private final DifferentialDriveOdometry m_odometry;
-  private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
-  private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
-
-  private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
-      TimeInterpolatableBuffer.createBuffer(1.5);
-
+public class DifferentialDrivePoseEstimator extends PoseEstimator<DifferentialDriveWheelPositions> {
   /**
    * Constructs a DifferentialDrivePoseEstimator with default standard deviations for the model and
    * vision measurements.
@@ -96,44 +81,12 @@
       Pose2d initialPoseMeters,
       Matrix<N3, N1> stateStdDevs,
       Matrix<N3, N1> visionMeasurementStdDevs) {
-    m_kinematics = kinematics;
-    m_odometry =
+    super(
+        kinematics,
         new DifferentialDriveOdometry(
-            gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters);
-
-    for (int i = 0; i < 3; ++i) {
-      m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
-    }
-
-    // Initialize vision R
-    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-  }
-
-  /**
-   * Sets the pose estimator's trust of global measurements. This might be used to change trust in
-   * vision measurements after the autonomous period, or to change trust as distance to a vision
-   * target increases.
-   *
-   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
-   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
-   *     theta]ᵀ, with units in meters and radians.
-   */
-  public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
-    var r = new double[3];
-    for (int i = 0; i < 3; ++i) {
-      r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
-    }
-
-    // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
-    // and C = I. See wpimath/algorithms.md.
-    for (int row = 0; row < 3; ++row) {
-      if (m_q.get(row, 0) == 0.0) {
-        m_visionK.set(row, row, 0.0);
-      } else {
-        m_visionK.set(
-            row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
-      }
-    }
+            gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters),
+        stateStdDevs,
+        visionMeasurementStdDevs);
   }
 
   /**
@@ -152,111 +105,10 @@
       double leftPositionMeters,
       double rightPositionMeters,
       Pose2d poseMeters) {
-    // Reset state estimate and error covariance
-    m_odometry.resetPosition(gyroAngle, leftPositionMeters, rightPositionMeters, poseMeters);
-    m_poseBuffer.clear();
-  }
-
-  /**
-   * Gets the estimated robot pose.
-   *
-   * @return The estimated robot pose in meters.
-   */
-  public Pose2d getEstimatedPosition() {
-    return m_odometry.getPoseMeters();
-  }
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
-   * while still accounting for measurement noise.
-   *
-   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
-   * DifferentialDrivePoseEstimator#update} every loop.
-   *
-   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
-   * recommend only adding vision measurements that are already within one meter or so of the
-   * current pose estimate.
-   *
-   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
-   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
-   *     don't use your own time source by calling {@link
-   *     DifferentialDrivePoseEstimator#updateWithTime(double,Rotation2d,double,double)} then you
-   *     must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
-   *     the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that
-   *     you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source
-   *     or sync the epochs.
-   */
-  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
-    // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
-    var sample = m_poseBuffer.getSample(timestampSeconds);
-
-    if (sample.isEmpty()) {
-      return;
-    }
-
-    // Step 2: Measure the twist between the odometry pose and the vision pose.
-    var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
-
-    // Step 3: We should not trust the twist entirely, so instead we scale this twist by a Kalman
-    // gain matrix representing how much we trust vision measurements compared to our current pose.
-    var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
-
-    // Step 4: Convert back to Twist2d.
-    var scaledTwist =
-        new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
-
-    // Step 5: Reset Odometry to state at sample with vision adjustment.
-    m_odometry.resetPosition(
-        sample.get().gyroAngle,
-        sample.get().leftMeters,
-        sample.get().rightMeters,
-        sample.get().poseMeters.exp(scaledTwist));
-
-    // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
-    // pose buffer and correct odometry.
-    for (Map.Entry<Double, InterpolationRecord> entry :
-        m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
-      updateWithTime(
-          entry.getKey(),
-          entry.getValue().gyroAngle,
-          entry.getValue().leftMeters,
-          entry.getValue().rightMeters);
-    }
-  }
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
-   * while still accounting for measurement noise.
-   *
-   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
-   * DifferentialDrivePoseEstimator#update} every loop.
-   *
-   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
-   * recommend only adding vision measurements that are already within one meter or so of the
-   * current pose estimate.
-   *
-   * <p>Note that the vision measurement standard deviations passed into this method will continue
-   * to apply to future measurements until a subsequent call to {@link
-   * DifferentialDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
-   *
-   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
-   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
-   *     don't use your own time source by calling {@link
-   *     DifferentialDrivePoseEstimator#updateWithTime(double,Rotation2d,double,double)}, then you
-   *     must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
-   *     the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that
-   *     you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source
-   *     in this case.
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
-   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
-   *     the vision pose measurement less.
-   */
-  public void addVisionMeasurement(
-      Pose2d visionRobotPoseMeters,
-      double timestampSeconds,
-      Matrix<N3, N1> visionMeasurementStdDevs) {
-    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+    resetPosition(
+        gyroAngle,
+        new DifferentialDriveWheelPositions(leftPositionMeters, rightPositionMeters),
+        poseMeters);
   }
 
   /**
@@ -270,8 +122,8 @@
    */
   public Pose2d update(
       Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters) {
-    return updateWithTime(
-        WPIUtilJNI.now() * 1.0e-6, gyroAngle, distanceLeftMeters, distanceRightMeters);
+    return update(
+        gyroAngle, new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters));
   }
 
   /**
@@ -289,98 +141,9 @@
       Rotation2d gyroAngle,
       double distanceLeftMeters,
       double distanceRightMeters) {
-    m_odometry.update(gyroAngle, distanceLeftMeters, distanceRightMeters);
-    m_poseBuffer.addSample(
+    return updateWithTime(
         currentTimeSeconds,
-        new InterpolationRecord(
-            getEstimatedPosition(), gyroAngle, distanceLeftMeters, distanceRightMeters));
-
-    return getEstimatedPosition();
-  }
-
-  /**
-   * Represents an odometry record. The record contains the inputs provided as well as the pose that
-   * was observed based on these inputs, as well as the previous record and its inputs.
-   */
-  private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
-    // The pose observed given the current sensor inputs and the previous pose.
-    private final Pose2d poseMeters;
-
-    // The current gyro angle.
-    private final Rotation2d gyroAngle;
-
-    // The distance traveled by the left encoder.
-    private final double leftMeters;
-
-    // The distance traveled by the right encoder.
-    private final double rightMeters;
-
-    /**
-     * Constructs an Interpolation Record with the specified parameters.
-     *
-     * @param pose The pose observed given the current sensor inputs and the previous pose.
-     * @param gyro The current gyro angle.
-     * @param leftMeters The distance traveled by the left encoder.
-     * @param rightMeters The distanced traveled by the right encoder.
-     */
-    private InterpolationRecord(
-        Pose2d poseMeters, Rotation2d gyro, double leftMeters, double rightMeters) {
-      this.poseMeters = poseMeters;
-      this.gyroAngle = gyro;
-      this.leftMeters = leftMeters;
-      this.rightMeters = rightMeters;
-    }
-
-    /**
-     * Return the interpolated record. This object is assumed to be the starting position, or lower
-     * bound.
-     *
-     * @param endValue The upper bound, or end.
-     * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
-     * @return The interpolated value.
-     */
-    @Override
-    public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
-      if (t < 0) {
-        return this;
-      } else if (t >= 1) {
-        return endValue;
-      } else {
-        // Find the new left distance.
-        var left_lerp = MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t);
-
-        // Find the new right distance.
-        var right_lerp = MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t);
-
-        // Find the new gyro angle.
-        var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
-
-        // Create a twist to represent this change based on the interpolated sensor inputs.
-        Twist2d twist = m_kinematics.toTwist2d(left_lerp - leftMeters, right_lerp - rightMeters);
-        twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
-
-        return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, left_lerp, right_lerp);
-      }
-    }
-
-    @Override
-    public boolean equals(Object obj) {
-      if (this == obj) {
-        return true;
-      }
-      if (!(obj instanceof InterpolationRecord)) {
-        return false;
-      }
-      InterpolationRecord record = (InterpolationRecord) obj;
-      return Objects.equals(gyroAngle, record.gyroAngle)
-          && Double.compare(leftMeters, record.leftMeters) == 0
-          && Double.compare(rightMeters, record.rightMeters) == 0
-          && Objects.equals(poseMeters, record.poseMeters);
-    }
-
-    @Override
-    public int hashCode() {
-      return Objects.hash(gyroAngle, leftMeters, rightMeters, poseMeters);
-    }
+        gyroAngle,
+        new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters));
   }
 }
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 5f9e52e..59323b2 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,7 +4,7 @@
 
 package edu.wpi.first.math.estimator;
 
-import edu.wpi.first.math.Drake;
+import edu.wpi.first.math.DARE;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
@@ -59,6 +59,10 @@
   /**
    * Constructs an extended Kalman filter.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param states a Nat representing the number of states.
    * @param inputs a Nat representing the number of inputs.
    * @param outputs a Nat representing the number of outputs.
@@ -93,6 +97,10 @@
   /**
    * Constructs an extended Kalman filter.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param states a Nat representing the number of states.
    * @param inputs a Nat representing the number of inputs.
    * @param outputs a Nat representing the number of outputs.
@@ -138,15 +146,14 @@
         NumericalJacobian.numericalJacobianX(
             outputs, states, h, m_xHat, new Matrix<>(inputs, Nat.N1()));
 
-    final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
+    final var discPair = Discretization.discretizeAQ(contA, m_contQ, dtSeconds);
     final var discA = discPair.getFirst();
     final var discQ = discPair.getSecond();
 
     final var discR = Discretization.discretizeR(m_contR, dtSeconds);
 
     if (StateSpaceUtil.isDetectable(discA, C) && outputs.getNum() <= states.getNum()) {
-      m_initP =
-          Drake.discreteAlgebraicRiccatiEquation(discA.transpose(), C.transpose(), discQ, discR);
+      m_initP = DARE.dare(discA.transpose(), C.transpose(), discQ, discR);
     } else {
       m_initP = new Matrix<>(states, states);
     }
@@ -260,7 +267,7 @@
     final var contA = NumericalJacobian.numericalJacobianX(m_states, m_states, f, m_xHat, u);
 
     // Find discrete A and Q
-    final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
+    final var discPair = Discretization.discretizeAQ(contA, m_contQ, dtSeconds);
     final var discA = discPair.getFirst();
     final var discQ = discPair.getSecond();
 
@@ -286,24 +293,14 @@
   /**
    * Correct the state estimate x-hat using the measurements in y.
    *
-   * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
-   * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
-   * of this function).
+   * <p>This is useful for when the measurement noise covariances vary.
    *
-   * @param <Rows> Number of rows in the result of f(x, u).
-   * @param rows Number of rows in the result of f(x, u).
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
-   * @param h A vector-valued function of x and u that returns the measurement vector.
-   * @param contR Continuous measurement noise covariance matrix.
+   * @param R Continuous measurement noise covariance matrix.
    */
-  public <Rows extends Num> void correct(
-      Nat<Rows> rows,
-      Matrix<Inputs, N1> u,
-      Matrix<Rows, N1> y,
-      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
-      Matrix<Rows, Rows> contR) {
-    correct(rows, u, y, h, contR, Matrix::minus, Matrix::plus);
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y, Matrix<Outputs, Outputs> R) {
+    correct(m_outputs, u, y, m_h, R, m_residualFuncY, m_addFuncX);
   }
 
   /**
@@ -318,7 +315,30 @@
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    * @param h A vector-valued function of x and u that returns the measurement vector.
-   * @param contR Continuous measurement noise covariance matrix.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  public <Rows extends Num> void correct(
+      Nat<Rows> rows,
+      Matrix<Inputs, N1> u,
+      Matrix<Rows, N1> y,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
+      Matrix<Rows, Rows> R) {
+    correct(rows, u, y, h, R, Matrix::minus, Matrix::plus);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
+   * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
+   * of this function).
+   *
+   * @param <Rows> Number of rows in the result of f(x, u).
+   * @param rows Number of rows in the result of f(x, u).
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
    * @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
    *     subtracts them.)
    * @param addFuncX A function that adds two state vectors.
@@ -328,11 +348,11 @@
       Matrix<Inputs, N1> u,
       Matrix<Rows, N1> y,
       BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
-      Matrix<Rows, Rows> contR,
+      Matrix<Rows, Rows> R,
       BiFunction<Matrix<Rows, N1>, Matrix<Rows, N1>, Matrix<Rows, N1>> residualFuncY,
       BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX) {
     final var C = NumericalJacobian.numericalJacobianX(rows, m_states, h, m_xHat, u);
-    final var discR = Discretization.discretizeR(contR, m_dtSeconds);
+    final var discR = Discretization.discretizeR(R, m_dtSeconds);
 
     final var S = C.times(m_P).times(C.transpose()).plus(discR);
 
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 24d6d91..08e3270 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,7 @@
 
 package edu.wpi.first.math.estimator;
 
-import edu.wpi.first.math.Drake;
+import edu.wpi.first.math.DARE;
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -43,6 +43,10 @@
   /**
    * Constructs a state-space observer with the given plant.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param states A Nat representing the states of the system.
    * @param outputs A Nat representing the outputs of the system.
    * @param plant The plant used for the prediction step.
@@ -65,7 +69,7 @@
     var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
     var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
 
-    var pair = Discretization.discretizeAQTaylor(plant.getA(), contQ, dtSeconds);
+    var pair = Discretization.discretizeAQ(plant.getA(), contQ, dtSeconds);
     var discA = pair.getFirst();
     var discQ = pair.getSecond();
 
@@ -87,9 +91,7 @@
       throw new IllegalArgumentException(msg);
     }
 
-    var P =
-        new Matrix<>(
-            Drake.discreteAlgebraicRiccatiEquation(discA.transpose(), C.transpose(), discQ, discR));
+    var P = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
 
     // S = CPCᵀ + R
     var S = C.times(P).times(C.transpose()).plus(discR);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
index 7a100f5..7b14839 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
@@ -8,7 +8,7 @@
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.numbers.N1;
 
-interface KalmanTypeFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
+public interface KalmanTypeFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
   Matrix<States, States> getP();
 
   double getP(int i, int j);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
index 448b8d3..59bbb32 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
@@ -4,23 +4,15 @@
 
 package edu.wpi.first.math.estimator;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
 import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
 import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.util.WPIUtilJNI;
-import java.util.Map;
-import java.util.Objects;
 
 /**
  * This class wraps {@link MecanumDriveOdometry Mecanum Drive Odometry} to fuse latency-compensated
@@ -33,15 +25,7 @@
  * <p>{@link MecanumDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
  * want; if you never call it, then this class will behave mostly like regular encoder odometry.
  */
-public class MecanumDrivePoseEstimator {
-  private final MecanumDriveKinematics m_kinematics;
-  private final MecanumDriveOdometry m_odometry;
-  private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
-  private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
-
-  private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
-      TimeInterpolatableBuffer.createBuffer(1.5);
-
+public class MecanumDrivePoseEstimator extends PoseEstimator<MecanumDriveWheelPositions> {
   /**
    * Constructs a MecanumDrivePoseEstimator with default standard deviations for the model and
    * vision measurements.
@@ -90,294 +74,10 @@
       Pose2d initialPoseMeters,
       Matrix<N3, N1> stateStdDevs,
       Matrix<N3, N1> visionMeasurementStdDevs) {
-    m_kinematics = kinematics;
-    m_odometry = new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
-
-    for (int i = 0; i < 3; ++i) {
-      m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
-    }
-
-    // Initialize vision R
-    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-  }
-
-  /**
-   * Sets the pose estimator's trust of global measurements. This might be used to change trust in
-   * vision measurements after the autonomous period, or to change trust as distance to a vision
-   * target increases.
-   *
-   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
-   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
-   *     theta]ᵀ, with units in meters and radians.
-   */
-  public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
-    var r = new double[3];
-    for (int i = 0; i < 3; ++i) {
-      r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
-    }
-
-    // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
-    // and C = I. See wpimath/algorithms.md.
-    for (int row = 0; row < 3; ++row) {
-      if (m_q.get(row, 0) == 0.0) {
-        m_visionK.set(row, row, 0.0);
-      } else {
-        m_visionK.set(
-            row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
-      }
-    }
-  }
-
-  /**
-   * Resets the robot's position on the field.
-   *
-   * <p>The gyroscope angle does not need to be reset in the user's robot code. The library
-   * automatically takes care of offsetting the gyro angle.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelPositions The distances driven by each wheel.
-   * @param poseMeters The position on the field that your robot is at.
-   */
-  public void resetPosition(
-      Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
-    // Reset state estimate and error covariance
-    m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters);
-    m_poseBuffer.clear();
-  }
-
-  /**
-   * Gets the estimated robot pose.
-   *
-   * @return The estimated robot pose in meters.
-   */
-  public Pose2d getEstimatedPosition() {
-    return m_odometry.getPoseMeters();
-  }
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
-   * while still accounting for measurement noise.
-   *
-   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
-   * MecanumDrivePoseEstimator#update} every loop.
-   *
-   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
-   * recommend only adding vision measurements that are already within one meter or so of the
-   * current pose estimate.
-   *
-   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
-   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
-   *     don't use your own time source by calling {@link
-   *     MecanumDrivePoseEstimator#updateWithTime(double,Rotation2d,MecanumDriveWheelPositions)}
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
-   *     timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.)
-   *     This means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as
-   *     your time source or sync the epochs.
-   */
-  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
-    // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
-    var sample = m_poseBuffer.getSample(timestampSeconds);
-
-    if (sample.isEmpty()) {
-      return;
-    }
-
-    // Step 2: Measure the twist between the odometry pose and the vision pose.
-    var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
-
-    // Step 3: We should not trust the twist entirely, so instead we scale this twist by a Kalman
-    // gain matrix representing how much we trust vision measurements compared to our current pose.
-    var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
-
-    // Step 4: Convert back to Twist2d.
-    var scaledTwist =
-        new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
-
-    // Step 5: Reset Odometry to state at sample with vision adjustment.
-    m_odometry.resetPosition(
-        sample.get().gyroAngle,
-        sample.get().wheelPositions,
-        sample.get().poseMeters.exp(scaledTwist));
-
-    // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
-    // pose buffer and correct odometry.
-    for (Map.Entry<Double, InterpolationRecord> entry :
-        m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
-      updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().wheelPositions);
-    }
-  }
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
-   * while still accounting for measurement noise.
-   *
-   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
-   * MecanumDrivePoseEstimator#update} every loop.
-   *
-   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
-   * recommend only adding vision measurements that are already within one meter or so of the
-   * current pose estimate.
-   *
-   * <p>Note that the vision measurement standard deviations passed into this method will continue
-   * to apply to future measurements until a subsequent call to {@link
-   * MecanumDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
-   *
-   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
-   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
-   *     don't use your own time source by calling {@link
-   *     MecanumDrivePoseEstimator#updateWithTime(double,Rotation2d,MecanumDriveWheelPositions)},
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
-   *     timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}).
-   *     This means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as
-   *     your time source in this case.
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
-   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
-   *     the vision pose measurement less.
-   */
-  public void addVisionMeasurement(
-      Pose2d visionRobotPoseMeters,
-      double timestampSeconds,
-      Matrix<N3, N1> visionMeasurementStdDevs) {
-    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
-  }
-
-  /**
-   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
-   * loop.
-   *
-   * @param gyroAngle The current gyro angle.
-   * @param wheelPositions The distances driven by each wheel.
-   * @return The estimated pose of the robot in meters.
-   */
-  public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
-    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelPositions);
-  }
-
-  /**
-   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
-   * loop.
-   *
-   * @param currentTimeSeconds Time at which this method was called, in seconds.
-   * @param gyroAngle The current gyroscope angle.
-   * @param wheelPositions The distances driven by each wheel.
-   * @return The estimated pose of the robot in meters.
-   */
-  public Pose2d updateWithTime(
-      double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
-    m_odometry.update(gyroAngle, wheelPositions);
-
-    m_poseBuffer.addSample(
-        currentTimeSeconds,
-        new InterpolationRecord(
-            getEstimatedPosition(),
-            gyroAngle,
-            new MecanumDriveWheelPositions(
-                wheelPositions.frontLeftMeters,
-                wheelPositions.frontRightMeters,
-                wheelPositions.rearLeftMeters,
-                wheelPositions.rearRightMeters)));
-
-    return getEstimatedPosition();
-  }
-
-  /**
-   * Represents an odometry record. The record contains the inputs provided as well as the pose that
-   * was observed based on these inputs, as well as the previous record and its inputs.
-   */
-  private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
-    // The pose observed given the current sensor inputs and the previous pose.
-    private final Pose2d poseMeters;
-
-    // The current gyro angle.
-    private final Rotation2d gyroAngle;
-
-    // The distances traveled by each wheel encoder.
-    private final MecanumDriveWheelPositions wheelPositions;
-
-    /**
-     * Constructs an Interpolation Record with the specified parameters.
-     *
-     * @param pose The pose observed given the current sensor inputs and the previous pose.
-     * @param gyro The current gyro angle.
-     * @param wheelPositions The distances traveled by each wheel encoder.
-     */
-    private InterpolationRecord(
-        Pose2d poseMeters, Rotation2d gyro, MecanumDriveWheelPositions wheelPositions) {
-      this.poseMeters = poseMeters;
-      this.gyroAngle = gyro;
-      this.wheelPositions = wheelPositions;
-    }
-
-    /**
-     * Return the interpolated record. This object is assumed to be the starting position, or lower
-     * bound.
-     *
-     * @param endValue The upper bound, or end.
-     * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
-     * @return The interpolated value.
-     */
-    @Override
-    public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
-      if (t < 0) {
-        return this;
-      } else if (t >= 1) {
-        return endValue;
-      } else {
-        // Find the new wheel distances.
-        var wheels_lerp =
-            new MecanumDriveWheelPositions(
-                MathUtil.interpolate(
-                    this.wheelPositions.frontLeftMeters,
-                    endValue.wheelPositions.frontLeftMeters,
-                    t),
-                MathUtil.interpolate(
-                    this.wheelPositions.frontRightMeters,
-                    endValue.wheelPositions.frontRightMeters,
-                    t),
-                MathUtil.interpolate(
-                    this.wheelPositions.rearLeftMeters, endValue.wheelPositions.rearLeftMeters, t),
-                MathUtil.interpolate(
-                    this.wheelPositions.rearRightMeters,
-                    endValue.wheelPositions.rearRightMeters,
-                    t));
-
-        // Find the distance travelled between this measurement and the interpolated measurement.
-        var wheels_delta =
-            new MecanumDriveWheelPositions(
-                wheels_lerp.frontLeftMeters - this.wheelPositions.frontLeftMeters,
-                wheels_lerp.frontRightMeters - this.wheelPositions.frontRightMeters,
-                wheels_lerp.rearLeftMeters - this.wheelPositions.rearLeftMeters,
-                wheels_lerp.rearRightMeters - this.wheelPositions.rearRightMeters);
-
-        // Find the new gyro angle.
-        var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
-
-        // Create a twist to represent this change based on the interpolated sensor inputs.
-        Twist2d twist = m_kinematics.toTwist2d(wheels_delta);
-        twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
-
-        return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, wheels_lerp);
-      }
-    }
-
-    @Override
-    public boolean equals(Object obj) {
-      if (this == obj) {
-        return true;
-      }
-      if (!(obj instanceof InterpolationRecord)) {
-        return false;
-      }
-      InterpolationRecord record = (InterpolationRecord) obj;
-      return Objects.equals(gyroAngle, record.gyroAngle)
-          && Objects.equals(wheelPositions, record.wheelPositions)
-          && Objects.equals(poseMeters, record.poseMeters);
-    }
-
-    @Override
-    public int hashCode() {
-      return Objects.hash(gyroAngle, wheelPositions, poseMeters);
-    }
+    super(
+        kinematics,
+        new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPoseMeters),
+        stateStdDevs,
+        visionMeasurementStdDevs);
   }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
new file mode 100644
index 0000000..bb30f25
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
@@ -0,0 +1,333 @@
+// 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.estimator;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
+import edu.wpi.first.math.kinematics.Kinematics;
+import edu.wpi.first.math.kinematics.Odometry;
+import edu.wpi.first.math.kinematics.WheelPositions;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import java.util.Map;
+import java.util.NoSuchElementException;
+import java.util.Objects;
+
+/**
+ * This class wraps {@link Odometry} to fuse latency-compensated vision measurements with encoder
+ * measurements. Robot code should not use this directly- Instead, use the particular type for your
+ * drivetrain (e.g., {@link DifferentialDrivePoseEstimator}). It is intended to be a drop-in
+ * replacement for {@link Odometry}; in fact, if you never call {@link
+ * PoseEstimator#addVisionMeasurement} and only call {@link PoseEstimator#update} then this will
+ * behave exactly the same as Odometry.
+ *
+ * <p>{@link PoseEstimator#update} should be called every robot loop.
+ *
+ * <p>{@link PoseEstimator#addVisionMeasurement} can be called as infrequently as you want; if you
+ * never call it then this class will behave exactly like regular encoder odometry.
+ */
+public class PoseEstimator<T extends WheelPositions<T>> {
+  private final Kinematics<?, T> m_kinematics;
+  private final Odometry<T> m_odometry;
+  private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
+  private final Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
+
+  private static final double kBufferDuration = 1.5;
+  private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
+      TimeInterpolatableBuffer.createBuffer(kBufferDuration);
+
+  /**
+   * Constructs a PoseEstimator.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param odometry A correctly-configured odometry object for your drivetrain.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
+   *     in meters, and heading in radians). Increase these numbers to trust your state estimate
+   *     less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
+   *     the vision pose measurement less.
+   */
+  public PoseEstimator(
+      Kinematics<?, T> kinematics,
+      Odometry<T> odometry,
+      Matrix<N3, N1> stateStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    m_kinematics = kinematics;
+    m_odometry = odometry;
+
+    for (int i = 0; i < 3; ++i) {
+      m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
+    }
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+  }
+
+  /**
+   * Sets the pose estimator's trust of global measurements. This might be used to change trust in
+   * vision measurements after the autonomous period, or to change trust as distance to a vision
+   * target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]ᵀ, with units in meters and radians.
+   */
+  public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
+    var r = new double[3];
+    for (int i = 0; i < 3; ++i) {
+      r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
+    }
+
+    // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+    // and C = I. See wpimath/algorithms.md.
+    for (int row = 0; row < 3; ++row) {
+      if (m_q.get(row, 0) == 0.0) {
+        m_visionK.set(row, row, 0.0);
+      } else {
+        m_visionK.set(
+            row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
+      }
+    }
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
+   * automatically takes care of offsetting the gyro angle.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current encoder readings.
+   * @param poseMeters The position on the field that your robot is at.
+   */
+  public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) {
+    // Reset state estimate and error covariance
+    m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters);
+    m_poseBuffer.clear();
+  }
+
+  /**
+   * Gets the estimated robot pose.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  public Pose2d getEstimatedPosition() {
+    return m_odometry.getPoseMeters();
+  }
+
+  /**
+   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+   * while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * PoseEstimator#update} every loop.
+   *
+   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
+   * recommend only adding vision measurements that are already within one meter or so of the
+   * current pose estimate.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
+   *     don't use your own time source by calling {@link
+   *     PoseEstimator#updateWithTime(double,Rotation2d,WheelPositions)} then you must use a
+   *     timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same
+   *     epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you
+   *     should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or
+   *     sync the epochs.
+   */
+  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+    // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
+    try {
+      if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
+        return;
+      }
+    } catch (NoSuchElementException ex) {
+      return;
+    }
+
+    // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
+    var sample = m_poseBuffer.getSample(timestampSeconds);
+
+    if (sample.isEmpty()) {
+      return;
+    }
+
+    // Step 2: Measure the twist between the odometry pose and the vision pose.
+    var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
+
+    // Step 3: We should not trust the twist entirely, so instead we scale this twist by a Kalman
+    // gain matrix representing how much we trust vision measurements compared to our current pose.
+    var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
+
+    // Step 4: Convert back to Twist2d.
+    var scaledTwist =
+        new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
+
+    // Step 5: Reset Odometry to state at sample with vision adjustment.
+    m_odometry.resetPosition(
+        sample.get().gyroAngle,
+        sample.get().wheelPositions,
+        sample.get().poseMeters.exp(scaledTwist));
+
+    // Step 6: Record the current pose to allow multiple measurements from the same timestamp
+    m_poseBuffer.addSample(
+        timestampSeconds,
+        new InterpolationRecord(
+            getEstimatedPosition(), sample.get().gyroAngle, sample.get().wheelPositions));
+
+    // Step 7: Replay odometry inputs between sample time and latest recorded sample to update the
+    // pose buffer and correct odometry.
+    for (Map.Entry<Double, InterpolationRecord> entry :
+        m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
+      updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().wheelPositions);
+    }
+  }
+
+  /**
+   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+   * while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * PoseEstimator#update} every loop.
+   *
+   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
+   * recommend only adding vision measurements that are already within one meter or so of the
+   * current pose estimate.
+   *
+   * <p>Note that the vision measurement standard deviations passed into this method will continue
+   * to apply to future measurements until a subsequent call to {@link
+   * PoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
+   *     don't use your own time source by calling {@link #updateWithTime}, then you must use a
+   *     timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same
+   *     epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you
+   *     should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in
+   *     this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
+   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
+   *     the vision pose measurement less.
+   */
+  public void addVisionMeasurement(
+      Pose2d visionRobotPoseMeters,
+      double timestampSeconds,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+   * loop.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The current encoder readings.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
+    return updateWithTime(MathSharedStore.getTimestamp(), gyroAngle, wheelPositions);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+   * loop.
+   *
+   * @param currentTimeSeconds Time at which this method was called, in seconds.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The current encoder readings.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, T wheelPositions) {
+    m_odometry.update(gyroAngle, wheelPositions);
+    m_poseBuffer.addSample(
+        currentTimeSeconds,
+        new InterpolationRecord(getEstimatedPosition(), gyroAngle, wheelPositions.copy()));
+
+    return getEstimatedPosition();
+  }
+
+  /**
+   * Represents an odometry record. The record contains the inputs provided as well as the pose that
+   * was observed based on these inputs, as well as the previous record and its inputs.
+   */
+  private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
+    // The pose observed given the current sensor inputs and the previous pose.
+    private final Pose2d poseMeters;
+
+    // The current gyro angle.
+    private final Rotation2d gyroAngle;
+
+    // The current encoder readings.
+    private final T wheelPositions;
+
+    /**
+     * Constructs an Interpolation Record with the specified parameters.
+     *
+     * @param poseMeters The pose observed given the current sensor inputs and the previous pose.
+     * @param gyro The current gyro angle.
+     * @param wheelPositions The current encoder readings.
+     */
+    private InterpolationRecord(Pose2d poseMeters, Rotation2d gyro, T wheelPositions) {
+      this.poseMeters = poseMeters;
+      this.gyroAngle = gyro;
+      this.wheelPositions = wheelPositions;
+    }
+
+    /**
+     * Return the interpolated record. This object is assumed to be the starting position, or lower
+     * bound.
+     *
+     * @param endValue The upper bound, or end.
+     * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
+     * @return The interpolated value.
+     */
+    @Override
+    public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
+      if (t < 0) {
+        return this;
+      } else if (t >= 1) {
+        return endValue;
+      } else {
+        // Find the new wheel distances.
+        var wheelLerp = wheelPositions.interpolate(endValue.wheelPositions, t);
+
+        // Find the new gyro angle.
+        var gyroLerp = gyroAngle.interpolate(endValue.gyroAngle, t);
+
+        // Create a twist to represent the change based on the interpolated sensor inputs.
+        Twist2d twist = m_kinematics.toTwist2d(wheelPositions, wheelLerp);
+        twist.dtheta = gyroLerp.minus(gyroAngle).getRadians();
+
+        return new InterpolationRecord(poseMeters.exp(twist), gyroLerp, wheelLerp);
+      }
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+      if (this == obj) {
+        return true;
+      }
+      if (!(obj instanceof PoseEstimator.InterpolationRecord)) {
+        return false;
+      }
+      var record = (PoseEstimator<?>.InterpolationRecord) obj;
+      return Objects.equals(gyroAngle, record.gyroAngle)
+          && Objects.equals(wheelPositions, record.wheelPositions)
+          && Objects.equals(poseMeters, record.poseMeters);
+    }
+
+    @Override
+    public int hashCode() {
+      return Objects.hash(gyroAngle, wheelPositions, poseMeters);
+    }
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
index 57451a0..f778b62 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
@@ -4,24 +4,16 @@
 
 package edu.wpi.first.math.estimator;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.util.WPIUtilJNI;
-import java.util.Arrays;
-import java.util.Map;
-import java.util.Objects;
 
 /**
  * This class wraps {@link SwerveDriveOdometry Swerve Drive Odometry} to fuse latency-compensated
@@ -33,15 +25,8 @@
  * <p>{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
  * want; if you never call it, then this class will behave as regular encoder odometry.
  */
-public class SwerveDrivePoseEstimator {
-  private final SwerveDriveKinematics m_kinematics;
-  private final SwerveDriveOdometry m_odometry;
-  private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
+public class SwerveDrivePoseEstimator extends PoseEstimator<SwerveDriveWheelPositions> {
   private final int m_numModules;
-  private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
-
-  private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
-      TimeInterpolatableBuffer.createBuffer(1.5);
 
   /**
    * Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision
@@ -91,43 +76,13 @@
       Pose2d initialPoseMeters,
       Matrix<N3, N1> stateStdDevs,
       Matrix<N3, N1> visionMeasurementStdDevs) {
-    m_kinematics = kinematics;
-    m_odometry = new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters);
-
-    for (int i = 0; i < 3; ++i) {
-      m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
-    }
+    super(
+        kinematics,
+        new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters),
+        stateStdDevs,
+        visionMeasurementStdDevs);
 
     m_numModules = modulePositions.length;
-
-    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-  }
-
-  /**
-   * Sets the pose estimator's trust of global measurements. This might be used to change trust in
-   * vision measurements after the autonomous period, or to change trust as distance to a vision
-   * target increases.
-   *
-   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
-   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
-   *     theta]ᵀ, with units in meters and radians.
-   */
-  public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
-    var r = new double[3];
-    for (int i = 0; i < 3; ++i) {
-      r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
-    }
-
-    // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
-    // and C = I. See wpimath/algorithms.md.
-    for (int row = 0; row < 3; ++row) {
-      if (m_q.get(row, 0) == 0.0) {
-        m_visionK.set(row, row, 0.0);
-      } else {
-        m_visionK.set(
-            row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
-      }
-    }
   }
 
   /**
@@ -142,106 +97,7 @@
    */
   public void resetPosition(
       Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) {
-    // Reset state estimate and error covariance
-    m_odometry.resetPosition(gyroAngle, modulePositions, poseMeters);
-    m_poseBuffer.clear();
-  }
-
-  /**
-   * Gets the estimated robot pose.
-   *
-   * @return The estimated robot pose in meters.
-   */
-  public Pose2d getEstimatedPosition() {
-    return m_odometry.getPoseMeters();
-  }
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
-   * while still accounting for measurement noise.
-   *
-   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
-   * SwerveDrivePoseEstimator#update} every loop.
-   *
-   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
-   * recommend only adding vision measurements that are already within one meter or so of the
-   * current pose estimate.
-   *
-   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
-   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
-   *     don't use your own time source by calling {@link
-   *     SwerveDrivePoseEstimator#updateWithTime(double,Rotation2d,SwerveModulePosition[])} then you
-   *     must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
-   *     the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that
-   *     you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source
-   *     or sync the epochs.
-   */
-  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
-    // Step 1: Get the pose odometry measured at the moment the vision measurement was made.
-    var sample = m_poseBuffer.getSample(timestampSeconds);
-
-    if (sample.isEmpty()) {
-      return;
-    }
-
-    // Step 2: Measure the twist between the odometry pose and the vision pose.
-    var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
-
-    // Step 3: We should not trust the twist entirely, so instead we scale this twist by a Kalman
-    // gain matrix representing how much we trust vision measurements compared to our current pose.
-    var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
-
-    // Step 4: Convert back to Twist2d.
-    var scaledTwist =
-        new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
-
-    // Step 5: Reset Odometry to state at sample with vision adjustment.
-    m_odometry.resetPosition(
-        sample.get().gyroAngle,
-        sample.get().modulePositions,
-        sample.get().poseMeters.exp(scaledTwist));
-
-    // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
-    // pose buffer and correct odometry.
-    for (Map.Entry<Double, InterpolationRecord> entry :
-        m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
-      updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().modulePositions);
-    }
-  }
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
-   * while still accounting for measurement noise.
-   *
-   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
-   * SwerveDrivePoseEstimator#update} every loop.
-   *
-   * <p>To promote stability of the pose estimate and make it robust to bad vision data, we
-   * recommend only adding vision measurements that are already within one meter or so of the
-   * current pose estimate.
-   *
-   * <p>Note that the vision measurement standard deviations passed into this method will continue
-   * to apply to future measurements until a subsequent call to {@link
-   * SwerveDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
-   *
-   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
-   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
-   *     don't use your own time source by calling {@link
-   *     SwerveDrivePoseEstimator#updateWithTime(double,Rotation2d,SwerveModulePosition[])}, then
-   *     you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
-   *     timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}).
-   *     This means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as
-   *     your time source in this case.
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
-   *     in meters, y position in meters, and heading in radians). Increase these numbers to trust
-   *     the vision pose measurement less.
-   */
-  public void addVisionMeasurement(
-      Pose2d visionRobotPoseMeters,
-      double timestampSeconds,
-      Matrix<N3, N1> visionMeasurementStdDevs) {
-    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+    resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), poseMeters);
   }
 
   /**
@@ -253,7 +109,7 @@
    * @return The estimated pose of the robot in meters.
    */
   public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
-    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, modulePositions);
+    return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions));
   }
 
   /**
@@ -267,118 +123,19 @@
    */
   public Pose2d updateWithTime(
       double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
-    if (modulePositions.length != m_numModules) {
+    return updateWithTime(
+        currentTimeSeconds, gyroAngle, new SwerveDriveWheelPositions(modulePositions));
+  }
+
+  @Override
+  public Pose2d updateWithTime(
+      double currentTimeSeconds, Rotation2d gyroAngle, SwerveDriveWheelPositions wheelPositions) {
+    if (wheelPositions.positions.length != m_numModules) {
       throw new IllegalArgumentException(
           "Number of modules is not consistent with number of wheel locations provided in "
               + "constructor");
     }
 
-    var internalModulePositions = new SwerveModulePosition[m_numModules];
-
-    for (int i = 0; i < m_numModules; i++) {
-      internalModulePositions[i] =
-          new SwerveModulePosition(modulePositions[i].distanceMeters, modulePositions[i].angle);
-    }
-
-    m_odometry.update(gyroAngle, internalModulePositions);
-
-    m_poseBuffer.addSample(
-        currentTimeSeconds,
-        new InterpolationRecord(getEstimatedPosition(), gyroAngle, internalModulePositions));
-
-    return getEstimatedPosition();
-  }
-
-  /**
-   * Represents an odometry record. The record contains the inputs provided as well as the pose that
-   * was observed based on these inputs, as well as the previous record and its inputs.
-   */
-  private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
-    // The pose observed given the current sensor inputs and the previous pose.
-    private final Pose2d poseMeters;
-
-    // The current gyro angle.
-    private final Rotation2d gyroAngle;
-
-    // The distances and rotations measured at each module.
-    private final SwerveModulePosition[] modulePositions;
-
-    /**
-     * Constructs an Interpolation Record with the specified parameters.
-     *
-     * @param pose The pose observed given the current sensor inputs and the previous pose.
-     * @param gyro The current gyro angle.
-     * @param wheelPositions The distances and rotations measured at each wheel.
-     */
-    private InterpolationRecord(
-        Pose2d poseMeters, Rotation2d gyro, SwerveModulePosition[] modulePositions) {
-      this.poseMeters = poseMeters;
-      this.gyroAngle = gyro;
-      this.modulePositions = modulePositions;
-    }
-
-    /**
-     * Return the interpolated record. This object is assumed to be the starting position, or lower
-     * bound.
-     *
-     * @param endValue The upper bound, or end.
-     * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
-     * @return The interpolated value.
-     */
-    @Override
-    public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
-      if (t < 0) {
-        return this;
-      } else if (t >= 1) {
-        return endValue;
-      } else {
-        // Find the new wheel distances.
-        var modulePositions = new SwerveModulePosition[m_numModules];
-
-        // Find the distance travelled between this measurement and the interpolated measurement.
-        var moduleDeltas = new SwerveModulePosition[m_numModules];
-
-        for (int i = 0; i < m_numModules; i++) {
-          double ds =
-              MathUtil.interpolate(
-                  this.modulePositions[i].distanceMeters,
-                  endValue.modulePositions[i].distanceMeters,
-                  t);
-          Rotation2d theta =
-              this.modulePositions[i].angle.interpolate(endValue.modulePositions[i].angle, t);
-          modulePositions[i] = new SwerveModulePosition(ds, theta);
-          moduleDeltas[i] =
-              new SwerveModulePosition(ds - this.modulePositions[i].distanceMeters, theta);
-        }
-
-        // Find the new gyro angle.
-        var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
-
-        // Create a twist to represent this change based on the interpolated sensor inputs.
-        Twist2d twist = m_kinematics.toTwist2d(moduleDeltas);
-        twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
-
-        return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, modulePositions);
-      }
-    }
-
-    @Override
-    public boolean equals(Object obj) {
-      if (this == obj) {
-        return true;
-      }
-      if (!(obj instanceof InterpolationRecord)) {
-        return false;
-      }
-      InterpolationRecord record = (InterpolationRecord) obj;
-      return Objects.equals(gyroAngle, record.gyroAngle)
-          && Arrays.equals(modulePositions, record.modulePositions)
-          && Objects.equals(poseMeters, record.poseMeters);
-    }
-
-    @Override
-    public int hashCode() {
-      return Objects.hash(gyroAngle, Arrays.hashCode(modulePositions), poseMeters);
-    }
+    return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions);
   }
 }
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 d668564..a5de856 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
@@ -64,6 +64,10 @@
   /**
    * Constructs an Unscented Kalman Filter.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param states A Nat representing the number of states.
    * @param outputs A Nat representing the number of outputs.
    * @param f A vector-valued function of x and u that returns the derivative of the state vector.
@@ -100,6 +104,10 @@
    * custom functions for arithmetic can be useful if you have angles in the state or measurements,
    * because they allow you to correctly account for the modular nature of angle arithmetic.
    *
+   * <p>See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param states A Nat representing the number of states.
    * @param outputs A Nat representing the number of outputs.
    * @param f A vector-valued function of x and u that returns the derivative of the state vector.
@@ -331,7 +339,7 @@
     // Discretize Q before projecting mean and covariance forward
     Matrix<States, States> contA =
         NumericalJacobian.numericalJacobianX(m_states, m_states, m_f, m_xHat, u);
-    var discQ = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds).getSecond();
+    var discQ = Discretization.discretizeAQ(contA, m_contQ, dtSeconds).getSecond();
     var squareRootDiscQ = discQ.lltDecompose(true);
 
     var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
@@ -373,6 +381,19 @@
   /**
    * Correct the state estimate x-hat using the measurements in y.
    *
+   * <p>This is useful for when the measurement noise covariances vary.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y, Matrix<Outputs, Outputs> R) {
+    correct(m_outputs, u, y, m_h, R, m_meanFuncY, m_residualFuncY, m_residualFuncX, m_addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
    * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
    * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
    * of this function).
@@ -382,7 +403,7 @@
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    * @param h A vector-valued function of x and u that returns the measurement vector.
-   * @param R Measurement noise covariance matrix (continuous-time).
+   * @param R Continuous measurement noise covariance matrix.
    */
   public <R extends Num> void correct(
       Nat<R> rows,
@@ -411,7 +432,7 @@
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    * @param h A vector-valued function of x and u that returns the measurement vector.
-   * @param R Measurement noise covariance matrix (continuous-time).
+   * @param R Continuous measurement noise covariance matrix.
    * @param meanFuncY A function that computes the mean of 2 * States + 1 measurement vectors using
    *     a given set of weights.
    * @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
index 8da45e9..3472198 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
@@ -4,7 +4,7 @@
 
 package edu.wpi.first.math.filter;
 
-import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.math.MathSharedStore;
 
 /**
  * A simple debounce filter for boolean streams. Requires that the boolean change value from
@@ -60,11 +60,11 @@
   }
 
   private void resetTimer() {
-    m_prevTimeSeconds = WPIUtilJNI.now() * 1e-6;
+    m_prevTimeSeconds = MathSharedStore.getTimestamp();
   }
 
   private boolean hasElapsed() {
-    return (WPIUtilJNI.now() * 1e-6) - m_prevTimeSeconds >= m_debounceTimeSeconds;
+    return MathSharedStore.getTimestamp() - m_prevTimeSeconds >= m_debounceTimeSeconds;
   }
 
   /**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
index 668b8b1..6c34f35 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
@@ -4,8 +4,8 @@
 
 package edu.wpi.first.math.filter;
 
+import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.util.WPIUtilJNI;
 
 /**
  * A class that limits the rate of change of an input value. Useful for implementing voltage,
@@ -33,21 +33,7 @@
     m_positiveRateLimit = positiveRateLimit;
     m_negativeRateLimit = negativeRateLimit;
     m_prevVal = initialValue;
-    m_prevTime = WPIUtilJNI.now() * 1e-6;
-  }
-
-  /**
-   * Creates a new SlewRateLimiter with the given positive rate limit and negative rate limit of
-   * -rateLimit and initial value.
-   *
-   * @param rateLimit The rate-of-change limit, in units per second.
-   * @param initalValue The initial value of the input.
-   * @deprecated Use SlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double
-   *     initalValue) instead.
-   */
-  @Deprecated(since = "2023", forRemoval = true)
-  public SlewRateLimiter(double rateLimit, double initalValue) {
-    this(rateLimit, -rateLimit, initalValue);
+    m_prevTime = MathSharedStore.getTimestamp();
   }
 
   /**
@@ -67,7 +53,7 @@
    * @return The filtered value, which will not change faster than the slew rate.
    */
   public double calculate(double input) {
-    double currentTime = WPIUtilJNI.now() * 1e-6;
+    double currentTime = MathSharedStore.getTimestamp();
     double elapsedTime = currentTime - m_prevTime;
     m_prevVal +=
         MathUtil.clamp(
@@ -85,6 +71,6 @@
    */
   public void reset(double value) {
     m_prevVal = value;
-    m_prevTime = WPIUtilJNI.now() * 1e-6;
+    m_prevTime = MathSharedStore.getTimestamp();
   }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java
index 5733177..9e827c3 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java
@@ -124,7 +124,9 @@
    */
   public static Transform3d convert(
       Transform3d transform, CoordinateSystem from, CoordinateSystem to) {
+    var coordRot = from.m_rotation.minus(to.m_rotation);
     return new Transform3d(
-        convert(transform.getTranslation(), from, to), convert(transform.getRotation(), from, to));
+        convert(transform.getTranslation(), from, to),
+        coordRot.unaryMinus().plus(transform.getRotation().rotateBy(coordRot)));
   }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
index bce832e..bc7f536 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
@@ -9,7 +9,15 @@
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.List;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /** Represents a 2D pose containing translational and rotational elements. */
 @JsonIgnoreProperties(ignoreUnknown = true)
@@ -136,6 +144,16 @@
   }
 
   /**
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose2d rotateBy(Rotation2d other) {
+    return new Pose2d(m_translation.rotateBy(other), m_rotation.rotateBy(other));
+  }
+
+  /**
    * Transforms the pose by the given transformation and returns the new pose. See + operator for
    * the matrix multiplication performed.
    *
@@ -238,6 +256,23 @@
     return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
   }
 
+  /**
+   * Returns the nearest Pose2d from a list of poses. If two or more poses in the list have the same
+   * distance from this pose, return the one with the closest rotation component.
+   *
+   * @param poses The list of poses to find the nearest.
+   * @return The nearest Pose2d from the list.
+   */
+  public Pose2d nearest(List<Pose2d> poses) {
+    return Collections.min(
+        poses,
+        Comparator.comparing(
+                (Pose2d other) -> this.getTranslation().getDistance(other.getTranslation()))
+            .thenComparing(
+                (Pose2d other) ->
+                    Math.abs(this.getRotation().minus(other.getRotation()).getRadians())));
+  }
+
   @Override
   public String toString() {
     return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
@@ -275,4 +310,83 @@
       return this.exp(scaledTwist);
     }
   }
+
+  public static final class AStruct implements Struct<Pose2d> {
+    @Override
+    public Class<Pose2d> getTypeClass() {
+      return Pose2d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Pose2d";
+    }
+
+    @Override
+    public int getSize() {
+      return Translation2d.struct.getSize() + Rotation2d.struct.getSize();
+    }
+
+    @Override
+    public String getSchema() {
+      return "Translation2d translation;Rotation2d rotation";
+    }
+
+    @Override
+    public Struct<?>[] getNested() {
+      return new Struct<?>[] {Translation2d.struct, Rotation2d.struct};
+    }
+
+    @Override
+    public Pose2d unpack(ByteBuffer bb) {
+      Translation2d translation = Translation2d.struct.unpack(bb);
+      Rotation2d rotation = Rotation2d.struct.unpack(bb);
+      return new Pose2d(translation, rotation);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Pose2d value) {
+      Translation2d.struct.pack(bb, value.m_translation);
+      Rotation2d.struct.pack(bb, value.m_rotation);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Pose2d, ProtobufPose2d> {
+    @Override
+    public Class<Pose2d> getTypeClass() {
+      return Pose2d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufPose2d.getDescriptor();
+    }
+
+    @Override
+    public Protobuf<?, ?>[] getNested() {
+      return new Protobuf<?, ?>[] {Translation2d.proto, Rotation2d.proto};
+    }
+
+    @Override
+    public ProtobufPose2d createMessage() {
+      return ProtobufPose2d.newInstance();
+    }
+
+    @Override
+    public Pose2d unpack(ProtobufPose2d msg) {
+      return new Pose2d(
+          Translation2d.proto.unpack(msg.getTranslation()),
+          Rotation2d.proto.unpack(msg.getRotation()));
+    }
+
+    @Override
+    public void pack(ProtobufPose2d msg, Pose2d value) {
+      Translation2d.proto.pack(msg.getMutableTranslation(), value.m_translation);
+      Rotation2d.proto.pack(msg.getMutableRotation(), value.m_rotation);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
index 8e3a3fe..2ea7094 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
@@ -8,14 +8,14 @@
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
-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.Vector;
+import edu.wpi.first.math.WPIMathJNI;
 import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /** Represents a 3D pose containing translational and rotational elements. */
 @JsonIgnoreProperties(ignoreUnknown = true)
@@ -68,7 +68,10 @@
   }
 
   /**
-   * Transforms the pose by the given transformation and returns the new transformed pose.
+   * Transforms the pose by the given transformation and returns the new transformed pose. The
+   * transform is applied relative to the pose's frame. Note that this differs from {@link
+   * Pose3d#rotateBy(Rotation3d)}, which is applied relative to the global frame and around the
+   * origin.
    *
    * @param other The transform to transform the pose by.
    * @return The transformed pose.
@@ -156,8 +159,21 @@
   }
 
   /**
-   * Transforms the pose by the given transformation and returns the new pose. See + operator for
-   * the matrix multiplication performed.
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by, which is applied extrinsically (from the
+   *     global frame).
+   * @return The rotated pose.
+   */
+  public Pose3d rotateBy(Rotation3d other) {
+    return new Pose3d(m_translation.rotateBy(other), m_rotation.rotateBy(other));
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new transformed pose. The
+   * transform is applied relative to the pose's frame. Note that this differs from {@link
+   * Pose3d#rotateBy(Rotation3d)}, which is applied relative to the global frame and around the
+   * origin.
    *
    * @param other The transform to transform the pose by.
    * @return The transformed pose.
@@ -200,36 +216,28 @@
    * @return The new pose of the robot.
    */
   public Pose3d exp(Twist3d twist) {
-    final var Omega = rotationVectorToMatrix(VecBuilder.fill(twist.rx, twist.ry, twist.rz));
-    final var OmegaSq = Omega.times(Omega);
-
-    double thetaSq = twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz;
-
-    // Get left Jacobian of SO3. See first line in right column of
-    // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
-    Matrix<N3, N3> J;
-    if (thetaSq < 1E-9 * 1E-9) {
-      // J = I + 0.5ω
-      J = Matrix.eye(Nat.N3()).plus(Omega.times(0.5));
-    } else {
-      double theta = Math.sqrt(thetaSq);
-      // J = I + (1 − cos(θ))/θ² ω + (θ − sin(θ))/θ³ ω²
-      J =
-          Matrix.eye(Nat.N3())
-              .plus(Omega.times((1.0 - Math.cos(theta)) / thetaSq))
-              .plus(OmegaSq.times((theta - Math.sin(theta)) / (thetaSq * theta)));
-    }
-
-    // Get translation component
-    final var translation =
-        J.times(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(twist.dx, twist.dy, twist.dz));
-
-    final var transform =
-        new Transform3d(
-            new Translation3d(translation.get(0, 0), translation.get(1, 0), translation.get(2, 0)),
-            new Rotation3d(twist.rx, twist.ry, twist.rz));
-
-    return this.plus(transform);
+    var quaternion = this.getRotation().getQuaternion();
+    double[] resultArray =
+        WPIMathJNI.expPose3d(
+            this.getX(),
+            this.getY(),
+            this.getZ(),
+            quaternion.getW(),
+            quaternion.getX(),
+            quaternion.getY(),
+            quaternion.getZ(),
+            twist.dx,
+            twist.dy,
+            twist.dz,
+            twist.rx,
+            twist.ry,
+            twist.rz);
+    return new Pose3d(
+        resultArray[0],
+        resultArray[1],
+        resultArray[2],
+        new Rotation3d(
+            new Quaternion(resultArray[3], resultArray[4], resultArray[5], resultArray[6])));
   }
 
   /**
@@ -240,50 +248,31 @@
    * @return The twist that maps this to end.
    */
   public Twist3d log(Pose3d end) {
-    final var transform = end.relativeTo(this);
-
-    final var rotVec = transform.getRotation().getQuaternion().toRotationVector();
-
-    final var Omega = rotationVectorToMatrix(rotVec);
-    final var OmegaSq = Omega.times(Omega);
-
-    double thetaSq =
-        rotVec.get(0, 0) * rotVec.get(0, 0)
-            + rotVec.get(1, 0) * rotVec.get(1, 0)
-            + rotVec.get(2, 0) * rotVec.get(2, 0);
-
-    // Get left Jacobian inverse of SO3. See fourth line in right column of
-    // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
-    Matrix<N3, N3> Jinv;
-    if (thetaSq < 1E-9 * 1E-9) {
-      // J⁻¹ = I − 0.5ω + 1/12 ω²
-      Jinv = Matrix.eye(Nat.N3()).minus(Omega.times(0.5)).plus(OmegaSq.times(1.0 / 12.0));
-    } else {
-      double theta = Math.sqrt(thetaSq);
-      double halfTheta = 0.5 * theta;
-
-      // J⁻¹ = I − 0.5ω + (1 − 0.5θ cos(θ/2) / sin(θ/2))/θ² ω²
-      Jinv =
-          Matrix.eye(Nat.N3())
-              .minus(Omega.times(0.5))
-              .plus(
-                  OmegaSq.times(
-                      (1.0 - 0.5 * theta * Math.cos(halfTheta) / Math.sin(halfTheta)) / thetaSq));
-    }
-
-    // Get dtranslation component
-    final var dtranslation =
-        Jinv.times(
-            new MatBuilder<>(Nat.N3(), Nat.N1())
-                .fill(transform.getX(), transform.getY(), transform.getZ()));
-
+    var thisQuaternion = this.getRotation().getQuaternion();
+    var endQuaternion = end.getRotation().getQuaternion();
+    double[] resultArray =
+        WPIMathJNI.logPose3d(
+            this.getX(),
+            this.getY(),
+            this.getZ(),
+            thisQuaternion.getW(),
+            thisQuaternion.getX(),
+            thisQuaternion.getY(),
+            thisQuaternion.getZ(),
+            end.getX(),
+            end.getY(),
+            end.getZ(),
+            endQuaternion.getW(),
+            endQuaternion.getX(),
+            endQuaternion.getY(),
+            endQuaternion.getZ());
     return new Twist3d(
-        dtranslation.get(0, 0),
-        dtranslation.get(1, 0),
-        dtranslation.get(2, 0),
-        rotVec.get(0, 0),
-        rotVec.get(1, 0),
-        rotVec.get(2, 0));
+        resultArray[0],
+        resultArray[1],
+        resultArray[2],
+        resultArray[3],
+        resultArray[4],
+        resultArray[5]);
   }
 
   /**
@@ -335,30 +324,82 @@
     }
   }
 
-  /**
-   * Applies the hat operator to a rotation vector.
-   *
-   * <p>It takes a rotation vector and returns the corresponding matrix representation of the Lie
-   * algebra element (a 3x3 rotation matrix).
-   *
-   * @param rotation The rotation vector.
-   * @return The rotation vector as a 3x3 rotation matrix.
-   */
-  private Matrix<N3, N3> rotationVectorToMatrix(Vector<N3> rotation) {
-    // Given a rotation vector <a, b, c>,
-    //         [ 0 -c  b]
-    // Omega = [ c  0 -a]
-    //         [-b  a  0]
-    return new MatBuilder<>(Nat.N3(), Nat.N3())
-        .fill(
-            0.0,
-            -rotation.get(2, 0),
-            rotation.get(1, 0),
-            rotation.get(2, 0),
-            0.0,
-            -rotation.get(0, 0),
-            -rotation.get(1, 0),
-            rotation.get(0, 0),
-            0.0);
+  public static final class AStruct implements Struct<Pose3d> {
+    @Override
+    public Class<Pose3d> getTypeClass() {
+      return Pose3d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Pose3d";
+    }
+
+    @Override
+    public int getSize() {
+      return Translation3d.struct.getSize() + Rotation3d.struct.getSize();
+    }
+
+    @Override
+    public String getSchema() {
+      return "Translation3d translation;Rotation3d rotation";
+    }
+
+    @Override
+    public Struct<?>[] getNested() {
+      return new Struct<?>[] {Translation3d.struct, Rotation3d.struct};
+    }
+
+    @Override
+    public Pose3d unpack(ByteBuffer bb) {
+      Translation3d translation = Translation3d.struct.unpack(bb);
+      Rotation3d rotation = Rotation3d.struct.unpack(bb);
+      return new Pose3d(translation, rotation);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Pose3d value) {
+      Translation3d.struct.pack(bb, value.m_translation);
+      Rotation3d.struct.pack(bb, value.m_rotation);
+    }
   }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Pose3d, ProtobufPose3d> {
+    @Override
+    public Class<Pose3d> getTypeClass() {
+      return Pose3d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufPose3d.getDescriptor();
+    }
+
+    @Override
+    public Protobuf<?, ?>[] getNested() {
+      return new Protobuf<?, ?>[] {Translation3d.proto, Rotation3d.proto};
+    }
+
+    @Override
+    public ProtobufPose3d createMessage() {
+      return ProtobufPose3d.newInstance();
+    }
+
+    @Override
+    public Pose3d unpack(ProtobufPose3d msg) {
+      return new Pose3d(
+          Translation3d.proto.unpack(msg.getTranslation()),
+          Rotation3d.proto.unpack(msg.getRotation()));
+    }
+
+    @Override
+    public void pack(ProtobufPose3d msg, Pose3d value) {
+      Translation3d.proto.pack(msg.getMutableTranslation(), value.m_translation);
+      Rotation3d.proto.pack(msg.getMutableRotation(), value.m_rotation);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
index cadfaa4..23fd26b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
@@ -11,18 +11,30 @@
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Quaternion {
-  private final double m_r;
-  private final Vector<N3> m_v;
+  // Scalar r in versor form
+  private final double m_w;
+
+  // Vector v in versor form
+  private final double m_x;
+  private final double m_y;
+  private final double m_z;
 
   /** Constructs a quaternion with a default angle of 0 degrees. */
   public Quaternion() {
-    m_r = 1.0;
-    m_v = VecBuilder.fill(0.0, 0.0, 0.0);
+    m_w = 1.0;
+    m_x = 0.0;
+    m_y = 0.0;
+    m_z = 0.0;
   }
 
   /**
@@ -39,8 +51,52 @@
       @JsonProperty(required = true, value = "X") double x,
       @JsonProperty(required = true, value = "Y") double y,
       @JsonProperty(required = true, value = "Z") double z) {
-    m_r = w;
-    m_v = VecBuilder.fill(x, y, z);
+    m_w = w;
+    m_x = x;
+    m_y = y;
+    m_z = z;
+  }
+
+  /**
+   * Adds another quaternion to this quaternion entrywise.
+   *
+   * @param other The other quaternion.
+   * @return The quaternion sum.
+   */
+  public Quaternion plus(Quaternion other) {
+    return new Quaternion(
+        getW() + other.getW(), getX() + other.getX(), getY() + other.getY(), getZ() + other.getZ());
+  }
+
+  /**
+   * Subtracts another quaternion from this quaternion entrywise.
+   *
+   * @param other The other quaternion.
+   * @return The quaternion difference.
+   */
+  public Quaternion minus(Quaternion other) {
+    return new Quaternion(
+        getW() - other.getW(), getX() - other.getX(), getY() - other.getY(), getZ() - other.getZ());
+  }
+
+  /**
+   * Divides by a scalar.
+   *
+   * @param scalar The value to scale each component by.
+   * @return The scaled quaternion.
+   */
+  public Quaternion divide(double scalar) {
+    return new Quaternion(getW() / scalar, getX() / scalar, getY() / scalar, getZ() / scalar);
+  }
+
+  /**
+   * Multiplies with a scalar.
+   *
+   * @param scalar The value to scale each component by.
+   * @return The scaled quaternion.
+   */
+  public Quaternion times(double scalar) {
+    return new Quaternion(getW() * scalar, getX() * scalar, getY() * scalar, getZ() * scalar);
   }
 
   /**
@@ -51,28 +107,29 @@
    */
   public Quaternion times(Quaternion other) {
     // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
-    final var r1 = m_r;
-    final var v1 = m_v;
-    final var r2 = other.m_r;
-    final var v2 = other.m_v;
+    final var r1 = m_w;
+    final var r2 = other.m_w;
+
+    // v₁ ⋅ v₂
+    double dot = m_x * other.m_x + m_y * other.m_y + m_z * other.m_z;
 
     // v₁ x v₂
-    var cross =
-        VecBuilder.fill(
-            v1.get(1, 0) * v2.get(2, 0) - v2.get(1, 0) * v1.get(2, 0),
-            v2.get(0, 0) * v1.get(2, 0) - v1.get(0, 0) * v2.get(2, 0),
-            v1.get(0, 0) * v2.get(1, 0) - v2.get(0, 0) * v1.get(1, 0));
+    double cross_x = m_y * other.m_z - other.m_y * m_z;
+    double cross_y = other.m_x * m_z - m_x * other.m_z;
+    double cross_z = m_x * other.m_y - other.m_x * m_y;
 
-    // v = r₁v₂ + r₂v₁ + v₁ x v₂
-    final var v = v2.times(r1).plus(v1.times(r2)).plus(cross);
-
-    return new Quaternion(r1 * r2 - v1.dot(v2), v.get(0, 0), v.get(1, 0), v.get(2, 0));
+    return new Quaternion(
+        // r = r₁r₂ − v₁ ⋅ v₂
+        r1 * r2 - dot,
+        // v = r₁v₂ + r₂v₁ + v₁ x v₂
+        r1 * other.m_x + r2 * m_x + cross_x,
+        r1 * other.m_y + r2 * m_y + cross_y,
+        r1 * other.m_z + r2 * m_z + cross_z);
   }
 
   @Override
   public String toString() {
-    return String.format(
-        "Quaternion(%s, %s, %s, %s)", m_r, m_v.get(0, 0), m_v.get(1, 0), m_v.get(2, 0));
+    return String.format("Quaternion(%s, %s, %s, %s)", getW(), getX(), getY(), getZ());
   }
 
   /**
@@ -86,14 +143,37 @@
     if (obj instanceof Quaternion) {
       var other = (Quaternion) obj;
 
-      return Math.abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
+      return Math.abs(dot(other) - norm() * other.norm()) < 1e-9
+          && Math.abs(norm() - other.norm()) < 1e-9;
     }
     return false;
   }
 
   @Override
   public int hashCode() {
-    return Objects.hash(m_r, m_v);
+    return Objects.hash(m_w, m_x, m_y, m_z);
+  }
+
+  /**
+   * Returns the conjugate of the quaternion.
+   *
+   * @return The conjugate quaternion.
+   */
+  public Quaternion conjugate() {
+    return new Quaternion(getW(), -getX(), -getY(), -getZ());
+  }
+
+  /**
+   * Returns the elementwise product of two quaternions.
+   *
+   * @param other The other quaternion.
+   * @return The dot product of two quaternions.
+   */
+  public double dot(final Quaternion other) {
+    return getW() * other.getW()
+        + getX() * other.getX()
+        + getY() * other.getY()
+        + getZ() * other.getZ();
   }
 
   /**
@@ -102,7 +182,17 @@
    * @return The inverse quaternion.
    */
   public Quaternion inverse() {
-    return new Quaternion(m_r, -m_v.get(0, 0), -m_v.get(1, 0), -m_v.get(2, 0));
+    var norm = norm();
+    return conjugate().divide(norm * norm);
+  }
+
+  /**
+   * Calculates the L2 norm of the quaternion.
+   *
+   * @return The L2 norm.
+   */
+  public double norm() {
+    return Math.sqrt(dot(this));
   }
 
   /**
@@ -111,7 +201,7 @@
    * @return The normalized quaternion.
    */
   public Quaternion normalize() {
-    double norm = Math.sqrt(getW() * getW() + getX() * getX() + getY() * getY() + getZ() * getZ());
+    double norm = norm();
     if (norm == 0.0) {
       return new Quaternion();
     } else {
@@ -120,13 +210,111 @@
   }
 
   /**
+   * Rational power of a quaternion.
+   *
+   * @param t the power to raise this quaternion to.
+   * @return The quaternion power
+   */
+  public Quaternion pow(double t) {
+    // q^t = e^(ln(q^t)) = e^(t * ln(q))
+    return this.log().times(t).exp();
+  }
+
+  /**
+   * Matrix exponential of a quaternion.
+   *
+   * @param adjustment the "Twist" that will be applied to this quaternion.
+   * @return The quaternion product of exp(adjustment) * this
+   */
+  public Quaternion exp(Quaternion adjustment) {
+    return adjustment.exp().times(this);
+  }
+
+  /**
+   * Matrix exponential of a quaternion.
+   *
+   * <p>source: wpimath/algorithms.md
+   *
+   * <p>If this quaternion is in 𝖘𝖔(3) and you are looking for an element of SO(3), use {@link
+   * fromRotationVector}
+   *
+   * @return The Matrix exponential of this quaternion.
+   */
+  public Quaternion exp() {
+    var scalar = Math.exp(getW());
+
+    var axial_magnitude = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
+    var cosine = Math.cos(axial_magnitude);
+
+    double axial_scalar;
+
+    if (axial_magnitude < 1e-9) {
+      // Taylor series of sin(θ) / θ near θ = 0: 1 − θ²/6 + θ⁴/120 + O(n⁶)
+      var axial_magnitude_sq = axial_magnitude * axial_magnitude;
+      var axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq;
+      axial_scalar = 1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0;
+    } else {
+      axial_scalar = Math.sin(axial_magnitude) / axial_magnitude;
+    }
+
+    return new Quaternion(
+        cosine * scalar,
+        getX() * axial_scalar * scalar,
+        getY() * axial_scalar * scalar,
+        getZ() * axial_scalar * scalar);
+  }
+
+  /**
+   * Log operator of a quaternion.
+   *
+   * @param end The quaternion to map this quaternion onto.
+   * @return The "Twist" that maps this quaternion to the argument.
+   */
+  public Quaternion log(Quaternion end) {
+    return end.times(this.inverse()).log();
+  }
+
+  /**
+   * The Log operator of a general quaternion.
+   *
+   * <p>source: wpimath/algorithms.md
+   *
+   * <p>If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3), use {@link
+   * toRotationVector}
+   *
+   * @return The logarithm of this quaternion.
+   */
+  public Quaternion log() {
+    var scalar = Math.log(norm());
+
+    var v_norm = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
+
+    var s_norm = getW() / norm();
+
+    if (Math.abs(s_norm + 1) < 1e-9) {
+      return new Quaternion(scalar, -Math.PI, 0, 0);
+    }
+
+    double v_scalar;
+
+    if (v_norm < 1e-9) {
+      // Taylor series expansion of atan2(y / x) / y around y = 0 => 1/x - y²/3*x³ + O(y⁴)
+      v_scalar = 1.0 / getW() - 1.0 / 3.0 * v_norm * v_norm / (getW() * getW() * getW());
+    } else {
+      v_scalar = Math.atan2(v_norm, getW()) / v_norm;
+    }
+
+    return new Quaternion(scalar, v_scalar * getX(), v_scalar * getY(), v_scalar * getZ());
+  }
+
+  /**
    * Returns W component of the quaternion.
    *
    * @return W component of the quaternion.
    */
   @JsonProperty(value = "W")
   public double getW() {
-    return m_r;
+    return m_w;
   }
 
   /**
@@ -136,7 +324,7 @@
    */
   @JsonProperty(value = "X")
   public double getX() {
-    return m_v.get(0, 0);
+    return m_x;
   }
 
   /**
@@ -146,7 +334,7 @@
    */
   @JsonProperty(value = "Y")
   public double getY() {
-    return m_v.get(1, 0);
+    return m_y;
   }
 
   /**
@@ -156,7 +344,38 @@
    */
   @JsonProperty(value = "Z")
   public double getZ() {
-    return m_v.get(2, 0);
+    return m_z;
+  }
+
+  /**
+   * Returns the quaternion representation of this rotation vector.
+   *
+   * <p>This is also the exp operator of 𝖘𝖔(3).
+   *
+   * <p>source: wpimath/algorithms.md
+   *
+   * @param rvec The rotation vector.
+   * @return The quaternion representation of this rotation vector.
+   */
+  public static Quaternion fromRotationVector(Vector<N3> rvec) {
+    double theta = rvec.norm();
+
+    double cos = Math.cos(theta / 2);
+
+    double axial_scalar;
+
+    if (theta < 1e-9) {
+      // taylor series expansion of sin(θ/2) / θ = 1/2 - θ²/48 + O(θ⁴)
+      axial_scalar = 1.0 / 2.0 - theta * theta / 48.0;
+    } else {
+      axial_scalar = Math.sin(theta / 2) / theta;
+    }
+
+    return new Quaternion(
+        cos,
+        axial_scalar * rvec.get(0, 0),
+        axial_scalar * rvec.get(1, 0),
+        axial_scalar * rvec.get(2, 0));
   }
 
   /**
@@ -171,16 +390,89 @@
     // Sound State Representation through Encapsulation of Manifolds"
     //
     // https://arxiv.org/pdf/1107.1119.pdf
-    double norm = m_v.norm();
+    double norm = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
 
+    double coeff;
     if (norm < 1e-9) {
-      return m_v.times(2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW()));
+      coeff = 2.0 / getW() - 2.0 / 3.0 * norm * norm / (getW() * getW() * getW());
     } else {
       if (getW() < 0.0) {
-        return m_v.times(2.0 * Math.atan2(-norm, -getW()) / norm);
+        coeff = 2.0 * Math.atan2(-norm, -getW()) / norm;
       } else {
-        return m_v.times(2.0 * Math.atan2(norm, getW()) / norm);
+        coeff = 2.0 * Math.atan2(norm, getW()) / norm;
       }
     }
+
+    return VecBuilder.fill(coeff * getX(), coeff * getY(), coeff * getZ());
   }
+
+  public static final class AStruct implements Struct<Quaternion> {
+    @Override
+    public Class<Quaternion> getTypeClass() {
+      return Quaternion.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Quaternion";
+    }
+
+    @Override
+    public int getSize() {
+      return kSizeDouble * 4;
+    }
+
+    @Override
+    public String getSchema() {
+      return "double w;double x;double y;double z";
+    }
+
+    @Override
+    public Quaternion unpack(ByteBuffer bb) {
+      double w = bb.getDouble();
+      double x = bb.getDouble();
+      double y = bb.getDouble();
+      double z = bb.getDouble();
+      return new Quaternion(w, x, y, z);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Quaternion value) {
+      bb.putDouble(value.getW());
+      bb.putDouble(value.getX());
+      bb.putDouble(value.getY());
+      bb.putDouble(value.getZ());
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Quaternion, ProtobufQuaternion> {
+    @Override
+    public Class<Quaternion> getTypeClass() {
+      return Quaternion.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufQuaternion.getDescriptor();
+    }
+
+    @Override
+    public ProtobufQuaternion createMessage() {
+      return ProtobufQuaternion.newInstance();
+    }
+
+    @Override
+    public Quaternion unpack(ProtobufQuaternion msg) {
+      return new Quaternion(msg.getW(), msg.getX(), msg.getY(), msg.getZ());
+    }
+
+    @Override
+    public void pack(ProtobufQuaternion msg, Quaternion value) {
+      msg.setW(value.getW()).setX(value.getX()).setY(value.getY()).setZ(value.getZ());
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
index 5be6156..0556669 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
@@ -10,8 +10,13 @@
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /**
  * A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
@@ -256,4 +261,67 @@
   public Rotation2d interpolate(Rotation2d endValue, double t) {
     return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
   }
+
+  public static final class AStruct implements Struct<Rotation2d> {
+    @Override
+    public Class<Rotation2d> getTypeClass() {
+      return Rotation2d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Rotation2d";
+    }
+
+    @Override
+    public int getSize() {
+      return kSizeDouble;
+    }
+
+    @Override
+    public String getSchema() {
+      return "double value";
+    }
+
+    @Override
+    public Rotation2d unpack(ByteBuffer bb) {
+      return new Rotation2d(bb.getDouble());
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Rotation2d value) {
+      bb.putDouble(value.m_value);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Rotation2d, ProtobufRotation2d> {
+    @Override
+    public Class<Rotation2d> getTypeClass() {
+      return Rotation2d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufRotation2d.getDescriptor();
+    }
+
+    @Override
+    public ProtobufRotation2d createMessage() {
+      return ProtobufRotation2d.newInstance();
+    }
+
+    @Override
+    public Rotation2d unpack(ProtobufRotation2d msg) {
+      return new Rotation2d(msg.getValue());
+    }
+
+    @Override
+    public void pack(ProtobufRotation2d msg, Rotation2d value) {
+      msg.setValue(value.m_value);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
index 3226e31..b434523 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
@@ -17,17 +17,24 @@
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
 import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /** A rotation in a 3D coordinate frame represented by a quaternion. */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Rotation3d implements Interpolatable<Rotation3d> {
-  private Quaternion m_q = new Quaternion();
+  private final Quaternion m_q;
 
   /** Constructs a Rotation3d with a default angle of 0 degrees. */
-  public Rotation3d() {}
+  public Rotation3d() {
+    m_q = new Quaternion();
+  }
 
   /**
    * Constructs a Rotation3d from a quaternion.
@@ -73,6 +80,17 @@
   }
 
   /**
+   * Constructs a Rotation3d with the given rotation vector representation. This representation is
+   * equivalent to axis-angle, where the normalized axis is multiplied by the rotation around the
+   * axis in radians.
+   *
+   * @param rvec The rotation vector.
+   */
+  public Rotation3d(Vector<N3> rvec) {
+    this(rvec, rvec.norm());
+  }
+
+  /**
    * Constructs a Rotation3d with the given axis-angle representation. The axis doesn't have to be
    * normalized.
    *
@@ -82,6 +100,7 @@
   public Rotation3d(Vector<N3> axis, double angleRadians) {
     double norm = axis.norm();
     if (norm == 0.0) {
+      m_q = new Quaternion();
       return;
     }
 
@@ -175,6 +194,7 @@
     if (dotNorm > 1.0 - 1E-9) {
       // If the dot product is 1, the two vectors point in the same direction so
       // there's no rotation. The default initialization of m_q will work.
+      m_q = new Quaternion();
       return;
     } else if (dotNorm < -1.0 + 1E-9) {
       // If the dot product is -1, the two vectors point in opposite directions
@@ -267,9 +287,14 @@
   }
 
   /**
-   * Adds the new rotation to the current rotation.
+   * Adds the new rotation to the current rotation. The other rotation is applied extrinsically,
+   * which means that it rotates around the global axes. For example, {@code new
+   * Rotation3d(Units.degreesToRadians(90), 0, 0).rotateBy(new Rotation3d(0,
+   * Units.degreesToRadians(45), 0))} rotates by 90 degrees around the +X axis and then by 45
+   * degrees around the global +Y axis. (This is equivalent to {@code new
+   * Rotation3d(Units.degreesToRadians(90), Units.degreesToRadians(45), 0)})
    *
-   * @param other The rotation to rotate by.
+   * @param other The extrinsic rotation to rotate by.
    * @return The new rotated Rotation3d.
    */
   public Rotation3d rotateBy(Rotation3d other) {
@@ -297,8 +322,15 @@
     final var y = m_q.getY();
     final var z = m_q.getZ();
 
-    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
-    return Math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y));
+    // wpimath/algorithms.md
+    final var cxcy = 1.0 - 2.0 * (x * x + y * y);
+    final var sxcy = 2.0 * (w * x + y * z);
+    final var cy_sq = cxcy * cxcy + sxcy * sxcy;
+    if (cy_sq > 1e-20) {
+      return Math.atan2(sxcy, cxcy);
+    } else {
+      return 0.0;
+    }
   }
 
   /**
@@ -312,7 +344,7 @@
     final var y = m_q.getY();
     final var z = m_q.getZ();
 
-    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
     double ratio = 2.0 * (w * y - z * x);
     if (Math.abs(ratio) >= 1.0) {
       return Math.copySign(Math.PI / 2.0, ratio);
@@ -332,8 +364,15 @@
     final var y = m_q.getY();
     final var z = m_q.getZ();
 
-    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
-    return Math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
+    // wpimath/algorithms.md
+    final var cycz = 1.0 - 2.0 * (y * y + z * z);
+    final var cysz = 2.0 * (w * z + x * y);
+    final var cy_sq = cycz * cycz + cysz * cysz;
+    if (cy_sq > 1e-20) {
+      return Math.atan2(cysz, cycz);
+    } else {
+      return Math.atan2(2.0 * w * z, w * w - z * z);
+    }
   }
 
   /**
@@ -386,7 +425,7 @@
   public boolean equals(Object obj) {
     if (obj instanceof Rotation3d) {
       var other = (Rotation3d) obj;
-      return m_q.equals(other.m_q);
+      return Math.abs(Math.abs(m_q.dot(other.m_q)) - m_q.norm() * other.m_q.norm()) < 1e-9;
     }
     return false;
   }
@@ -400,4 +439,77 @@
   public Rotation3d interpolate(Rotation3d endValue, double t) {
     return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
   }
+
+  public static final class AStruct implements Struct<Rotation3d> {
+    @Override
+    public Class<Rotation3d> getTypeClass() {
+      return Rotation3d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Rotation3d";
+    }
+
+    @Override
+    public int getSize() {
+      return Quaternion.struct.getSize();
+    }
+
+    @Override
+    public String getSchema() {
+      return "Quaternion q";
+    }
+
+    @Override
+    public Struct<?>[] getNested() {
+      return new Struct<?>[] {Quaternion.struct};
+    }
+
+    @Override
+    public Rotation3d unpack(ByteBuffer bb) {
+      return new Rotation3d(Quaternion.struct.unpack(bb));
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Rotation3d value) {
+      Quaternion.struct.pack(bb, value.m_q);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Rotation3d, ProtobufRotation3d> {
+    @Override
+    public Class<Rotation3d> getTypeClass() {
+      return Rotation3d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufRotation3d.getDescriptor();
+    }
+
+    @Override
+    public Protobuf<?, ?>[] getNested() {
+      return new Protobuf<?, ?>[] {Quaternion.proto};
+    }
+
+    @Override
+    public ProtobufRotation3d createMessage() {
+      return ProtobufRotation3d.newInstance();
+    }
+
+    @Override
+    public Rotation3d unpack(ProtobufRotation3d msg) {
+      return new Rotation3d(Quaternion.proto.unpack(msg.getQ()));
+    }
+
+    @Override
+    public void pack(ProtobufRotation3d msg, Rotation3d value) {
+      Quaternion.proto.pack(msg.getMutableQ(), value.m_q);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
index c3c6b0c..c7959ba 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
@@ -4,9 +4,14 @@
 
 package edu.wpi.first.math.geometry;
 
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTransform2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
-/** Represents a transformation for a Pose2d. */
+/** Represents a transformation for a Pose2d in the pose's frame. */
 public class Transform2d {
   private final Translation2d m_translation;
   private final Rotation2d m_rotation;
@@ -40,6 +45,18 @@
     m_rotation = rotation;
   }
 
+  /**
+   * Constructs a transform with x and y translations instead of a separate Translation2d.
+   *
+   * @param x The x component of the translational component of the transform.
+   * @param y The y component of the translational component of the transform.
+   * @param rotation The rotational component of the transform.
+   */
+  public Transform2d(double x, double y, Rotation2d rotation) {
+    m_translation = new Translation2d(x, y);
+    m_rotation = rotation;
+  }
+
   /** Constructs the identity transform -- maps an initial pose to itself. */
   public Transform2d() {
     m_translation = new Translation2d();
@@ -67,7 +84,8 @@
   }
 
   /**
-   * Composes two transformations.
+   * Composes two transformations. The second transform is applied relative to the orientation of
+   * the first.
    *
    * @param other The transform to compose with this one.
    * @return The composition of the two transformations.
@@ -150,4 +168,83 @@
   public int hashCode() {
     return Objects.hash(m_translation, m_rotation);
   }
+
+  public static final class AStruct implements Struct<Transform2d> {
+    @Override
+    public Class<Transform2d> getTypeClass() {
+      return Transform2d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Transform2d";
+    }
+
+    @Override
+    public int getSize() {
+      return Translation2d.struct.getSize() + Rotation2d.struct.getSize();
+    }
+
+    @Override
+    public String getSchema() {
+      return "Translation2d translation;Rotation2d rotation";
+    }
+
+    @Override
+    public Struct<?>[] getNested() {
+      return new Struct<?>[] {Translation2d.struct, Rotation2d.struct};
+    }
+
+    @Override
+    public Transform2d unpack(ByteBuffer bb) {
+      Translation2d translation = Translation2d.struct.unpack(bb);
+      Rotation2d rotation = Rotation2d.struct.unpack(bb);
+      return new Transform2d(translation, rotation);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Transform2d value) {
+      Translation2d.struct.pack(bb, value.m_translation);
+      Rotation2d.struct.pack(bb, value.m_rotation);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Transform2d, ProtobufTransform2d> {
+    @Override
+    public Class<Transform2d> getTypeClass() {
+      return Transform2d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufTransform2d.getDescriptor();
+    }
+
+    @Override
+    public Protobuf<?, ?>[] getNested() {
+      return new Protobuf<?, ?>[] {Translation2d.proto, Rotation2d.proto};
+    }
+
+    @Override
+    public ProtobufTransform2d createMessage() {
+      return ProtobufTransform2d.newInstance();
+    }
+
+    @Override
+    public Transform2d unpack(ProtobufTransform2d msg) {
+      return new Transform2d(
+          Translation2d.proto.unpack(msg.getTranslation()),
+          Rotation2d.proto.unpack(msg.getRotation()));
+    }
+
+    @Override
+    public void pack(ProtobufTransform2d msg, Transform2d value) {
+      Translation2d.proto.pack(msg.getMutableTranslation(), value.m_translation);
+      Rotation2d.proto.pack(msg.getMutableRotation(), value.m_rotation);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
index 4920ef6..223a14b 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
@@ -4,9 +4,14 @@
 
 package edu.wpi.first.math.geometry;
 
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
-/** Represents a transformation for a Pose3d. */
+/** Represents a transformation for a Pose3d in the pose's frame. */
 public class Transform3d {
   private final Translation3d m_translation;
   private final Rotation3d m_rotation;
@@ -40,6 +45,19 @@
     m_rotation = rotation;
   }
 
+  /**
+   * Constructs a transform with x, y, and z translations instead of a separate Translation3d.
+   *
+   * @param x The x component of the translational component of the transform.
+   * @param y The y component of the translational component of the transform.
+   * @param z The z component of the translational component of the transform.
+   * @param rotation The rotational component of the transform.
+   */
+  public Transform3d(double x, double y, double z, Rotation3d rotation) {
+    m_translation = new Translation3d(x, y, z);
+    m_rotation = rotation;
+  }
+
   /** Constructs the identity transform -- maps an initial pose to itself. */
   public Transform3d() {
     m_translation = new Translation3d();
@@ -67,7 +85,8 @@
   }
 
   /**
-   * Composes two transformations.
+   * Composes two transformations. The second transform is applied relative to the orientation of
+   * the first.
    *
    * @param other The transform to compose with this one.
    * @return The composition of the two transformations.
@@ -159,4 +178,83 @@
   public int hashCode() {
     return Objects.hash(m_translation, m_rotation);
   }
+
+  public static final class AStruct implements Struct<Transform3d> {
+    @Override
+    public Class<Transform3d> getTypeClass() {
+      return Transform3d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Transform3d";
+    }
+
+    @Override
+    public int getSize() {
+      return Translation3d.struct.getSize() + Rotation3d.struct.getSize();
+    }
+
+    @Override
+    public String getSchema() {
+      return "Translation3d translation;Rotation3d rotation";
+    }
+
+    @Override
+    public Struct<?>[] getNested() {
+      return new Struct<?>[] {Translation3d.struct, Rotation3d.struct};
+    }
+
+    @Override
+    public Transform3d unpack(ByteBuffer bb) {
+      Translation3d translation = Translation3d.struct.unpack(bb);
+      Rotation3d rotation = Rotation3d.struct.unpack(bb);
+      return new Transform3d(translation, rotation);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Transform3d value) {
+      Translation3d.struct.pack(bb, value.m_translation);
+      Rotation3d.struct.pack(bb, value.m_rotation);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Transform3d, ProtobufTransform3d> {
+    @Override
+    public Class<Transform3d> getTypeClass() {
+      return Transform3d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufTransform3d.getDescriptor();
+    }
+
+    @Override
+    public Protobuf<?, ?>[] getNested() {
+      return new Protobuf<?, ?>[] {Translation3d.proto, Rotation3d.proto};
+    }
+
+    @Override
+    public ProtobufTransform3d createMessage() {
+      return ProtobufTransform3d.newInstance();
+    }
+
+    @Override
+    public Transform3d unpack(ProtobufTransform3d msg) {
+      return new Transform3d(
+          Translation3d.proto.unpack(msg.getTranslation()),
+          Rotation3d.proto.unpack(msg.getRotation()));
+    }
+
+    @Override
+    public void pack(ProtobufTransform3d msg, Transform3d value) {
+      Translation3d.proto.pack(msg.getMutableTranslation(), value.m_translation);
+      Rotation3d.proto.pack(msg.getMutableRotation(), value.m_rotation);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
index 2d57edc..96e0001 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
@@ -10,7 +10,15 @@
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.List;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /**
  * Represents a translation in 2D space. This object can be used to represent a point or a vector.
@@ -185,6 +193,16 @@
     return new Translation2d(m_x / scalar, m_y / scalar);
   }
 
+  /**
+   * Returns the nearest Translation2d from a list of translations.
+   *
+   * @param translations The list of translations.
+   * @return The nearest Translation2d from the list.
+   */
+  public Translation2d nearest(List<Translation2d> translations) {
+    return Collections.min(translations, Comparator.comparing(this::getDistance));
+  }
+
   @Override
   public String toString() {
     return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
@@ -216,4 +234,70 @@
         MathUtil.interpolate(this.getX(), endValue.getX(), t),
         MathUtil.interpolate(this.getY(), endValue.getY(), t));
   }
+
+  public static final class AStruct implements Struct<Translation2d> {
+    @Override
+    public Class<Translation2d> getTypeClass() {
+      return Translation2d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Translation2d";
+    }
+
+    @Override
+    public int getSize() {
+      return kSizeDouble * 2;
+    }
+
+    @Override
+    public String getSchema() {
+      return "double x;double y";
+    }
+
+    @Override
+    public Translation2d unpack(ByteBuffer bb) {
+      double x = bb.getDouble();
+      double y = bb.getDouble();
+      return new Translation2d(x, y);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Translation2d value) {
+      bb.putDouble(value.m_x);
+      bb.putDouble(value.m_y);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Translation2d, ProtobufTranslation2d> {
+    @Override
+    public Class<Translation2d> getTypeClass() {
+      return Translation2d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufTranslation2d.getDescriptor();
+    }
+
+    @Override
+    public ProtobufTranslation2d createMessage() {
+      return ProtobufTranslation2d.newInstance();
+    }
+
+    @Override
+    public Translation2d unpack(ProtobufTranslation2d msg) {
+      return new Translation2d(msg.getX(), msg.getY());
+    }
+
+    @Override
+    public void pack(ProtobufTranslation2d msg, Translation2d value) {
+      msg.setX(value.m_x).setY(value.m_y);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
index 810f56c..bc55f65 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
@@ -10,7 +10,12 @@
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /**
  * Represents a translation in 3D space. This object can be used to represent a point or a vector.
@@ -231,4 +236,72 @@
         MathUtil.interpolate(this.getY(), endValue.getY(), t),
         MathUtil.interpolate(this.getZ(), endValue.getZ(), t));
   }
+
+  public static final class AStruct implements Struct<Translation3d> {
+    @Override
+    public Class<Translation3d> getTypeClass() {
+      return Translation3d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Translation3d";
+    }
+
+    @Override
+    public int getSize() {
+      return kSizeDouble * 3;
+    }
+
+    @Override
+    public String getSchema() {
+      return "double x;double y;double z";
+    }
+
+    @Override
+    public Translation3d unpack(ByteBuffer bb) {
+      double x = bb.getDouble();
+      double y = bb.getDouble();
+      double z = bb.getDouble();
+      return new Translation3d(x, y, z);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Translation3d value) {
+      bb.putDouble(value.m_x);
+      bb.putDouble(value.m_y);
+      bb.putDouble(value.m_z);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Translation3d, ProtobufTranslation3d> {
+    @Override
+    public Class<Translation3d> getTypeClass() {
+      return Translation3d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufTranslation3d.getDescriptor();
+    }
+
+    @Override
+    public ProtobufTranslation3d createMessage() {
+      return ProtobufTranslation3d.newInstance();
+    }
+
+    @Override
+    public Translation3d unpack(ProtobufTranslation3d msg) {
+      return new Translation3d(msg.getX(), msg.getY(), msg.getZ());
+    }
+
+    @Override
+    public void pack(ProtobufTranslation3d msg, Translation3d value) {
+      msg.setX(value.m_x).setY(value.m_y).setZ(value.m_z);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
index be6831e..a4ae9f8 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
@@ -4,11 +4,16 @@
 
 package edu.wpi.first.math.geometry;
 
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /**
  * A change in distance along a 2D arc since the last pose update. We can use ideas from
- * differential calculus to create new Pose2d objects from a Twist2d and vise versa.
+ * differential calculus to create new Pose2d objects from a Twist2d and vice versa.
  *
  * <p>A Twist can be used to represent a difference between two poses.
  */
@@ -62,4 +67,72 @@
   public int hashCode() {
     return Objects.hash(dx, dy, dtheta);
   }
+
+  public static final class AStruct implements Struct<Twist2d> {
+    @Override
+    public Class<Twist2d> getTypeClass() {
+      return Twist2d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Twist2d";
+    }
+
+    @Override
+    public int getSize() {
+      return kSizeDouble * 3;
+    }
+
+    @Override
+    public String getSchema() {
+      return "double dx;double dy;double dtheta";
+    }
+
+    @Override
+    public Twist2d unpack(ByteBuffer bb) {
+      double dx = bb.getDouble();
+      double dy = bb.getDouble();
+      double dtheta = bb.getDouble();
+      return new Twist2d(dx, dy, dtheta);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Twist2d value) {
+      bb.putDouble(value.dx);
+      bb.putDouble(value.dy);
+      bb.putDouble(value.dtheta);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Twist2d, ProtobufTwist2d> {
+    @Override
+    public Class<Twist2d> getTypeClass() {
+      return Twist2d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufTwist2d.getDescriptor();
+    }
+
+    @Override
+    public ProtobufTwist2d createMessage() {
+      return ProtobufTwist2d.newInstance();
+    }
+
+    @Override
+    public Twist2d unpack(ProtobufTwist2d msg) {
+      return new Twist2d(msg.getDx(), msg.getDy(), msg.getDtheta());
+    }
+
+    @Override
+    public void pack(ProtobufTwist2d msg, Twist2d value) {
+      msg.setDx(value.dx).setDy(value.dy).setDtheta(value.dtheta);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
index b78505e..d08d5cf 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
@@ -4,11 +4,16 @@
 
 package edu.wpi.first.math.geometry;
 
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
 import java.util.Objects;
+import us.hebi.quickbuf.Descriptors.Descriptor;
 
 /**
  * A change in distance along a 3D arc since the last pose update. We can use ideas from
- * differential calculus to create new Pose3d objects from a Twist3d and vise versa.
+ * differential calculus to create new Pose3d objects from a Twist3d and vice versa.
  *
  * <p>A Twist can be used to represent a difference between two poses.
  */
@@ -82,4 +87,84 @@
   public int hashCode() {
     return Objects.hash(dx, dy, dz, rx, ry, rz);
   }
+
+  public static final class AStruct implements Struct<Twist3d> {
+    @Override
+    public Class<Twist3d> getTypeClass() {
+      return Twist3d.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:Twist3d";
+    }
+
+    @Override
+    public int getSize() {
+      return kSizeDouble * 6;
+    }
+
+    @Override
+    public String getSchema() {
+      return "double dx;double dy;double dz;double rx;double ry;double rz";
+    }
+
+    @Override
+    public Twist3d unpack(ByteBuffer bb) {
+      double dx = bb.getDouble();
+      double dy = bb.getDouble();
+      double dz = bb.getDouble();
+      double rx = bb.getDouble();
+      double ry = bb.getDouble();
+      double rz = bb.getDouble();
+      return new Twist3d(dx, dy, dz, rx, ry, rz);
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, Twist3d value) {
+      bb.putDouble(value.dx);
+      bb.putDouble(value.dy);
+      bb.putDouble(value.dz);
+      bb.putDouble(value.rx);
+      bb.putDouble(value.ry);
+      bb.putDouble(value.rz);
+    }
+  }
+
+  public static final AStruct struct = new AStruct();
+
+  public static final class AProto implements Protobuf<Twist3d, ProtobufTwist3d> {
+    @Override
+    public Class<Twist3d> getTypeClass() {
+      return Twist3d.class;
+    }
+
+    @Override
+    public Descriptor getDescriptor() {
+      return ProtobufTwist3d.getDescriptor();
+    }
+
+    @Override
+    public ProtobufTwist3d createMessage() {
+      return ProtobufTwist3d.newInstance();
+    }
+
+    @Override
+    public Twist3d unpack(ProtobufTwist3d msg) {
+      return new Twist3d(
+          msg.getDx(), msg.getDy(), msg.getDz(), msg.getRx(), msg.getRy(), msg.getRz());
+    }
+
+    @Override
+    public void pack(ProtobufTwist3d msg, Twist3d value) {
+      msg.setDx(value.dx)
+          .setDy(value.dy)
+          .setDz(value.dz)
+          .setRx(value.rx)
+          .setRy(value.ry)
+          .setRz(value.rz);
+    }
+  }
+
+  public static final AProto proto = new AProto();
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java
new file mode 100644
index 0000000..bcd57a3
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java
@@ -0,0 +1,15 @@
+// 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.interpolation;
+
+/**
+ * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
+ * from points that are defined. This uses linear interpolation.
+ */
+public class InterpolatingDoubleTreeMap extends InterpolatingTreeMap<Double, Double> {
+  public InterpolatingDoubleTreeMap() {
+    super(InverseInterpolator.forDouble(), Interpolator.forDouble());
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingTreeMap.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingTreeMap.java
new file mode 100644
index 0000000..22eb232
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingTreeMap.java
@@ -0,0 +1,104 @@
+// 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.interpolation;
+
+import java.util.Comparator;
+import java.util.TreeMap;
+
+/**
+ * Interpolating Tree Maps are used to get values at points that are not defined by making a guess
+ * from points that are defined. This uses linear interpolation.
+ *
+ * <p>{@code K} must implement {@link Comparable}, or a {@link Comparator} on {@code K} can be
+ * provided.
+ *
+ * @param <K> The type of keys held in this map.
+ * @param <V> The type of values held in this map.
+ */
+public class InterpolatingTreeMap<K, V> {
+  private final TreeMap<K, V> m_map;
+
+  private final InverseInterpolator<K> m_inverseInterpolator;
+  private final Interpolator<V> m_interpolator;
+
+  /**
+   * Constructs an InterpolatingTreeMap.
+   *
+   * @param inverseInterpolator Function to use for inverse interpolation of the keys.
+   * @param interpolator Function to use for interpolation of the values.
+   */
+  public InterpolatingTreeMap(
+      InverseInterpolator<K> inverseInterpolator, Interpolator<V> interpolator) {
+    m_map = new TreeMap<>();
+    m_inverseInterpolator = inverseInterpolator;
+    m_interpolator = interpolator;
+  }
+
+  /**
+   * Constructs an InterpolatingTreeMap using {@code comparator}.
+   *
+   * @param inverseInterpolator Function to use for inverse interpolation of the keys.
+   * @param interpolator Function to use for interpolation of the values.
+   * @param comparator Comparator to use on keys.
+   */
+  public InterpolatingTreeMap(
+      InverseInterpolator<K> inverseInterpolator,
+      Interpolator<V> interpolator,
+      Comparator<K> comparator) {
+    m_inverseInterpolator = inverseInterpolator;
+    m_interpolator = interpolator;
+    m_map = new TreeMap<>(comparator);
+  }
+
+  /**
+   * Inserts a key-value pair.
+   *
+   * @param key The key.
+   * @param value The value.
+   */
+  public void put(K key, V value) {
+    m_map.put(key, value);
+  }
+
+  /**
+   * Returns the value associated with a given key.
+   *
+   * <p>If there's no matching key, the value returned will be an interpolation between the keys
+   * before and after the provided one, using the {@link Interpolator} and {@link
+   * InverseInterpolator} provided.
+   *
+   * @param key The key.
+   * @return The value associated with the given key.
+   */
+  public V get(K key) {
+    V val = m_map.get(key);
+    if (val == null) {
+      K ceilingKey = m_map.ceilingKey(key);
+      K floorKey = m_map.floorKey(key);
+
+      if (ceilingKey == null && floorKey == null) {
+        return null;
+      }
+      if (ceilingKey == null) {
+        return m_map.get(floorKey);
+      }
+      if (floorKey == null) {
+        return m_map.get(ceilingKey);
+      }
+      V floor = m_map.get(floorKey);
+      V ceiling = m_map.get(ceilingKey);
+
+      return m_interpolator.interpolate(
+          floor, ceiling, m_inverseInterpolator.inverseInterpolate(floorKey, ceilingKey, key));
+    } else {
+      return val;
+    }
+  }
+
+  /** Clears the contents. */
+  public void clear() {
+    m_map.clear();
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java
new file mode 100644
index 0000000..be6d8a2
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java
@@ -0,0 +1,30 @@
+// 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.interpolation;
+
+import edu.wpi.first.math.MathUtil;
+
+/**
+ * An interpolation function that returns a value interpolated between an upper and lower bound.
+ * This behavior can be linear or nonlinear.
+ *
+ * @param <T> The type that the {@link Interpolator} will operate on.
+ */
+@FunctionalInterface
+public interface Interpolator<T> {
+  /**
+   * Perform interpolation between two values.
+   *
+   * @param startValue The value to start at.
+   * @param endValue The value to end at.
+   * @param t How far between the two values to interpolate. This should be bounded to [0, 1].
+   * @return The interpolated value.
+   */
+  T interpolate(T startValue, T endValue, double t);
+
+  static Interpolator<Double> forDouble() {
+    return MathUtil::interpolate;
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java
new file mode 100644
index 0000000..8278af3
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java
@@ -0,0 +1,30 @@
+// 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.interpolation;
+
+import edu.wpi.first.math.MathUtil;
+
+/**
+ * An inverse interpolation function which determines where within an interpolation range an object
+ * lies. This behavior can be linear or nonlinear.
+ *
+ * @param <T> The type that the {@link InverseInterpolator} will operate on.
+ */
+@FunctionalInterface
+public interface InverseInterpolator<T> {
+  /**
+   * Return where within interpolation range [0, 1] q is between startValue and endValue.
+   *
+   * @param startValue Lower part of interpolation range.
+   * @param endValue Upper part of interpolation range.
+   * @param q Query.
+   * @return Interpolant in range [0, 1].
+   */
+  double inverseInterpolate(T startValue, T endValue, T q);
+
+  static InverseInterpolator<Double> forDouble() {
+    return MathUtil::inverseInterpolate;
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
index 7e0712d..f9f20c3 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
@@ -19,11 +19,10 @@
  */
 public final class TimeInterpolatableBuffer<T> {
   private final double m_historySize;
-  private final InterpolateFunction<T> m_interpolatingFunc;
+  private final Interpolator<T> m_interpolatingFunc;
   private final NavigableMap<Double, T> m_pastSnapshots = new TreeMap<>();
 
-  private TimeInterpolatableBuffer(
-      InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
+  private TimeInterpolatableBuffer(Interpolator<T> interpolateFunction, double historySizeSeconds) {
     this.m_historySize = historySizeSeconds;
     this.m_interpolatingFunc = interpolateFunction;
   }
@@ -37,7 +36,7 @@
    * @return The new TimeInterpolatableBuffer.
    */
   public static <T> TimeInterpolatableBuffer<T> createBuffer(
-      InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
+      Interpolator<T> interpolateFunction, double historySizeSeconds) {
     return new TimeInterpolatableBuffer<>(interpolateFunction, historySizeSeconds);
   }
 
@@ -143,17 +142,4 @@
   public NavigableMap<Double, T> getInternalBuffer() {
     return m_pastSnapshots;
   }
-
-  public interface InterpolateFunction<T> {
-    /**
-     * Return the interpolated value. This object is assumed to be the starting position, or lower
-     * bound.
-     *
-     * @param start The lower bound, or start.
-     * @param end The upper bound, or end.
-     * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
-     * @return The interpolated value.
-     */
-    T interpolate(T start, T end, double t);
-  }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
index 6c98337..ffbce71 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
@@ -4,23 +4,24 @@
 
 package edu.wpi.first.math.kinematics;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 
 /**
- * Represents the speed of a robot chassis. Although this struct contains similar members compared
- * to a Twist2d, they do NOT represent the same thing. Whereas a Twist2d represents a change in pose
- * w.r.t to the robot frame of reference, this ChassisSpeeds struct represents a velocity w.r.t to
- * the robot frame of reference.
+ * Represents the speed of a robot chassis. Although this class contains similar members compared to
+ * a Twist2d, they do NOT represent the same thing. Whereas a Twist2d represents a change in pose
+ * w.r.t to the robot frame of reference, a ChassisSpeeds object represents a robot's velocity.
  *
  * <p>A strictly non-holonomic drivetrain, such as a differential drive, should never have a dy
  * component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum
  * will often have all three components.
  */
 public class ChassisSpeeds {
-  /** Represents forward velocity w.r.t the robot frame of reference. (Fwd is +) */
+  /** Velocity along the x-axis. (Fwd is +) */
   public double vxMetersPerSecond;
 
-  /** Represents sideways velocity w.r.t the robot frame of reference. (Left is +) */
+  /** Velocity along the y-axis. (Left is +) */
   public double vyMetersPerSecond;
 
   /** Represents the angular velocity of the robot frame. (CCW is +) */
@@ -44,6 +45,60 @@
   }
 
   /**
+   * Discretizes a continuous-time chassis speed.
+   *
+   * <p>This function converts a continuous-time chassis speed into a discrete-time one such that
+   * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the
+   * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt
+   * along the y-axis, and omega * dt around the z-axis).
+   *
+   * <p>This is useful for compensating for translational skew when translating and rotating a
+   * swerve drivetrain.
+   *
+   * @param vxMetersPerSecond Forward velocity.
+   * @param vyMetersPerSecond Sideways velocity.
+   * @param omegaRadiansPerSecond Angular velocity.
+   * @param dtSeconds The duration of the timestep the speeds should be applied for.
+   * @return Discretized ChassisSpeeds.
+   */
+  public static ChassisSpeeds discretize(
+      double vxMetersPerSecond,
+      double vyMetersPerSecond,
+      double omegaRadiansPerSecond,
+      double dtSeconds) {
+    var desiredDeltaPose =
+        new Pose2d(
+            vxMetersPerSecond * dtSeconds,
+            vyMetersPerSecond * dtSeconds,
+            new Rotation2d(omegaRadiansPerSecond * dtSeconds));
+    var twist = new Pose2d().log(desiredDeltaPose);
+    return new ChassisSpeeds(twist.dx / dtSeconds, twist.dy / dtSeconds, twist.dtheta / dtSeconds);
+  }
+
+  /**
+   * Discretizes a continuous-time chassis speed.
+   *
+   * <p>This function converts a continuous-time chassis speed into a discrete-time one such that
+   * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the
+   * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt
+   * along the y-axis, and omega * dt around the z-axis).
+   *
+   * <p>This is useful for compensating for translational skew when translating and rotating a
+   * swerve drivetrain.
+   *
+   * @param continuousSpeeds The continuous speeds.
+   * @param dtSeconds The duration of the timestep the speeds should be applied for.
+   * @return Discretized ChassisSpeeds.
+   */
+  public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dtSeconds) {
+    return discretize(
+        continuousSpeeds.vxMetersPerSecond,
+        continuousSpeeds.vyMetersPerSecond,
+        continuousSpeeds.omegaRadiansPerSecond,
+        dtSeconds);
+  }
+
+  /**
    * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
    * object.
    *
@@ -62,10 +117,10 @@
       double vyMetersPerSecond,
       double omegaRadiansPerSecond,
       Rotation2d robotAngle) {
-    return new ChassisSpeeds(
-        vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(),
-        -vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(),
-        omegaRadiansPerSecond);
+    // CW rotation into chassis frame
+    var rotated =
+        new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus());
+    return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond);
   }
 
   /**
@@ -89,6 +144,119 @@
         robotAngle);
   }
 
+  /**
+   * Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
+   * object.
+   *
+   * @param vxMetersPerSecond The component of speed in the x direction relative to the robot.
+   *     Positive x is towards the robot's front.
+   * @param vyMetersPerSecond The component of speed in the y direction relative to the robot.
+   *     Positive y is towards the robot's left.
+   * @param omegaRadiansPerSecond The angular rate of the robot.
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
+   *     considered to be zero when it is facing directly away from your alliance station wall.
+   *     Remember that this should be CCW positive.
+   * @return ChassisSpeeds object representing the speeds in the field's frame of reference.
+   */
+  public static ChassisSpeeds fromRobotRelativeSpeeds(
+      double vxMetersPerSecond,
+      double vyMetersPerSecond,
+      double omegaRadiansPerSecond,
+      Rotation2d robotAngle) {
+    // CCW rotation out of chassis frame
+    var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle);
+    return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond);
+  }
+
+  /**
+   * Converts a user provided robot-relative ChassisSpeeds object into a field-relative
+   * ChassisSpeeds object.
+   *
+   * @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds in the robot frame
+   *     of reference. Positive x is towards the robot's front. Positive y is towards the robot's
+   *     left.
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
+   *     considered to be zero when it is facing directly away from your alliance station wall.
+   *     Remember that this should be CCW positive.
+   * @return ChassisSpeeds object representing the speeds in the field's frame of reference.
+   */
+  public static ChassisSpeeds fromRobotRelativeSpeeds(
+      ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) {
+    return fromRobotRelativeSpeeds(
+        robotRelativeSpeeds.vxMetersPerSecond,
+        robotRelativeSpeeds.vyMetersPerSecond,
+        robotRelativeSpeeds.omegaRadiansPerSecond,
+        robotAngle);
+  }
+
+  /**
+   * Adds two ChassisSpeeds and returns the sum.
+   *
+   * <p>For example, ChassisSpeeds{1.0, 0.5, 0.75} + ChassisSpeeds{2.0, 1.5, 0.25} =
+   * ChassisSpeeds{3.0, 2.0, 1.0}
+   *
+   * @param other The ChassisSpeeds to add.
+   * @return The sum of the ChassisSpeeds.
+   */
+  public ChassisSpeeds plus(ChassisSpeeds other) {
+    return new ChassisSpeeds(
+        vxMetersPerSecond + other.vxMetersPerSecond,
+        vyMetersPerSecond + other.vyMetersPerSecond,
+        omegaRadiansPerSecond + other.omegaRadiansPerSecond);
+  }
+
+  /**
+   * Subtracts the other ChassisSpeeds from the current ChassisSpeeds and returns the difference.
+   *
+   * <p>For example, ChassisSpeeds{5.0, 4.0, 2.0} - ChassisSpeeds{1.0, 2.0, 1.0} =
+   * ChassisSpeeds{4.0, 2.0, 1.0}
+   *
+   * @param other The ChassisSpeeds to subtract.
+   * @return The difference between the two ChassisSpeeds.
+   */
+  public ChassisSpeeds minus(ChassisSpeeds other) {
+    return new ChassisSpeeds(
+        vxMetersPerSecond - other.vxMetersPerSecond,
+        vyMetersPerSecond - other.vyMetersPerSecond,
+        omegaRadiansPerSecond - other.omegaRadiansPerSecond);
+  }
+
+  /**
+   * Returns the inverse of the current ChassisSpeeds. This is equivalent to negating all components
+   * of the ChassisSpeeds.
+   *
+   * @return The inverse of the current ChassisSpeeds.
+   */
+  public ChassisSpeeds unaryMinus() {
+    return new ChassisSpeeds(-vxMetersPerSecond, -vyMetersPerSecond, -omegaRadiansPerSecond);
+  }
+
+  /**
+   * Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
+   *
+   * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} * 2 = ChassisSpeeds{4.0, 5.0, 1.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled ChassisSpeeds.
+   */
+  public ChassisSpeeds times(double scalar) {
+    return new ChassisSpeeds(
+        vxMetersPerSecond * scalar, vyMetersPerSecond * scalar, omegaRadiansPerSecond * scalar);
+  }
+
+  /**
+   * Divides the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
+   *
+   * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} / 2 = ChassisSpeeds{1.0, 1.25, 0.5}
+   *
+   * @param scalar The scalar to divide by.
+   * @return The scaled ChassisSpeeds.
+   */
+  public ChassisSpeeds div(double scalar) {
+    return new ChassisSpeeds(
+        vxMetersPerSecond / scalar, vyMetersPerSecond / scalar, omegaRadiansPerSecond / scalar);
+  }
+
   @Override
   public String toString() {
     return String.format(
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
index 0dfb016..a1fb2db 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
@@ -16,7 +16,8 @@
  * whereas forward kinematics converts left and right component velocities into a linear and angular
  * chassis speed.
  */
-public class DifferentialDriveKinematics {
+public class DifferentialDriveKinematics
+    implements Kinematics<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions> {
   public final double trackWidthMeters;
 
   /**
@@ -37,6 +38,7 @@
    * @param wheelSpeeds The left and right velocities.
    * @return The chassis speed.
    */
+  @Override
   public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
     return new ChassisSpeeds(
         (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2,
@@ -51,6 +53,7 @@
    *     chassis' speed.
    * @return The left and right velocities.
    */
+  @Override
   public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
     return new DifferentialDriveWheelSpeeds(
         chassisSpeeds.vxMetersPerSecond
@@ -59,6 +62,12 @@
             + trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
   }
 
+  @Override
+  public Twist2d toTwist2d(
+      DifferentialDriveWheelPositions start, DifferentialDriveWheelPositions end) {
+    return toTwist2d(end.leftMeters - start.leftMeters, end.rightMeters - start.rightMeters);
+  }
+
   /**
    * Performs forward kinematics to return the resulting Twist2d from the given left and right side
    * distance deltas. This method is often used for odometry -- determining the robot's position on
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
index e8f97f5..db5e8a3 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
@@ -8,7 +8,6 @@
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
 
 /**
  * Class for differential drive odometry. Odometry allows you to track the robot's position on the
@@ -20,15 +19,7 @@
  * <p>It is important that you reset your encoders to zero before using this class. Any subsequent
  * pose resets also require the encoders to be reset to zero.
  */
-public class DifferentialDriveOdometry {
-  private Pose2d m_poseMeters;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
-
-  private double m_prevLeftDistance;
-  private double m_prevRightDistance;
-
+public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPositions> {
   /**
    * Constructs a DifferentialDriveOdometry object.
    *
@@ -42,13 +33,11 @@
       double leftDistanceMeters,
       double rightDistanceMeters,
       Pose2d initialPoseMeters) {
-    m_poseMeters = initialPoseMeters;
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPoseMeters.getRotation();
-
-    m_prevLeftDistance = leftDistanceMeters;
-    m_prevRightDistance = rightDistanceMeters;
-
+    super(
+        new DifferentialDriveKinematics(1),
+        gyroAngle,
+        new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
+        initialPoseMeters);
     MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
   }
 
@@ -80,21 +69,10 @@
       double leftDistanceMeters,
       double rightDistanceMeters,
       Pose2d poseMeters) {
-    m_poseMeters = poseMeters;
-    m_previousAngle = poseMeters.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-
-    m_prevLeftDistance = leftDistanceMeters;
-    m_prevRightDistance = rightDistanceMeters;
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   *
-   * @return The pose of the robot (x and y are in meters).
-   */
-  public Pose2d getPoseMeters() {
-    return m_poseMeters;
+    super.resetPosition(
+        gyroAngle,
+        new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
+        poseMeters);
   }
 
   /**
@@ -109,22 +87,7 @@
    */
   public Pose2d update(
       Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
-    double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
-    double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
-
-    m_prevLeftDistance = leftDistanceMeters;
-    m_prevRightDistance = rightDistanceMeters;
-
-    double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var newPose =
-        m_poseMeters.exp(
-            new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
-
-    m_previousAngle = angle;
-
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-    return m_poseMeters;
+    return super.update(
+        gyroAngle, new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters));
   }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java
new file mode 100644
index 0000000..18866af
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java
@@ -0,0 +1,62 @@
+// 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.kinematics;
+
+import edu.wpi.first.math.MathUtil;
+import java.util.Objects;
+
+public class DifferentialDriveWheelPositions
+    implements WheelPositions<DifferentialDriveWheelPositions> {
+  /** Distance measured by the left side. */
+  public double leftMeters;
+
+  /** Distance measured by the right side. */
+  public double rightMeters;
+
+  /**
+   * Constructs a DifferentialDriveWheelPositions.
+   *
+   * @param leftMeters Distance measured by the left side.
+   * @param rightMeters Distance measured by the right side.
+   */
+  public DifferentialDriveWheelPositions(double leftMeters, double rightMeters) {
+    this.leftMeters = leftMeters;
+    this.rightMeters = rightMeters;
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof DifferentialDriveWheelPositions) {
+      DifferentialDriveWheelPositions other = (DifferentialDriveWheelPositions) obj;
+      return Math.abs(other.leftMeters - leftMeters) < 1E-9
+          && Math.abs(other.rightMeters - rightMeters) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(leftMeters, rightMeters);
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "DifferentialDriveWheelPositions(Left: %.2f m, Right: %.2f m", leftMeters, rightMeters);
+  }
+
+  @Override
+  public DifferentialDriveWheelPositions copy() {
+    return new DifferentialDriveWheelPositions(leftMeters, rightMeters);
+  }
+
+  @Override
+  public DifferentialDriveWheelPositions interpolate(
+      DifferentialDriveWheelPositions endValue, double t) {
+    return new DifferentialDriveWheelPositions(
+        MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t),
+        MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t));
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
index d4b235e..ec874b6 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
@@ -46,6 +46,77 @@
     }
   }
 
+  /**
+   * Adds two DifferentialDriveWheelSpeeds and returns the sum.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{1.0, 0.5} + DifferentialDriveWheelSpeeds{2.0, 1.5}
+   * = DifferentialDriveWheelSpeeds{3.0, 2.0}
+   *
+   * @param other The DifferentialDriveWheelSpeeds to add.
+   * @return The sum of the DifferentialDriveWheelSpeeds.
+   */
+  public DifferentialDriveWheelSpeeds plus(DifferentialDriveWheelSpeeds other) {
+    return new DifferentialDriveWheelSpeeds(
+        leftMetersPerSecond + other.leftMetersPerSecond,
+        rightMetersPerSecond + other.rightMetersPerSecond);
+  }
+
+  /**
+   * Subtracts the other DifferentialDriveWheelSpeeds from the current DifferentialDriveWheelSpeeds
+   * and returns the difference.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{5.0, 4.0} - DifferentialDriveWheelSpeeds{1.0, 2.0}
+   * = DifferentialDriveWheelSpeeds{4.0, 2.0}
+   *
+   * @param other The DifferentialDriveWheelSpeeds to subtract.
+   * @return The difference between the two DifferentialDriveWheelSpeeds.
+   */
+  public DifferentialDriveWheelSpeeds minus(DifferentialDriveWheelSpeeds other) {
+    return new DifferentialDriveWheelSpeeds(
+        leftMetersPerSecond - other.leftMetersPerSecond,
+        rightMetersPerSecond - other.rightMetersPerSecond);
+  }
+
+  /**
+   * Returns the inverse of the current DifferentialDriveWheelSpeeds. This is equivalent to negating
+   * all components of the DifferentialDriveWheelSpeeds.
+   *
+   * @return The inverse of the current DifferentialDriveWheelSpeeds.
+   */
+  public DifferentialDriveWheelSpeeds unaryMinus() {
+    return new DifferentialDriveWheelSpeeds(-leftMetersPerSecond, -rightMetersPerSecond);
+  }
+
+  /**
+   * Multiplies the DifferentialDriveWheelSpeeds by a scalar and returns the new
+   * DifferentialDriveWheelSpeeds.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} * 2 = DifferentialDriveWheelSpeeds{4.0,
+   * 5.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled DifferentialDriveWheelSpeeds.
+   */
+  public DifferentialDriveWheelSpeeds times(double scalar) {
+    return new DifferentialDriveWheelSpeeds(
+        leftMetersPerSecond * scalar, rightMetersPerSecond * scalar);
+  }
+
+  /**
+   * Divides the DifferentialDriveWheelSpeeds by a scalar and returns the new
+   * DifferentialDriveWheelSpeeds.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} / 2 = DifferentialDriveWheelSpeeds{1.0,
+   * 1.25}
+   *
+   * @param scalar The scalar to divide by.
+   * @return The scaled DifferentialDriveWheelSpeeds.
+   */
+  public DifferentialDriveWheelSpeeds div(double scalar) {
+    return new DifferentialDriveWheelSpeeds(
+        leftMetersPerSecond / scalar, rightMetersPerSecond / scalar);
+  }
+
   @Override
   public String toString() {
     return String.format(
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java
new file mode 100644
index 0000000..35e2642
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java
@@ -0,0 +1,47 @@
+// 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.kinematics;
+
+import edu.wpi.first.math.geometry.Twist2d;
+
+/**
+ * Helper class that converts a chassis velocity (dx and dtheta components) into wheel speeds. Robot
+ * code should not use this directly- Instead, use the particular type for your drivetrain (e.g.,
+ * {@link DifferentialDriveKinematics}).
+ *
+ * @param <S> The type of the wheel speeds.
+ * @param <P> The type of the wheel positions.
+ */
+public interface Kinematics<S, P> {
+  /**
+   * Performs forward kinematics to return the resulting chassis speed from the wheel speeds. This
+   * method is often used for odometry -- determining the robot's position on the field using data
+   * from the real-world speed of each wheel on the robot.
+   *
+   * @param wheelSpeeds The speeds of the wheels.
+   * @return The chassis speed.
+   */
+  ChassisSpeeds toChassisSpeeds(S wheelSpeeds);
+
+  /**
+   * Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
+   * method is often used to convert joystick values into wheel speeds.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @return The wheel speeds.
+   */
+  S toWheelSpeeds(ChassisSpeeds chassisSpeeds);
+
+  /**
+   * Performs forward kinematics to return the resulting Twist2d from the given change in wheel
+   * positions. This method is often used for odometry -- determining the robot's position on the
+   * field using changes in the distance driven by each wheel on the robot.
+   *
+   * @param start The starting distances driven by the wheels.
+   * @param end The ending distances driven by the wheels.
+   * @return The resulting Twist2d in the robot's movement.
+   */
+  Twist2d toTwist2d(P start, P end);
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
index 23204d4..76c857a 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
@@ -30,7 +30,8 @@
  * <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
  * field using encoders and a gyro.
  */
-public class MecanumDriveKinematics {
+public class MecanumDriveKinematics
+    implements Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
   private final SimpleMatrix m_inverseKinematics;
   private final SimpleMatrix m_forwardKinematics;
 
@@ -125,6 +126,7 @@
    * @param chassisSpeeds The desired chassis speed.
    * @return The wheel speeds.
    */
+  @Override
   public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
     return toWheelSpeeds(chassisSpeeds, new Translation2d());
   }
@@ -137,6 +139,7 @@
    * @param wheelSpeeds The current mecanum drive wheel speeds.
    * @return The resulting chassis speed.
    */
+  @Override
   public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
     var wheelSpeedsVector = new SimpleMatrix(4, 1);
     wheelSpeedsVector.setColumn(
@@ -154,6 +157,20 @@
         chassisSpeedsVector.get(2, 0));
   }
 
+  @Override
+  public Twist2d toTwist2d(MecanumDriveWheelPositions start, MecanumDriveWheelPositions end) {
+    var wheelDeltasVector = new SimpleMatrix(4, 1);
+    wheelDeltasVector.setColumn(
+        0,
+        0,
+        end.frontLeftMeters - start.frontLeftMeters,
+        end.frontRightMeters - start.frontRightMeters,
+        end.rearLeftMeters - start.rearLeftMeters,
+        end.rearRightMeters - start.rearRightMeters);
+    var twist = m_forwardKinematics.mult(wheelDeltasVector);
+    return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0));
+  }
+
   /**
    * Performs forward kinematics to return the resulting Twist2d from the given wheel deltas. This
    * method is often used for odometry -- determining the robot's position on the field using
@@ -172,7 +189,6 @@
         wheelDeltas.rearLeftMeters,
         wheelDeltas.rearRightMeters);
     var twist = m_forwardKinematics.mult(wheelDeltasVector);
-
     return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0));
   }
 
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
index 40a77e2..32bc9bf 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
@@ -16,14 +16,7 @@
  * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
  * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
  */
-public class MecanumDriveOdometry {
-  private final MecanumDriveKinematics m_kinematics;
-  private Pose2d m_poseMeters;
-  private MecanumDriveWheelPositions m_previousWheelPositions;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
-
+public class MecanumDriveOdometry extends Odometry<MecanumDriveWheelPositions> {
   /**
    * Constructs a MecanumDriveOdometry object.
    *
@@ -37,16 +30,7 @@
       Rotation2d gyroAngle,
       MecanumDriveWheelPositions wheelPositions,
       Pose2d initialPoseMeters) {
-    m_kinematics = kinematics;
-    m_poseMeters = initialPoseMeters;
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPoseMeters.getRotation();
-    m_previousWheelPositions =
-        new MecanumDriveWheelPositions(
-            wheelPositions.frontLeftMeters,
-            wheelPositions.frontRightMeters,
-            wheelPositions.rearLeftMeters,
-            wheelPositions.rearRightMeters);
+    super(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
     MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
   }
 
@@ -63,72 +47,4 @@
       MecanumDriveWheelPositions wheelPositions) {
     this(kinematics, gyroAngle, wheelPositions, new Pose2d());
   }
-
-  /**
-   * Resets the robot's position on the field.
-   *
-   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
-   * automatically takes care of offsetting the gyro angle.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelPositions The distances driven by each wheel.
-   * @param poseMeters The position on the field that your robot is at.
-   */
-  public void resetPosition(
-      Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
-    m_poseMeters = poseMeters;
-    m_previousAngle = poseMeters.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousWheelPositions =
-        new MecanumDriveWheelPositions(
-            wheelPositions.frontLeftMeters,
-            wheelPositions.frontRightMeters,
-            wheelPositions.rearLeftMeters,
-            wheelPositions.rearRightMeters);
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   *
-   * @return The pose of the robot (x and y are in meters).
-   */
-  public Pose2d getPoseMeters() {
-    return m_poseMeters;
-  }
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and integration of the pose
-   * over time. This method takes in an angle parameter which is used instead of the angular rate
-   * that is calculated from forward kinematics, in addition to the current distance measurement at
-   * each wheel.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelPositions The distances driven by each wheel.
-   * @return The new pose of the robot.
-   */
-  public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var wheelDeltas =
-        new MecanumDriveWheelPositions(
-            wheelPositions.frontLeftMeters - m_previousWheelPositions.frontLeftMeters,
-            wheelPositions.frontRightMeters - m_previousWheelPositions.frontRightMeters,
-            wheelPositions.rearLeftMeters - m_previousWheelPositions.rearLeftMeters,
-            wheelPositions.rearRightMeters - m_previousWheelPositions.rearRightMeters);
-
-    var twist = m_kinematics.toTwist2d(wheelDeltas);
-    twist.dtheta = angle.minus(m_previousAngle).getRadians();
-    var newPose = m_poseMeters.exp(twist);
-
-    m_previousAngle = angle;
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-    m_previousWheelPositions =
-        new MecanumDriveWheelPositions(
-            wheelPositions.frontLeftMeters,
-            wheelPositions.frontRightMeters,
-            wheelPositions.rearLeftMeters,
-            wheelPositions.rearRightMeters);
-
-    return m_poseMeters;
-  }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
index 9ff3341..2b284cb 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
@@ -4,7 +4,10 @@
 
 package edu.wpi.first.math.kinematics;
 
-public class MecanumDriveWheelPositions {
+import edu.wpi.first.math.MathUtil;
+import java.util.Objects;
+
+public class MecanumDriveWheelPositions implements WheelPositions<MecanumDriveWheelPositions> {
   /** Distance measured by the front left wheel. */
   public double frontLeftMeters;
 
@@ -40,10 +43,42 @@
   }
 
   @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof MecanumDriveWheelPositions) {
+      MecanumDriveWheelPositions other = (MecanumDriveWheelPositions) obj;
+      return Math.abs(other.frontLeftMeters - frontLeftMeters) < 1E-9
+          && Math.abs(other.frontRightMeters - frontRightMeters) < 1E-9
+          && Math.abs(other.rearLeftMeters - rearLeftMeters) < 1E-9
+          && Math.abs(other.rearRightMeters - rearRightMeters) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
+  }
+
+  @Override
   public String toString() {
     return String.format(
         "MecanumDriveWheelPositions(Front Left: %.2f m, Front Right: %.2f m, "
             + "Rear Left: %.2f m, Rear Right: %.2f m)",
         frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
   }
+
+  @Override
+  public MecanumDriveWheelPositions copy() {
+    return new MecanumDriveWheelPositions(
+        frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
+  }
+
+  @Override
+  public MecanumDriveWheelPositions interpolate(MecanumDriveWheelPositions endValue, double t) {
+    return new MecanumDriveWheelPositions(
+        MathUtil.interpolate(this.frontLeftMeters, endValue.frontLeftMeters, t),
+        MathUtil.interpolate(this.frontRightMeters, endValue.frontRightMeters, t),
+        MathUtil.interpolate(this.rearLeftMeters, endValue.rearLeftMeters, t),
+        MathUtil.interpolate(this.rearRightMeters, endValue.rearRightMeters, t));
+  }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
index 1dcfc85..63cef18 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
@@ -73,6 +73,89 @@
     }
   }
 
+  /**
+   * Adds two MecanumDriveWheelSpeeds and returns the sum.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{1.0, 0.5, 2.0, 1.5} + MecanumDriveWheelSpeeds{2.0, 1.5,
+   * 0.5, 1.0} = MecanumDriveWheelSpeeds{3.0, 2.0, 2.5, 2.5}
+   *
+   * @param other The MecanumDriveWheelSpeeds to add.
+   * @return The sum of the MecanumDriveWheelSpeeds.
+   */
+  public MecanumDriveWheelSpeeds plus(MecanumDriveWheelSpeeds other) {
+    return new MecanumDriveWheelSpeeds(
+        frontLeftMetersPerSecond + other.frontLeftMetersPerSecond,
+        frontRightMetersPerSecond + other.frontRightMetersPerSecond,
+        rearLeftMetersPerSecond + other.rearLeftMetersPerSecond,
+        rearRightMetersPerSecond + other.rearRightMetersPerSecond);
+  }
+
+  /**
+   * Subtracts the other MecanumDriveWheelSpeeds from the current MecanumDriveWheelSpeeds and
+   * returns the difference.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{5.0, 4.0, 6.0, 2.5} - MecanumDriveWheelSpeeds{1.0, 2.0,
+   * 3.0, 0.5} = MecanumDriveWheelSpeeds{4.0, 2.0, 3.0, 2.0}
+   *
+   * @param other The MecanumDriveWheelSpeeds to subtract.
+   * @return The difference between the two MecanumDriveWheelSpeeds.
+   */
+  public MecanumDriveWheelSpeeds minus(MecanumDriveWheelSpeeds other) {
+    return new MecanumDriveWheelSpeeds(
+        frontLeftMetersPerSecond - other.frontLeftMetersPerSecond,
+        frontRightMetersPerSecond - other.frontRightMetersPerSecond,
+        rearLeftMetersPerSecond - other.rearLeftMetersPerSecond,
+        rearRightMetersPerSecond - other.rearRightMetersPerSecond);
+  }
+
+  /**
+   * Returns the inverse of the current MecanumDriveWheelSpeeds. This is equivalent to negating all
+   * components of the MecanumDriveWheelSpeeds.
+   *
+   * @return The inverse of the current MecanumDriveWheelSpeeds.
+   */
+  public MecanumDriveWheelSpeeds unaryMinus() {
+    return new MecanumDriveWheelSpeeds(
+        -frontLeftMetersPerSecond,
+        -frontRightMetersPerSecond,
+        -rearLeftMetersPerSecond,
+        -rearRightMetersPerSecond);
+  }
+
+  /**
+   * Multiplies the MecanumDriveWheelSpeeds by a scalar and returns the new MecanumDriveWheelSpeeds.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 3.0, 3.5} * 2 = MecanumDriveWheelSpeeds{4.0,
+   * 5.0, 6.0, 7.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled MecanumDriveWheelSpeeds.
+   */
+  public MecanumDriveWheelSpeeds times(double scalar) {
+    return new MecanumDriveWheelSpeeds(
+        frontLeftMetersPerSecond * scalar,
+        frontRightMetersPerSecond * scalar,
+        rearLeftMetersPerSecond * scalar,
+        rearRightMetersPerSecond * scalar);
+  }
+
+  /**
+   * Divides the MecanumDriveWheelSpeeds by a scalar and returns the new MecanumDriveWheelSpeeds.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 1.5, 1.0} / 2 = MecanumDriveWheelSpeeds{1.0,
+   * 1.25, 0.75, 0.5}
+   *
+   * @param scalar The scalar to divide by.
+   * @return The scaled MecanumDriveWheelSpeeds.
+   */
+  public MecanumDriveWheelSpeeds div(double scalar) {
+    return new MecanumDriveWheelSpeeds(
+        frontLeftMetersPerSecond / scalar,
+        frontRightMetersPerSecond / scalar,
+        rearLeftMetersPerSecond / scalar,
+        rearRightMetersPerSecond / scalar);
+  }
+
   @Override
   public String toString() {
     return String.format(
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java
new file mode 100644
index 0000000..b2e5054
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java
@@ -0,0 +1,97 @@
+// 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.kinematics;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+/**
+ * Class for odometry. Robot code should not use this directly- Instead, use the particular type for
+ * your drivetrain (e.g., {@link DifferentialDriveOdometry}). Odometry allows you to track the
+ * robot's position on the field over the course of a match using readings from encoders and a
+ * gyroscope.
+ *
+ * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
+ * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
+ */
+public class Odometry<T extends WheelPositions<T>> {
+  private final Kinematics<?, T> m_kinematics;
+  private Pose2d m_poseMeters;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+  private T m_previousWheelPositions;
+
+  /**
+   * Constructs an Odometry object.
+   *
+   * @param kinematics The kinematics of the drivebase.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current encoder readings.
+   * @param initialPoseMeters The starting position of the robot on the field.
+   */
+  public Odometry(
+      Kinematics<?, T> kinematics,
+      Rotation2d gyroAngle,
+      T wheelPositions,
+      Pose2d initialPoseMeters) {
+    m_kinematics = kinematics;
+    m_poseMeters = initialPoseMeters;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = m_poseMeters.getRotation();
+    m_previousWheelPositions = wheelPositions.copy();
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
+   * automatically takes care of offsetting the gyro angle.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current encoder readings.
+   * @param poseMeters The position on the field that your robot is at.
+   */
+  public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) {
+    m_poseMeters = poseMeters;
+    m_previousAngle = m_poseMeters.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousWheelPositions = wheelPositions.copy();
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   *
+   * @return The pose of the robot (x and y are in meters).
+   */
+  public Pose2d getPoseMeters() {
+    return m_poseMeters;
+  }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and integration of the pose
+   * over time. This method takes in an angle parameter which is used instead of the angular rate
+   * that is calculated from forward kinematics, in addition to the current distance measurement at
+   * each wheel.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current encoder readings.
+   * @return The new pose of the robot.
+   */
+  public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
+    var angle = gyroAngle.plus(m_gyroOffset);
+
+    var twist = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
+    twist.dtheta = angle.minus(m_previousAngle).getRadians();
+
+    var newPose = m_poseMeters.exp(twist);
+
+    m_previousWheelPositions = wheelPositions.copy();
+    m_previousAngle = angle;
+    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+
+    return m_poseMeters;
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
index 98df547..aa5338e 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
@@ -10,7 +10,6 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Twist2d;
 import java.util.Arrays;
-import java.util.Collections;
 import org.ejml.simple.SimpleMatrix;
 
 /**
@@ -33,32 +32,50 @@
  * <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
  * field using encoders and a gyro.
  */
-public class SwerveDriveKinematics {
+public class SwerveDriveKinematics
+    implements Kinematics<SwerveDriveKinematics.SwerveDriveWheelStates, SwerveDriveWheelPositions> {
+  public static class SwerveDriveWheelStates {
+    public SwerveModuleState[] states;
+
+    /**
+     * Creates a new SwerveDriveWheelStates instance.
+     *
+     * @param states The swerve module states. This will be deeply copied.
+     */
+    public SwerveDriveWheelStates(SwerveModuleState[] states) {
+      this.states = new SwerveModuleState[states.length];
+      for (int i = 0; i < states.length; i++) {
+        this.states[i] = new SwerveModuleState(states[i].speedMetersPerSecond, states[i].angle);
+      }
+    }
+  }
+
   private final SimpleMatrix m_inverseKinematics;
   private final SimpleMatrix m_forwardKinematics;
 
   private final int m_numModules;
   private final Translation2d[] m_modules;
-  private final SwerveModuleState[] m_moduleStates;
+  private Rotation2d[] m_moduleHeadings;
   private Translation2d m_prevCoR = new Translation2d();
 
   /**
-   * Constructs a swerve drive kinematics object. This takes in a variable number of wheel locations
-   * as Translation2d objects. The order in which you pass in the wheel locations is the same order
-   * that you will receive the module states when performing inverse kinematics. It is also expected
-   * that you pass in the module states in the same order when calling the forward kinematics
-   * methods.
+   * Constructs a swerve drive kinematics object. This takes in a variable number of module
+   * locations as Translation2d objects. The order in which you pass in the module locations is the
+   * same order that you will receive the module states when performing inverse kinematics. It is
+   * also expected that you pass in the module states in the same order when calling the forward
+   * kinematics methods.
    *
-   * @param wheelsMeters The locations of the wheels relative to the physical center of the robot.
+   * @param moduleTranslationsMeters The locations of the modules relative to the physical center of
+   *     the robot.
    */
-  public SwerveDriveKinematics(Translation2d... wheelsMeters) {
-    if (wheelsMeters.length < 2) {
+  public SwerveDriveKinematics(Translation2d... moduleTranslationsMeters) {
+    if (moduleTranslationsMeters.length < 2) {
       throw new IllegalArgumentException("A swerve drive requires at least two modules");
     }
-    m_numModules = wheelsMeters.length;
-    m_modules = Arrays.copyOf(wheelsMeters, m_numModules);
-    m_moduleStates = new SwerveModuleState[m_numModules];
-    Arrays.fill(m_moduleStates, new SwerveModuleState());
+    m_numModules = moduleTranslationsMeters.length;
+    m_modules = Arrays.copyOf(moduleTranslationsMeters, m_numModules);
+    m_moduleHeadings = new Rotation2d[m_numModules];
+    Arrays.fill(m_moduleHeadings, new Rotation2d());
     m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
 
     for (int i = 0; i < m_numModules; i++) {
@@ -71,6 +88,21 @@
   }
 
   /**
+   * Reset the internal swerve module headings.
+   *
+   * @param moduleHeadings The swerve module headings. The order of the module headings should be
+   *     same as passed into the constructor of this class.
+   */
+  public void resetHeadings(Rotation2d... moduleHeadings) {
+    if (moduleHeadings.length != m_numModules) {
+      throw new IllegalArgumentException(
+          "Number of headings is not consistent with number of module locations provided in "
+              + "constructor");
+    }
+    m_moduleHeadings = Arrays.copyOf(moduleHeadings, m_numModules);
+  }
+
+  /**
    * Performs inverse kinematics to return the module states from a desired chassis velocity. This
    * method is often used to convert joystick values into module speeds and angles.
    *
@@ -91,17 +123,18 @@
    *     attainable max velocity. Use the {@link #desaturateWheelSpeeds(SwerveModuleState[], double)
    *     DesaturateWheelSpeeds} function to rectify this issue.
    */
-  @SuppressWarnings("PMD.MethodReturnsInternalArray")
   public SwerveModuleState[] toSwerveModuleStates(
       ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
+    var moduleStates = new SwerveModuleState[m_numModules];
+
     if (chassisSpeeds.vxMetersPerSecond == 0.0
         && chassisSpeeds.vyMetersPerSecond == 0.0
         && chassisSpeeds.omegaRadiansPerSecond == 0.0) {
       for (int i = 0; i < m_numModules; i++) {
-        m_moduleStates[i].speedMetersPerSecond = 0.0;
+        moduleStates[i] = new SwerveModuleState(0.0, m_moduleHeadings[i]);
       }
 
-      return m_moduleStates;
+      return moduleStates;
     }
 
     if (!centerOfRotationMeters.equals(m_prevCoR)) {
@@ -139,10 +172,11 @@
       double speed = Math.hypot(x, y);
       Rotation2d angle = new Rotation2d(x, y);
 
-      m_moduleStates[i] = new SwerveModuleState(speed, angle);
+      moduleStates[i] = new SwerveModuleState(speed, angle);
+      m_moduleHeadings[i] = angle;
     }
 
-    return m_moduleStates;
+    return moduleStates;
   }
 
   /**
@@ -156,26 +190,31 @@
     return toSwerveModuleStates(chassisSpeeds, new Translation2d());
   }
 
+  @Override
+  public SwerveDriveWheelStates toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
+    return new SwerveDriveWheelStates(toSwerveModuleStates(chassisSpeeds));
+  }
+
   /**
    * Performs forward kinematics to return the resulting chassis state from the given module states.
    * This method is often used for odometry -- determining the robot's position on the field using
    * data from the real-world speed and angle of each module on the robot.
    *
-   * @param wheelStates The state of the modules (as a SwerveModuleState type) as measured from
+   * @param moduleStates The state of the modules (as a SwerveModuleState type) as measured from
    *     respective encoders and gyros. The order of the swerve module states should be same as
    *     passed into the constructor of this class.
    * @return The resulting chassis speed.
    */
-  public ChassisSpeeds toChassisSpeeds(SwerveModuleState... wheelStates) {
-    if (wheelStates.length != m_numModules) {
+  public ChassisSpeeds toChassisSpeeds(SwerveModuleState... moduleStates) {
+    if (moduleStates.length != m_numModules) {
       throw new IllegalArgumentException(
-          "Number of modules is not consistent with number of wheel locations provided in "
+          "Number of modules is not consistent with number of module locations provided in "
               + "constructor");
     }
     var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1);
 
     for (int i = 0; i < m_numModules; i++) {
-      var module = wheelStates[i];
+      var module = moduleStates[i];
       moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos());
       moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin());
     }
@@ -187,26 +226,31 @@
         chassisSpeedsVector.get(2, 0));
   }
 
+  @Override
+  public ChassisSpeeds toChassisSpeeds(SwerveDriveWheelStates wheelStates) {
+    return toChassisSpeeds(wheelStates.states);
+  }
+
   /**
    * Performs forward kinematics to return the resulting chassis state from the given module states.
    * This method is often used for odometry -- determining the robot's position on the field using
    * data from the real-world speed and angle of each module on the robot.
    *
-   * @param wheelDeltas The latest change in position of the modules (as a SwerveModulePosition
+   * @param moduleDeltas The latest change in position of the modules (as a SwerveModulePosition
    *     type) as measured from respective encoders and gyros. The order of the swerve module states
    *     should be same as passed into the constructor of this class.
    * @return The resulting Twist2d.
    */
-  public Twist2d toTwist2d(SwerveModulePosition... wheelDeltas) {
-    if (wheelDeltas.length != m_numModules) {
+  public Twist2d toTwist2d(SwerveModulePosition... moduleDeltas) {
+    if (moduleDeltas.length != m_numModules) {
       throw new IllegalArgumentException(
-          "Number of modules is not consistent with number of wheel locations provided in "
+          "Number of modules is not consistent with number of module locations provided in "
               + "constructor");
     }
     var moduleDeltaMatrix = new SimpleMatrix(m_numModules * 2, 1);
 
     for (int i = 0; i < m_numModules; i++) {
-      var module = wheelDeltas[i];
+      var module = moduleDeltas[i];
       moduleDeltaMatrix.set(i * 2, 0, module.distanceMeters * module.angle.getCos());
       moduleDeltaMatrix.set(i * 2 + 1, module.distanceMeters * module.angle.getSin());
     }
@@ -216,6 +260,22 @@
         chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0));
   }
 
+  @Override
+  public Twist2d toTwist2d(SwerveDriveWheelPositions start, SwerveDriveWheelPositions end) {
+    if (start.positions.length != end.positions.length) {
+      throw new IllegalArgumentException("Inconsistent number of modules!");
+    }
+    var newPositions = new SwerveModulePosition[start.positions.length];
+    for (int i = 0; i < start.positions.length; i++) {
+      var startModule = start.positions[i];
+      var endModule = end.positions[i];
+      newPositions[i] =
+          new SwerveModulePosition(
+              endModule.distanceMeters - startModule.distanceMeters, endModule.angle);
+    }
+    return toTwist2d(newPositions);
+  }
+
   /**
    * Renormalizes the wheel speeds if any individual speed is above the specified maximum.
    *
@@ -230,7 +290,10 @@
    */
   public static void desaturateWheelSpeeds(
       SwerveModuleState[] moduleStates, double attainableMaxSpeedMetersPerSecond) {
-    double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
+    double realMaxSpeed = 0;
+    for (SwerveModuleState moduleState : moduleStates) {
+      realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond));
+    }
     if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
       for (SwerveModuleState moduleState : moduleStates) {
         moduleState.speedMetersPerSecond =
@@ -250,7 +313,7 @@
    *
    * @param moduleStates Reference to array of module states. The array will be mutated with the
    *     normalized speeds!
-   * @param currentChassisSpeed The current speed of the robot
+   * @param desiredChassisSpeed The desired speed of the robot
    * @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach
    * @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot
    *     can reach while translating
@@ -259,11 +322,14 @@
    */
   public static void desaturateWheelSpeeds(
       SwerveModuleState[] moduleStates,
-      ChassisSpeeds currentChassisSpeed,
+      ChassisSpeeds desiredChassisSpeed,
       double attainableMaxModuleSpeedMetersPerSecond,
       double attainableMaxTranslationalSpeedMetersPerSecond,
       double attainableMaxRotationalVelocityRadiansPerSecond) {
-    double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
+    double realMaxSpeed = 0;
+    for (SwerveModuleState moduleState : moduleStates) {
+      realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond));
+    }
 
     if (attainableMaxTranslationalSpeedMetersPerSecond == 0
         || attainableMaxRotationalVelocityRadiansPerSecond == 0
@@ -271,10 +337,10 @@
       return;
     }
     double translationalK =
-        Math.hypot(currentChassisSpeed.vxMetersPerSecond, currentChassisSpeed.vyMetersPerSecond)
+        Math.hypot(desiredChassisSpeed.vxMetersPerSecond, desiredChassisSpeed.vyMetersPerSecond)
             / attainableMaxTranslationalSpeedMetersPerSecond;
     double rotationalK =
-        Math.abs(currentChassisSpeed.omegaRadiansPerSecond)
+        Math.abs(desiredChassisSpeed.omegaRadiansPerSecond)
             / attainableMaxRotationalVelocityRadiansPerSecond;
     double k = Math.max(translationalK, rotationalK);
     double scale = Math.min(k * attainableMaxModuleSpeedMetersPerSecond / realMaxSpeed, 1);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
index c2e188f..4f954e9 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
@@ -17,14 +17,8 @@
  * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
  * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
  */
-public class SwerveDriveOdometry {
-  private final SwerveDriveKinematics m_kinematics;
-  private Pose2d m_poseMeters;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
+public class SwerveDriveOdometry extends Odometry<SwerveDriveWheelPositions> {
   private final int m_numModules;
-  private SwerveModulePosition[] m_previousModulePositions;
 
   /**
    * Constructs a SwerveDriveOdometry object.
@@ -39,18 +33,9 @@
       Rotation2d gyroAngle,
       SwerveModulePosition[] modulePositions,
       Pose2d initialPose) {
-    m_kinematics = kinematics;
-    m_poseMeters = initialPose;
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPose.getRotation();
-    m_numModules = modulePositions.length;
+    super(kinematics, gyroAngle, new SwerveDriveWheelPositions(modulePositions), initialPose);
 
-    m_previousModulePositions = new SwerveModulePosition[m_numModules];
-    for (int index = 0; index < m_numModules; index++) {
-      m_previousModulePositions[index] =
-          new SwerveModulePosition(
-              modulePositions[index].distanceMeters, modulePositions[index].angle);
-    }
+    m_numModules = modulePositions.length;
 
     MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
   }
@@ -83,29 +68,18 @@
    */
   public void resetPosition(
       Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) {
-    if (modulePositions.length != m_numModules) {
+    resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), pose);
+  }
+
+  @Override
+  public void resetPosition(
+      Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions, Pose2d pose) {
+    if (modulePositions.positions.length != m_numModules) {
       throw new IllegalArgumentException(
           "Number of modules is not consistent with number of wheel locations provided in "
               + "constructor");
     }
-
-    m_poseMeters = pose;
-    m_previousAngle = pose.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    for (int index = 0; index < m_numModules; index++) {
-      m_previousModulePositions[index] =
-          new SwerveModulePosition(
-              modulePositions[index].distanceMeters, modulePositions[index].angle);
-    }
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   *
-   * @return The pose of the robot (x and y are in meters).
-   */
-  public Pose2d getPoseMeters() {
-    return m_poseMeters;
+    super.resetPosition(gyroAngle, modulePositions, pose);
   }
 
   /**
@@ -121,32 +95,16 @@
    * @return The new pose of the robot.
    */
   public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
-    if (modulePositions.length != m_numModules) {
+    return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions));
+  }
+
+  @Override
+  public Pose2d update(Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions) {
+    if (modulePositions.positions.length != m_numModules) {
       throw new IllegalArgumentException(
           "Number of modules is not consistent with number of wheel locations provided in "
               + "constructor");
     }
-
-    var moduleDeltas = new SwerveModulePosition[m_numModules];
-    for (int index = 0; index < m_numModules; index++) {
-      var current = modulePositions[index];
-      var previous = m_previousModulePositions[index];
-
-      moduleDeltas[index] =
-          new SwerveModulePosition(current.distanceMeters - previous.distanceMeters, current.angle);
-      previous.distanceMeters = current.distanceMeters;
-    }
-
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var twist = m_kinematics.toTwist2d(moduleDeltas);
-    twist.dtheta = angle.minus(m_previousAngle).getRadians();
-
-    var newPose = m_poseMeters.exp(twist);
-
-    m_previousAngle = angle;
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-
-    return m_poseMeters;
+    return super.update(gyroAngle, modulePositions);
   }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java
new file mode 100644
index 0000000..e88f044
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java
@@ -0,0 +1,61 @@
+// 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.kinematics;
+
+import java.util.Arrays;
+import java.util.Objects;
+
+public class SwerveDriveWheelPositions implements WheelPositions<SwerveDriveWheelPositions> {
+  public SwerveModulePosition[] positions;
+
+  /**
+   * Creates a new SwerveDriveWheelPositions instance.
+   *
+   * @param positions The swerve module positions. This will be deeply copied.
+   */
+  public SwerveDriveWheelPositions(SwerveModulePosition[] positions) {
+    this.positions = new SwerveModulePosition[positions.length];
+    for (int i = 0; i < positions.length; i++) {
+      this.positions[i] = positions[i].copy();
+    }
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof SwerveDriveWheelPositions) {
+      SwerveDriveWheelPositions other = (SwerveDriveWheelPositions) obj;
+      return Arrays.equals(this.positions, other.positions);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    // Cast to interpret positions as single argument, not array of the arguments
+    return Objects.hash((Object) positions);
+  }
+
+  @Override
+  public String toString() {
+    return String.format("SwerveDriveWheelPositions(%s)", Arrays.toString(positions));
+  }
+
+  @Override
+  public SwerveDriveWheelPositions copy() {
+    return new SwerveDriveWheelPositions(positions);
+  }
+
+  @Override
+  public SwerveDriveWheelPositions interpolate(SwerveDriveWheelPositions endValue, double t) {
+    if (endValue.positions.length != positions.length) {
+      throw new IllegalArgumentException("Inconsistent number of modules!");
+    }
+    var newPositions = new SwerveModulePosition[positions.length];
+    for (int i = 0; i < positions.length; i++) {
+      newPositions[i] = positions[i].interpolate(endValue.positions[i], t);
+    }
+    return new SwerveDriveWheelPositions(newPositions);
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
index cdd7834..d058764 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
@@ -4,11 +4,14 @@
 
 package edu.wpi.first.math.kinematics;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.interpolation.Interpolatable;
 import java.util.Objects;
 
 /** Represents the state of one swerve module. */
-public class SwerveModulePosition implements Comparable<SwerveModulePosition> {
+public class SwerveModulePosition
+    implements Comparable<SwerveModulePosition>, Interpolatable<SwerveModulePosition> {
   /** Distance measured by the wheel of the module. */
   public double distanceMeters;
 
@@ -32,14 +35,15 @@
   @Override
   public boolean equals(Object obj) {
     if (obj instanceof SwerveModulePosition) {
-      return Double.compare(distanceMeters, ((SwerveModulePosition) obj).distanceMeters) == 0;
+      SwerveModulePosition other = (SwerveModulePosition) obj;
+      return Math.abs(other.distanceMeters - distanceMeters) < 1E-9 && angle.equals(other.angle);
     }
     return false;
   }
 
   @Override
   public int hashCode() {
-    return Objects.hash(distanceMeters);
+    return Objects.hash(distanceMeters, angle);
   }
 
   /**
@@ -59,4 +63,20 @@
     return String.format(
         "SwerveModulePosition(Distance: %.2f m, Angle: %s)", distanceMeters, angle);
   }
+
+  /**
+   * Returns a copy of this swerve module position.
+   *
+   * @return A copy.
+   */
+  public SwerveModulePosition copy() {
+    return new SwerveModulePosition(distanceMeters, angle);
+  }
+
+  @Override
+  public SwerveModulePosition interpolate(SwerveModulePosition endValue, double t) {
+    return new SwerveModulePosition(
+        MathUtil.interpolate(this.distanceMeters, endValue.distanceMeters, t),
+        this.angle.interpolate(endValue.angle, t));
+  }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
index ec7fd9f..10dee4f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
@@ -32,15 +32,16 @@
   @Override
   public boolean equals(Object obj) {
     if (obj instanceof SwerveModuleState) {
-      return Double.compare(speedMetersPerSecond, ((SwerveModuleState) obj).speedMetersPerSecond)
-          == 0;
+      SwerveModuleState other = (SwerveModuleState) obj;
+      return Math.abs(other.speedMetersPerSecond - speedMetersPerSecond) < 1E-9
+          && angle.equals(other.angle);
     }
     return false;
   }
 
   @Override
   public int hashCode() {
-    return Objects.hash(speedMetersPerSecond);
+    return Objects.hash(speedMetersPerSecond, angle);
   }
 
   /**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java
new file mode 100644
index 0000000..029a0ac
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java
@@ -0,0 +1,16 @@
+// 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.kinematics;
+
+import edu.wpi.first.math.interpolation.Interpolatable;
+
+public interface WheelPositions<T extends WheelPositions<T>> extends Interpolatable<T> {
+  /**
+   * Returns a copy of this instance.
+   *
+   * @return A copy.
+   */
+  T copy();
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
index c1a87f2..b2a34a5 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
@@ -71,8 +71,8 @@
   private SplineParameterizer() {}
 
   /**
-   * Parameterizes the spline. This method breaks up the spline into various arcs until their dx,
-   * dy, and dtheta are within specific tolerances.
+   * Parametrizes the spline. This method breaks up the spline into various arcs until their dx, dy,
+   * and dtheta are within specific tolerances.
    *
    * @param spline The spline to parameterize.
    * @return A list of poses and curvatures that represents various points on the spline.
@@ -84,8 +84,8 @@
   }
 
   /**
-   * Parameterizes the spline. This method breaks up the spline into various arcs until their dx,
-   * dy, and dtheta are within specific tolerances.
+   * Parametrizes the spline. This method breaks up the spline into various arcs until their dx, dy,
+   * and dtheta are within specific tolerances.
    *
    * @param spline The spline to parameterize.
    * @param t0 Starting internal spline parameter. It is recommended to use 0.0.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java b/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
index 1871803..b2fe9c8 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
@@ -109,93 +109,6 @@
   }
 
   /**
-   * Discretizes the given continuous A and Q matrices.
-   *
-   * <p>Rather than solving a 2N x 2N matrix exponential like in DiscretizeQ() (which is expensive),
-   * we take advantage of the structure of the block matrix of A and Q.
-   *
-   * <ul>
-   *   <li>eᴬᵀ, which is only N x N, is relatively cheap.
-   *   <li>The upper-right quarter of the 2N x 2N matrix, which we can approximate using a taylor
-   *       series to several terms and still be substantially cheaper than taking the big
-   *       exponential.
-   * </ul>
-   *
-   * @param <States> Nat representing the number of states.
-   * @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.
-   */
-  public static <States extends Num>
-      Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQTaylor(
-          Matrix<States, States> contA, Matrix<States, States> contQ, double dtSeconds) {
-    //       T
-    // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-    //       0
-    //
-    // M = [−A  Q ]
-    //     [ 0  Aᵀ]
-    // ϕ = eᴹᵀ
-    // ϕ₁₂ = A_d⁻¹Q_d
-    //
-    // Taylor series of ϕ:
-    //
-    //   ϕ = eᴹᵀ = I + MT + 1/2 M²T² + 1/6 M³T³ + …
-    //   ϕ = eᴹᵀ = I + MT + 1/2 T²M² + 1/6 T³M³ + …
-    //
-    // Taylor series of ϕ expanded for ϕ₁₂:
-    //
-    //   ϕ₁₂ = 0 + QT + 1/2 T² (−AQ + QAᵀ) + 1/6 T³ (−A lastTerm + Q Aᵀ²) + …
-    //
-    // ```
-    // lastTerm = Q
-    // lastCoeff = T
-    // ATn = Aᵀ
-    // ϕ₁₂ = lastTerm lastCoeff = QT
-    //
-    // for i in range(2, 6):
-    //   // i = 2
-    //   lastTerm = −A lastTerm + Q ATn = −AQ + QAᵀ
-    //   lastCoeff *= T/i → lastCoeff *= T/2 = 1/2 T²
-    //   ATn *= Aᵀ = Aᵀ²
-    //
-    //   // i = 3
-    //   lastTerm = −A lastTerm + Q ATn = −A (−AQ + QAᵀ) + QAᵀ² = …
-    //   …
-    // ```
-
-    // 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;
-
-    // Aᵀⁿ
-    Matrix<States, States> ATn = contA.transpose();
-
-    Matrix<States, States> phi12 = lastTerm.times(lastCoeff);
-
-    // i = 6 i.e. 5th order should be enough precision
-    for (int i = 2; i < 6; ++i) {
-      lastTerm = contA.times(-1).times(lastTerm).plus(Q.times(ATn));
-      lastCoeff *= dtSeconds / ((double) i);
-
-      phi12 = phi12.plus(lastTerm.times(lastCoeff));
-
-      ATn = ATn.times(contA.transpose());
-    }
-
-    var discA = discretizeA(contA, dtSeconds);
-    Q = discA.times(phi12);
-
-    // Make Q symmetric if it isn't already
-    var discQ = Q.plus(Q.transpose()).div(2.0);
-
-    return new Pair<>(discA, discQ);
-  }
-
-  /**
    * Returns a discretized version of the provided continuous measurement noise covariance matrix.
    * Note that dt=0.0 divides R by zero.
    *
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 94be369..e9b46fc 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
@@ -238,7 +238,11 @@
                     .times(h))
                 .normF();
 
-        h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
+        if (truncationError == 0.0) {
+          h = dtSeconds - dtElapsed;
+        } else {
+          h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
+        }
       } while (truncationError > maxError);
 
       dtElapsed += h;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
index cd431f7..0c6e334 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
@@ -21,7 +21,7 @@
    * Constructs a DC motor.
    *
    * @param nominalVoltageVolts Voltage at which the motor constants were measured.
-   * @param stallTorqueNewtonMeters Current draw when stalled.
+   * @param stallTorqueNewtonMeters Torque when stalled.
    * @param stallCurrentAmps Current draw when stalled.
    * @param freeCurrentAmps Current draw under no load.
    * @param freeSpeedRadPerSec Angular velocity under no load.
@@ -47,10 +47,10 @@
   }
 
   /**
-   * Estimate the current being drawn by this motor.
+   * Calculate current drawn by motor with given speed and input voltage.
    *
-   * @param speedRadiansPerSec The speed of the motor.
-   * @param voltageInputVolts The input voltage.
+   * @param speedRadiansPerSec The current angular velocity of the motor.
+   * @param voltageInputVolts The voltage being applied to the motor.
    * @return The estimated current.
    */
   public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) {
@@ -58,20 +58,20 @@
   }
 
   /**
-   * Calculate the torque produced by the motor for a given current.
+   * Calculate torque produced by the motor with a given current.
    *
    * @param currentAmpere The current drawn by the motor.
-   * @return The torque produced.
+   * @return The torque output.
    */
   public double getTorque(double currentAmpere) {
     return currentAmpere * KtNMPerAmp;
   }
 
   /**
-   * Calculate the voltage provided to the motor at a given torque and angular velocity.
+   * Calculate the voltage provided to the motor for a given torque and angular velocity.
    *
    * @param torqueNm The torque produced by the motor.
-   * @param speedRadiansPerSec The speed of the motor.
+   * @param speedRadiansPerSec The current angular velocity of the motor.
    * @return The voltage of the motor.
    */
   public double getVoltage(double torqueNm, double speedRadiansPerSec) {
@@ -79,14 +79,15 @@
   }
 
   /**
-   * Calculate the speed of the motor at a given torque and input voltage.
+   * Calculates the angular speed produced by the motor at a given torque and input voltage.
    *
    * @param torqueNm The torque produced by the motor.
    * @param voltageInputVolts The voltage applied to the motor.
-   * @return The speed of the motor.
+   * @return The angular speed of the motor.
    */
   public double getSpeed(double torqueNm, double voltageInputVolts) {
-    return voltageInputVolts - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt;
+    return voltageInputVolts * KvRadPerSecPerVolt
+        - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt;
   }
 
   /**
@@ -227,6 +228,18 @@
   }
 
   /**
+   * Return a gearbox of Falcon 500 motors with FOC (Field-Oriented Control) enabled.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Falcon 500 FOC enabled motors.
+   */
+  public static DCMotor getFalcon500Foc(int numMotors) {
+    // https://store.ctr-electronics.com/falcon-500-powered-by-talon-fx/
+    return new DCMotor(
+        12, 5.84, 304, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6080.0), numMotors);
+  }
+
+  /**
    * Return a gearbox of Romi/TI_RSLK MAX motors.
    *
    * @param numMotors Number of motors in the gearbox.
@@ -237,4 +250,40 @@
     return new DCMotor(
         4.5, 0.1765, 1.25, 0.13, Units.rotationsPerMinuteToRadiansPerSecond(150.0), numMotors);
   }
+
+  /**
+   * Return a gearbox of Kraken X60 brushless motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return a gearbox of Kraken X60 motors.
+   */
+  public static DCMotor getKrakenX60(int numMotors) {
+    // From https://store.ctr-electronics.com/announcing-kraken-x60/
+    return new DCMotor(
+        12, 7.09, 366, 2, Units.rotationsPerMinuteToRadiansPerSecond(6000), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Kraken X60 brushless motors with FOC (Field-Oriented Control) enabled.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Kraken X60 FOC enabled motors.
+   */
+  public static DCMotor getKrakenX60Foc(int numMotors) {
+    // From https://store.ctr-electronics.com/announcing-kraken-x60/
+    return new DCMotor(
+        12, 9.37, 483, 2, Units.rotationsPerMinuteToRadiansPerSecond(5800), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Neo Vortex brushless motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return a gearbox of Neo Vortex motors.
+   */
+  public static DCMotor getNeoVortex(int numMotors) {
+    // From https://www.revrobotics.com/next-generation-spark-neo/
+    return new DCMotor(
+        12, 3.60, 211, 3.6, Units.rotationsPerMinuteToRadiansPerSecond(6784), numMotors);
+  }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
index 332e690..8377022 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
@@ -123,6 +123,39 @@
   }
 
   /**
+   * Create a state-space model of a DC motor system. The states of the system are [angular
+   * position, angular velocity], inputs are [voltage], and outputs are [angular position, angular
+   * velocity].
+   *
+   * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
+   * {@link edu.wpi.first.math.util.Units} class for converting between unit types.
+   *
+   * <p>The parameters provided by the user are from this feedforward model:
+   *
+   * <p>u = K_v v + K_a a
+   *
+   * @param kV The velocity gain, in volts/(unit/sec)
+   * @param kA The acceleration gain, in volts/(unit/sec^2)
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if kV &lt;= 0 or kA &lt;= 0.
+   * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
+   */
+  public static LinearSystem<N2, N1, N2> createDCMotorSystem(double kV, double kA) {
+    if (kV <= 0.0) {
+      throw new IllegalArgumentException("Kv must be greater than zero.");
+    }
+    if (kA <= 0.0) {
+      throw new IllegalArgumentException("Ka must be greater than zero.");
+    }
+
+    return new LinearSystem<>(
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kV / kA),
+        VecBuilder.fill(0, 1 / kA),
+        Matrix.eye(Nat.N2()),
+        new Matrix<>(Nat.N2(), Nat.N1()));
+  }
+
+  /**
    * Create a state-space model of a differential drive drivetrain. In this model, the states are
    * [left velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are
    * [left velocity, right velocity]ᵀ.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java
new file mode 100644
index 0000000..1580e85
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java
@@ -0,0 +1,449 @@
+// 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.trajectory;
+
+import java.util.Objects;
+
+/**
+ * A exponential curve-shaped velocity profile.
+ *
+ * <p>While this class can be used for a profiled movement from start to finish, the intended usage
+ * is to filter a reference's dynamics based on state-space model dynamics. To compute the reference
+ * obeying this constraint, do the following.
+ *
+ * <p>Initialization:
+ *
+ * <pre><code>
+ * ExponentialProfile.Constraints constraints =
+ *   ExponentialProfile.Constraints.fromCharacteristics(kMaxV, kV, kA);
+ * ExponentialProfile.State previousProfiledReference =
+ *   new ExponentialProfile.State(initialReference, 0.0);
+ * ExponentialProfile profile = new ExponentialProfile(constraints);
+ * </code></pre>
+ *
+ * <p>Run on update:
+ *
+ * <pre><code>
+ * previousProfiledReference =
+ * profile.calculate(timeSincePreviousUpdate, previousProfiledReference, unprofiledReference);
+ * </code></pre>
+ *
+ * <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled
+ * reference is within the constraints, `calculate()` returns the unprofiled reference unchanged.
+ *
+ * <p>Otherwise, a timer can be started to provide monotonic values for `calculate()` and to
+ * determine when the profile has completed via `isFinished()`.
+ */
+public class ExponentialProfile {
+  private final Constraints m_constraints;
+
+  public static class ProfileTiming {
+    public final double inflectionTime;
+    public final double totalTime;
+
+    protected ProfileTiming(double inflectionTime, double totalTime) {
+      this.inflectionTime = inflectionTime;
+      this.totalTime = totalTime;
+    }
+
+    /**
+     * Decides if the profile is finished by time t.
+     *
+     * @param t The time since the beginning of the profile.
+     * @return if the profile is finished at time t.
+     */
+    public boolean isFinished(double t) {
+      return t > inflectionTime;
+    }
+  }
+
+  public static class Constraints {
+    public final double maxInput;
+
+    public final double A;
+    public final double B;
+
+    /**
+     * Construct constraints for an ExponentialProfile.
+     *
+     * @param maxInput maximum unsigned input voltage
+     * @param A The State-Space 1x1 system matrix.
+     * @param B The State-Space 1x1 input matrix.
+     */
+    private Constraints(double maxInput, double A, double B) {
+      this.maxInput = maxInput;
+      this.A = A;
+      this.B = B;
+    }
+
+    /**
+     * Computes the max achievable velocity for an Exponential Profile.
+     *
+     * @return The seady-state velocity achieved by this profile.
+     */
+    public double maxVelocity() {
+      return -maxInput * B / A;
+    }
+
+    /**
+     * Construct constraints for an ExponentialProfile from characteristics.
+     *
+     * @param maxInput maximum unsigned input voltage
+     * @param kV The velocity gain.
+     * @param kA The acceleration gain.
+     * @return The Constraints object.
+     */
+    public static Constraints fromCharacteristics(double maxInput, double kV, double kA) {
+      return new Constraints(maxInput, -kV / kA, 1.0 / kA);
+    }
+
+    /**
+     * Construct constraints for an ExponentialProfile from State-Space parameters.
+     *
+     * @param maxInput maximum unsigned input voltage
+     * @param A The State-Space 1x1 system matrix.
+     * @param B The State-Space 1x1 input matrix.
+     * @return The Constraints object.
+     */
+    public static Constraints fromStateSpace(double maxInput, double A, double B) {
+      return new Constraints(maxInput, A, B);
+    }
+  }
+
+  public static class State {
+    public final double position;
+
+    public final double velocity;
+
+    /**
+     * Construct a state within an exponential profile.
+     *
+     * @param position The position at this state.
+     * @param velocity The velocity at this state.
+     */
+    public State(double position, double velocity) {
+      this.position = position;
+      this.velocity = velocity;
+    }
+
+    @Override
+    public boolean equals(Object other) {
+      if (other instanceof State) {
+        State rhs = (State) other;
+        return this.position == rhs.position && this.velocity == rhs.velocity;
+      } else {
+        return false;
+      }
+    }
+
+    @Override
+    public int hashCode() {
+      return Objects.hash(position, velocity);
+    }
+  }
+
+  /**
+   * Construct an ExponentialProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum input.
+   */
+  public ExponentialProfile(Constraints constraints) {
+    m_constraints = constraints;
+  }
+
+  /**
+   * Calculate the correct position and velocity for the profile at a time t where the current state
+   * is at time t = 0.
+   *
+   * @param t The time since the beginning of the profile.
+   * @param current The current state.
+   * @param goal The desired state when the profile is complete.
+   * @return The position and velocity of the profile at time t.
+   */
+  public State calculate(double t, State current, State goal) {
+    var direction = shouldFlipInput(current, goal) ? -1 : 1;
+    var u = direction * m_constraints.maxInput;
+
+    var inflectionPoint = calculateInflectionPoint(current, goal, u);
+    var timing = calculateProfileTiming(current, inflectionPoint, goal, u);
+
+    if (t < 0) {
+      return current;
+    } else if (t < timing.inflectionTime) {
+      return new State(
+          computeDistanceFromTime(t, u, current), computeVelocityFromTime(t, u, current));
+    } else if (t < timing.totalTime) {
+      return new State(
+          computeDistanceFromTime(t - timing.totalTime, -u, goal),
+          computeVelocityFromTime(t - timing.totalTime, -u, goal));
+    } else {
+      return goal;
+    }
+  }
+
+  /**
+   * Calculate the point after which the fastest way to reach the goal state is to apply input in
+   * the opposite direction.
+   *
+   * @param current The current state.
+   * @param goal The desired state when the profile is complete.
+   * @return The position and velocity of the profile at the inflection point.
+   */
+  public State calculateInflectionPoint(State current, State goal) {
+    var direction = shouldFlipInput(current, goal) ? -1 : 1;
+    var u = direction * m_constraints.maxInput;
+
+    return calculateInflectionPoint(current, goal, u);
+  }
+
+  /**
+   * Calculate the point after which the fastest way to reach the goal state is to apply input in
+   * the opposite direction.
+   *
+   * @param current The current state.
+   * @param goal The desired state when the profile is complete.
+   * @param input The signed input applied to this profile from the current state.
+   * @return The position and velocity of the profile at the inflection point.
+   */
+  private State calculateInflectionPoint(State current, State goal, double input) {
+    var u = input;
+
+    if (current.equals(goal)) {
+      return current;
+    }
+
+    var inflectionVelocity = solveForInflectionVelocity(u, current, goal);
+    var inflectionPosition = computeDistanceFromVelocity(inflectionVelocity, -u, goal);
+
+    return new State(inflectionPosition, inflectionVelocity);
+  }
+
+  /**
+   * Calculate the time it will take for this profile to reach the goal state.
+   *
+   * @param current The current state.
+   * @param goal The desired state when the profile is complete.
+   * @return The total duration of this profile.
+   */
+  public double timeLeftUntil(State current, State goal) {
+    var timing = calculateProfileTiming(current, goal);
+
+    return timing.totalTime;
+  }
+
+  /**
+   * Calculate the time it will take for this profile to reach the inflection point, and the time it
+   * will take for this profile to reach the goal state.
+   *
+   * @param current The current state.
+   * @param goal The desired state when the profile is complete.
+   * @return The timing information for this profile.
+   */
+  public ProfileTiming calculateProfileTiming(State current, State goal) {
+    var direction = shouldFlipInput(current, goal) ? -1 : 1;
+    var u = direction * m_constraints.maxInput;
+
+    var inflectionPoint = calculateInflectionPoint(current, goal, u);
+    return calculateProfileTiming(current, inflectionPoint, goal, u);
+  }
+
+  /**
+   * Calculate the time it will take for this profile to reach the inflection point, and the time it
+   * will take for this profile to reach the goal state.
+   *
+   * @param current The current state.
+   * @param inflectionPoint The inflection point of this profile.
+   * @param goal The desired state when the profile is complete.
+   * @param input The signed input applied to this profile from the current state.
+   * @return The timing information for this profile.
+   */
+  private ProfileTiming calculateProfileTiming(
+      State current, State inflectionPoint, State goal, double input) {
+    var u = input;
+
+    double inflectionT_forward;
+
+    // We need to handle 5 cases here:
+    //
+    // - Approaching -maxVelocity from below
+    // - Approaching -maxVelocity from above
+    // - Approaching maxVelocity from below
+    // - Approaching maxVelocity from above
+    // - At +-maxVelocity
+    //
+    // For cases 1 and 3, we want to subtract epsilon from the inflection point velocity.
+    // For cases 2 and 4, we want to add epsilon to the inflection point velocity.
+    // For case 5, we have reached inflection point velocity.
+    double epsilon = 1e-9;
+    if (Math.abs(Math.signum(input) * m_constraints.maxVelocity() - inflectionPoint.velocity)
+        < epsilon) {
+      double solvableV = inflectionPoint.velocity;
+      double t_to_solvable_v;
+      double x_at_solvable_v;
+      if (Math.abs(current.velocity - inflectionPoint.velocity) < epsilon) {
+        t_to_solvable_v = 0;
+        x_at_solvable_v = current.position;
+      } else {
+        if (Math.abs(current.velocity) > m_constraints.maxVelocity()) {
+          solvableV += Math.signum(u) * epsilon;
+        } else {
+          solvableV -= Math.signum(u) * epsilon;
+        }
+
+        t_to_solvable_v = computeTimeFromVelocity(solvableV, u, current.velocity);
+        x_at_solvable_v = computeDistanceFromVelocity(solvableV, u, current);
+      }
+
+      inflectionT_forward =
+          t_to_solvable_v
+              + Math.signum(input)
+                  * (inflectionPoint.position - x_at_solvable_v)
+                  / m_constraints.maxVelocity();
+    } else {
+      inflectionT_forward = computeTimeFromVelocity(inflectionPoint.velocity, u, current.velocity);
+    }
+
+    var inflectionT_backward = computeTimeFromVelocity(inflectionPoint.velocity, -u, goal.velocity);
+
+    return new ProfileTiming(inflectionT_forward, inflectionT_forward - inflectionT_backward);
+  }
+
+  /**
+   * Calculate the position reached after t seconds when applying an input from the initial state.
+   *
+   * @param t The time since the initial state.
+   * @param input The signed input applied to this profile from the initial state.
+   * @param initial The initial state.
+   * @return The distance travelled by this profile.
+   */
+  private double computeDistanceFromTime(double t, double input, State initial) {
+    var A = m_constraints.A;
+    var B = m_constraints.B;
+    var u = input;
+
+    return initial.position
+        + (-B * u * t + (initial.velocity + B * u / A) * (Math.exp(A * t) - 1)) / A;
+  }
+
+  /**
+   * Calculate the velocity reached after t seconds when applying an input from the initial state.
+   *
+   * @param t The time since the initial state.
+   * @param input The signed input applied to this profile from the initial state.
+   * @param initial The initial state.
+   * @return The distance travelled by this profile.
+   */
+  private double computeVelocityFromTime(double t, double input, State initial) {
+    var A = m_constraints.A;
+    var B = m_constraints.B;
+    var u = input;
+
+    return (initial.velocity + B * u / A) * Math.exp(A * t) - B * u / A;
+  }
+
+  /**
+   * Calculate the time required to reach a specified velocity given the initial velocity.
+   *
+   * @param velocity The goal velocity.
+   * @param input The signed input applied to this profile from the initial state.
+   * @param initial The initial velocity.
+   * @return The time required to reach the goal velocity.
+   */
+  private double computeTimeFromVelocity(double velocity, double input, double initial) {
+    var A = m_constraints.A;
+    var B = m_constraints.B;
+    var u = input;
+
+    return Math.log((A * velocity + B * u) / (A * initial + B * u)) / A;
+  }
+
+  /**
+   * Calculate the distance reached at the same time as the given velocity when applying the given
+   * input from the initial state.
+   *
+   * @param velocity The velocity reached by this profile
+   * @param input The signed input applied to this profile from the initial state.
+   * @param initial The initial state.
+   * @return The distance reached when the given velocity is reached.
+   */
+  private double computeDistanceFromVelocity(double velocity, double input, State initial) {
+    var A = m_constraints.A;
+    var B = m_constraints.B;
+    var u = input;
+
+    return initial.position
+        + (velocity - initial.velocity) / A
+        - B * u / (A * A) * Math.log((A * velocity + B * u) / (A * initial.velocity + B * u));
+  }
+
+  /**
+   * Calculate the velocity at which input should be reversed in order to reach the goal state from
+   * the current state.
+   *
+   * @param input The signed input applied to this profile from the current state.
+   * @param current The current state.
+   * @param goal The goal state.
+   * @return The inflection velocity.
+   */
+  private double solveForInflectionVelocity(double input, State current, State goal) {
+    var A = m_constraints.A;
+    var B = m_constraints.B;
+    var u = input;
+
+    var U_dir = Math.signum(u);
+
+    var position_delta = goal.position - current.position;
+    var velocity_delta = goal.velocity - current.velocity;
+
+    var scalar = (A * current.velocity + B * u) * (A * goal.velocity - B * u);
+    var power = -A / B / u * (A * position_delta - velocity_delta);
+
+    var a = -A * A;
+    var c = (B * B) * (u * u) + scalar * Math.exp(power);
+
+    if (-1e-9 < c && c < 0) {
+      // Numerical stability issue - the heuristic gets it right but c is around -1e-13
+      return 0;
+    }
+
+    return U_dir * Math.sqrt(-c / a);
+  }
+
+  /**
+   * Returns true if the profile should be inverted.
+   *
+   * <p>The profile is inverted if we should first apply negative input in order to reach the goal
+   * state.
+   *
+   * @param current The initial state (usually the current state).
+   * @param goal The desired state when the profile is complete.
+   */
+  @SuppressWarnings("UnnecessaryParentheses")
+  private boolean shouldFlipInput(State current, State goal) {
+    var u = m_constraints.maxInput;
+
+    var xf = goal.position;
+    var v0 = current.velocity;
+    var vf = goal.velocity;
+
+    var x_forward = computeDistanceFromVelocity(vf, u, current);
+    var x_reverse = computeDistanceFromVelocity(vf, -u, current);
+
+    if (v0 >= m_constraints.maxVelocity()) {
+      return xf < x_reverse;
+    }
+
+    if (v0 <= -m_constraints.maxVelocity()) {
+      return xf < x_forward;
+    }
+
+    var a = v0 >= 0;
+    var b = vf >= 0;
+    var c = xf >= x_forward;
+    var d = xf >= x_reverse;
+
+    return (a && !d) || (b && !c) || (!c && !d);
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
index f716dc0..f7120be 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
@@ -30,9 +30,15 @@
    * Constructs a trajectory from a vector of states.
    *
    * @param states A vector of states.
+   * @throws IllegalArgumentException if the vector of states is empty.
    */
   public Trajectory(final List<State> states) {
     m_states = states;
+
+    if (m_states.isEmpty()) {
+      throw new IllegalArgumentException("Trajectory manually created with no states.");
+    }
+
     m_totalTimeSeconds = m_states.get(m_states.size() - 1).timeSeconds;
   }
 
@@ -92,8 +98,13 @@
    *
    * @param timeSeconds The point in time since the beginning of the trajectory to sample.
    * @return The state at that point in time.
+   * @throws IllegalStateException if the trajectory has no states.
    */
   public State sample(double timeSeconds) {
+    if (m_states.isEmpty()) {
+      throw new IllegalStateException("Trajectory cannot be sampled if it has no states.");
+    }
+
     if (timeSeconds <= m_states.get(0).timeSeconds) {
       return m_states.get(0);
     }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
index cf9b6f8..a741aa0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
@@ -271,11 +271,7 @@
       if (minMaxAccel.minAccelerationMetersPerSecondSq
           > minMaxAccel.maxAccelerationMetersPerSecondSq) {
         throw new TrajectoryGenerationException(
-            "The constraint's min acceleration "
-                + "was greater than its max acceleration.\n Offending Constraint: "
-                + constraint.getClass().getName()
-                + "\n If the offending constraint was packaged with WPILib, please file a bug"
-                + " report.");
+            "Infeasible trajectory constraint: " + constraint.getClass().getName() + "\n");
       }
 
       state.minAccelerationMetersPerSecondSq =
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
index 3b6559e..fd7494c 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
@@ -22,14 +22,14 @@
  *   new TrapezoidProfile.Constraints(kMaxV, kMaxA);
  * TrapezoidProfile.State previousProfiledReference =
  *   new TrapezoidProfile.State(initialReference, 0.0);
+ * TrapezoidProfile profile = new TrapezoidProfile(constraints);
  * </code></pre>
  *
  * <p>Run on update:
  *
  * <pre><code>
- * TrapezoidProfile profile =
- *   new TrapezoidProfile(constraints, unprofiledReference, previousProfiledReference);
- * previousProfiledReference = profile.calculate(timeSincePreviousUpdate);
+ * previousProfiledReference =
+ * profile.calculate(timeSincePreviousUpdate, unprofiledReference, previousProfiledReference);
  * </code></pre>
  *
  * <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled
@@ -42,9 +42,10 @@
   // The direction of the profile, either 1 for forwards or -1 for inverted
   private int m_direction;
 
-  private Constraints m_constraints;
-  private State m_initial;
-  private State m_goal;
+  private final Constraints m_constraints;
+  private State m_current;
+  private State m_goal; // TODO: Remove
+  private final boolean m_newAPI; // TODO: Remove
 
   private double m_endAccel;
   private double m_endFullSpeed;
@@ -100,23 +101,36 @@
    * Construct a TrapezoidProfile.
    *
    * @param constraints The constraints on the profile, like maximum velocity.
+   */
+  public TrapezoidProfile(Constraints constraints) {
+    m_constraints = constraints;
+    m_newAPI = true;
+  }
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
    * @param goal The desired state when the profile is complete.
    * @param initial The initial state (usually the current state).
+   * @deprecated Pass the desired and current state into calculate instead of constructing a new
+   *     TrapezoidProfile with the desired and current state
    */
+  @Deprecated(since = "2024", forRemoval = true)
   public TrapezoidProfile(Constraints constraints, State goal, State initial) {
     m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1;
     m_constraints = constraints;
-    m_initial = direct(initial);
+    m_current = direct(initial);
     m_goal = direct(goal);
-
-    if (m_initial.velocity > m_constraints.maxVelocity) {
-      m_initial.velocity = m_constraints.maxVelocity;
+    m_newAPI = false;
+    if (m_current.velocity > m_constraints.maxVelocity) {
+      m_current.velocity = m_constraints.maxVelocity;
     }
 
     // Deal with a possibly truncated motion profile (with nonzero initial or
     // final velocity) by calculating the parameters as if the profile began and
     // ended at zero velocity
-    double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration;
+    double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration;
     double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
 
     double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
@@ -126,7 +140,7 @@
     // of a truncated one
 
     double fullTrapezoidDist =
-        cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
+        cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd;
     double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
 
     double fullSpeedDist =
@@ -148,7 +162,10 @@
    *
    * @param constraints The constraints on the profile, like maximum velocity.
    * @param goal The desired state when the profile is complete.
+   * @deprecated Pass the desired and current state into calculate instead of constructing a new
+   *     TrapezoidProfile with the desired and current state
    */
+  @Deprecated(since = "2024", forRemoval = true)
   public TrapezoidProfile(Constraints constraints, State goal) {
     this(constraints, goal, new State(0, 0));
   }
@@ -159,17 +176,23 @@
    *
    * @param t The time since the beginning of the profile.
    * @return The position and velocity of the profile at time t.
+   * @deprecated Pass the desired and current state into calculate instead of constructing a new
+   *     TrapezoidProfile with the desired and current state
    */
+  @Deprecated(since = "2024", forRemoval = true)
   public State calculate(double t) {
-    State result = new State(m_initial.position, m_initial.velocity);
+    if (m_newAPI) {
+      throw new RuntimeException("Cannot use new constructor with deprecated calculate()");
+    }
+    State result = new State(m_current.position, m_current.velocity);
 
     if (t < m_endAccel) {
       result.velocity += t * m_constraints.maxAcceleration;
-      result.position += (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+      result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
     } else if (t < m_endFullSpeed) {
       result.velocity = m_constraints.maxVelocity;
       result.position +=
-          (m_initial.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
+          (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
               + m_constraints.maxVelocity * (t - m_endAccel);
     } else if (t <= m_endDeccel) {
       result.velocity = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
@@ -185,14 +208,83 @@
   }
 
   /**
+   * Calculate the correct position and velocity for the profile at a time t where the beginning of
+   * the profile was at time t = 0.
+   *
+   * @param t The time since the beginning of the profile.
+   * @param goal The desired state when the profile is complete.
+   * @param current The current state.
+   * @return The position and velocity of the profile at time t.
+   */
+  public State calculate(double t, State goal, State current) {
+    m_direction = shouldFlipAcceleration(current, goal) ? -1 : 1;
+    m_current = direct(current);
+    goal = direct(goal);
+
+    if (m_current.velocity > m_constraints.maxVelocity) {
+      m_current.velocity = m_constraints.maxVelocity;
+    }
+
+    // Deal with a possibly truncated motion profile (with nonzero initial or
+    // final velocity) by calculating the parameters as if the profile began and
+    // ended at zero velocity
+    double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration;
+    double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+    double cutoffEnd = goal.velocity / m_constraints.maxAcceleration;
+    double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
+
+    // Now we can calculate the parameters as if it was a full trapezoid instead
+    // of a truncated one
+
+    double fullTrapezoidDist =
+        cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd;
+    double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+    double fullSpeedDist =
+        fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration;
+
+    // Handle the case where the profile never reaches full speed
+    if (fullSpeedDist < 0) {
+      accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+      fullSpeedDist = 0;
+    }
+
+    m_endAccel = accelerationTime - cutoffBegin;
+    m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+    m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+    State result = new State(m_current.position, m_current.velocity);
+
+    if (t < m_endAccel) {
+      result.velocity += t * m_constraints.maxAcceleration;
+      result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+    } else if (t < m_endFullSpeed) {
+      result.velocity = m_constraints.maxVelocity;
+      result.position +=
+          (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
+              + m_constraints.maxVelocity * (t - m_endAccel);
+    } else if (t <= m_endDeccel) {
+      result.velocity = goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
+      double timeLeft = m_endDeccel - t;
+      result.position =
+          goal.position
+              - (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft;
+    } else {
+      result = goal;
+    }
+
+    return direct(result);
+  }
+
+  /**
    * Returns the time left until a target distance in the profile is reached.
    *
    * @param target The target distance.
    * @return The time left until a target distance in the profile is reached.
    */
   public double timeLeftUntil(double target) {
-    double position = m_initial.position * m_direction;
-    double velocity = m_initial.velocity * m_direction;
+    double position = m_current.position * m_direction;
+    double velocity = m_current.velocity * m_direction;
 
     double endAccel = m_endAccel * m_direction;
     double endFullSpeed = m_endFullSpeed * m_direction - endAccel;
diff --git a/wpimath/src/main/native/cpp/MathShared.cpp b/wpimath/src/main/native/cpp/MathShared.cpp
index 5252e87..b53d080 100644
--- a/wpimath/src/main/native/cpp/MathShared.cpp
+++ b/wpimath/src/main/native/cpp/MathShared.cpp
@@ -5,6 +5,9 @@
 #include "wpimath/MathShared.h"
 
 #include <wpi/mutex.h>
+#include <wpi/timestamp.h>
+
+#include "units/time.h"
 
 using namespace wpi::math;
 
@@ -15,6 +18,9 @@
   void ReportWarningV(fmt::string_view format, fmt::format_args args) override {
   }
   void ReportUsage(MathUsageId id, int count) override {}
+  units::second_t GetTimestamp() override {
+    return units::second_t{wpi::Now() * 1.0e-6};
+  }
 };
 }  // namespace
 
diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
index fc532db..758d2e7 100644
--- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
+++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
@@ -6,16 +6,16 @@
 
 namespace frc {
 
-Vectord<3> PoseTo3dVector(const Pose2d& pose) {
-  return Vectord<3>{pose.Translation().X().value(),
-                    pose.Translation().Y().value(),
-                    pose.Rotation().Radians().value()};
+Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) {
+  return Eigen::Vector3d{pose.Translation().X().value(),
+                         pose.Translation().Y().value(),
+                         pose.Rotation().Radians().value()};
 }
 
-Vectord<4> PoseTo4dVector(const Pose2d& pose) {
-  return Vectord<4>{pose.Translation().X().value(),
-                    pose.Translation().Y().value(), pose.Rotation().Cos(),
-                    pose.Rotation().Sin()};
+Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) {
+  return Eigen::Vector4d{pose.Translation().X().value(),
+                         pose.Translation().Y().value(), pose.Rotation().Cos(),
+                         pose.Rotation().Sin()};
 }
 
 template <>
@@ -28,9 +28,15 @@
   return detail::IsStabilizableImpl<2, 1>(A, B);
 }
 
-Vectord<3> PoseToVector(const Pose2d& pose) {
-  return Vectord<3>{pose.X().value(), pose.Y().value(),
-                    pose.Rotation().Radians().value()};
+template <>
+bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(const Eigen::MatrixXd& A,
+                                                    const Eigen::MatrixXd& B) {
+  return detail::IsStabilizableImpl<Eigen::Dynamic, Eigen::Dynamic>(A, B);
+}
+
+Eigen::Vector3d PoseToVector(const Pose2d& pose) {
+  return Eigen::Vector3d{pose.X().value(), pose.Y().value(),
+                         pose.Rotation().Radians().value()};
 }
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/cpp/controller/BangBangController.cpp b/wpimath/src/main/native/cpp/controller/BangBangController.cpp
index a4fda0a..4a093d6 100644
--- a/wpimath/src/main/native/cpp/controller/BangBangController.cpp
+++ b/wpimath/src/main/native/cpp/controller/BangBangController.cpp
@@ -11,10 +11,7 @@
 using namespace frc;
 
 BangBangController::BangBangController(double tolerance)
-    : m_tolerance(tolerance) {
-  static int instances = 0;
-  instances++;
-}
+    : m_tolerance(tolerance) {}
 
 void BangBangController::SetSetpoint(double setpoint) {
   m_setpoint = setpoint;
diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
index 9788023..05be645 100644
--- a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
+++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
@@ -6,7 +6,7 @@
 
 #include <utility>
 
-#include "Eigen/QR"
+#include <Eigen/QR>
 
 using namespace frc;
 
diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
index e1b2732..257d7e1 100644
--- a/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
+++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
@@ -4,7 +4,8 @@
 
 #include "frc/controller/DifferentialDriveFeedforward.h"
 
-#include "frc/EigenCore.h"
+#include <Eigen/Core>
+
 #include "frc/controller/LinearPlantInversionFeedforward.h"
 #include "frc/system/plant/LinearSystemId.h"
 
@@ -30,8 +31,8 @@
     units::meters_per_second_t nextRightVelocity, units::second_t dt) {
   frc::LinearPlantInversionFeedforward<2, 2> feedforward{m_plant, dt};
 
-  frc::Vectord<2> r{currentLeftVelocity, currentRightVelocity};
-  frc::Vectord<2> nextR{nextLeftVelocity, nextRightVelocity};
+  Eigen::Vector2d r{currentLeftVelocity, currentRightVelocity};
+  Eigen::Vector2d nextR{nextLeftVelocity, nextRightVelocity};
   auto u = feedforward.Calculate(r, nextR);
   return {units::volt_t{u(0)}, units::volt_t{u(1)}};
 }
diff --git a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
index def7234..64210a7 100644
--- a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
+++ b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
@@ -11,7 +11,7 @@
 using namespace frc;
 
 HolonomicDriveController::HolonomicDriveController(
-    frc2::PIDController xController, frc2::PIDController yController,
+    PIDController xController, PIDController yController,
     ProfiledPIDController<units::radian> thetaController)
     : m_xController(std::move(xController)),
       m_yController(std::move(yController)),
@@ -79,3 +79,16 @@
 void HolonomicDriveController::SetEnabled(bool enabled) {
   m_enabled = enabled;
 }
+
+ProfiledPIDController<units::radian>&
+HolonomicDriveController::getThetaController() {
+  return m_thetaController;
+}
+
+PIDController& HolonomicDriveController::getXController() {
+  return m_xController;
+}
+
+PIDController& HolonomicDriveController::getYController() {
+  return m_yController;
+}
diff --git a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
index 17a0c56..173f2ea 100644
--- a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
+++ b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
@@ -5,10 +5,14 @@
 #include "frc/controller/LTVDifferentialDriveController.h"
 
 #include <cmath>
+#include <stdexcept>
 
+#include <Eigen/Cholesky>
+
+#include "frc/DARE.h"
 #include "frc/MathUtil.h"
 #include "frc/StateSpaceUtil.h"
-#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/system/Discretization.h"
 
 using namespace frc;
 
@@ -63,18 +67,39 @@
   // Ax = -Bu
   // x = -A⁻¹Bu
   units::meters_per_second_t maxV{
+      // NOLINTNEXTLINE(clang-analyzer-unix.Malloc)
       -plant.A().householderQr().solve(plant.B() * Vectord<2>{12.0, 12.0})(0)};
 
+  if (maxV <= 0_mps) {
+    throw std::domain_error(
+        "Max velocity of plant with 12 V input must be greater than 0 m/s.");
+  }
+  if (maxV >= 15_mps) {
+    throw std::domain_error(
+        "Max velocity of plant with 12 V input must be less than 15 m/s.");
+  }
+
+  auto R_llt = R.llt();
+
   for (auto velocity = -maxV; velocity < maxV; velocity += 0.01_mps) {
     // The DARE is ill-conditioned if the velocity is close to zero, so don't
     // let the system stop.
     if (units::math::abs(velocity) < 1e-4_mps) {
-      m_table.insert(velocity, Matrixd<2, 5>::Zero());
+      A(State::kY, State::kHeading) = 1e-4;
     } else {
       A(State::kY, State::kHeading) = velocity.value();
-      m_table.insert(velocity,
-                     frc::LinearQuadraticRegulator<5, 2>{A, B, Q, R, dt}.K());
     }
+
+    Matrixd<5, 5> discA;
+    Matrixd<5, 2> discB;
+    DiscretizeAB(A, B, dt, &discA, &discB);
+
+    Matrixd<5, 5> S = detail::DARE<5, 2>(discA, discB, Q, R_llt);
+
+    // K = (BᵀSB + R)⁻¹BᵀSA
+    m_table.insert(velocity, (discB.transpose() * S * discB + R)
+                                 .llt()
+                                 .solve(discB.transpose() * S * discA));
   }
 }
 
diff --git a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
index 256b757..aca317a 100644
--- a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
+++ b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
@@ -4,8 +4,13 @@
 
 #include "frc/controller/LTVUnicycleController.h"
 
+#include <stdexcept>
+
+#include <Eigen/Cholesky>
+
+#include "frc/DARE.h"
 #include "frc/StateSpaceUtil.h"
-#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/system/Discretization.h"
 #include "units/math.h"
 
 using namespace frc;
@@ -37,6 +42,13 @@
 LTVUnicycleController::LTVUnicycleController(
     const wpi::array<double, 3>& Qelems, const wpi::array<double, 2>& Relems,
     units::second_t dt, units::meters_per_second_t maxVelocity) {
+  if (maxVelocity <= 0_mps) {
+    throw std::domain_error("Max velocity must be greater than 0 m/s.");
+  }
+  if (maxVelocity >= 15_mps) {
+    throw std::domain_error("Max velocity must be less than 15 m/s.");
+  }
+
   // The change in global pose for a unicycle is defined by the following three
   // equations.
   //
@@ -74,17 +86,28 @@
   Matrixd<3, 3> Q = frc::MakeCostMatrix(Qelems);
   Matrixd<2, 2> R = frc::MakeCostMatrix(Relems);
 
+  auto R_llt = R.llt();
+
   for (auto velocity = -maxVelocity; velocity < maxVelocity;
        velocity += 0.01_mps) {
     // The DARE is ill-conditioned if the velocity is close to zero, so don't
     // let the system stop.
     if (units::math::abs(velocity) < 1e-4_mps) {
-      m_table.insert(velocity, Matrixd<2, 3>::Zero());
+      A(State::kY, State::kHeading) = 1e-4;
     } else {
       A(State::kY, State::kHeading) = velocity.value();
-      m_table.insert(velocity,
-                     frc::LinearQuadraticRegulator<3, 2>{A, B, Q, R, dt}.K());
     }
+
+    Matrixd<3, 3> discA;
+    Matrixd<3, 2> discB;
+    DiscretizeAB(A, B, dt, &discA, &discB);
+
+    Matrixd<3, 3> S = detail::DARE<3, 2>(discA, discB, Q, R_llt);
+
+    // K = (BᵀSB + R)⁻¹BᵀSA
+    m_table.insert(velocity, (discB.transpose() * S * discB + R)
+                                 .llt()
+                                 .solve(discB.transpose() * S * discA));
   }
 }
 
diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp
index 6c89d25..2638f9e 100644
--- a/wpimath/src/main/native/cpp/controller/PIDController.cpp
+++ b/wpimath/src/main/native/cpp/controller/PIDController.cpp
@@ -13,7 +13,7 @@
 #include "frc/MathUtil.h"
 #include "wpimath/MathShared.h"
 
-using namespace frc2;
+using namespace frc;
 
 PIDController::PIDController(double Kp, double Ki, double Kd,
                              units::second_t period)
@@ -52,6 +52,14 @@
   m_Kd = Kd;
 }
 
+void PIDController::SetIZone(double iZone) {
+  if (iZone < 0) {
+    wpi::math::MathSharedStore::ReportError(
+        "IZone must be a non-negative number, got {}!", iZone);
+  }
+  m_iZone = iZone;
+}
+
 double PIDController::GetP() const {
   return m_Kp;
 }
@@ -64,6 +72,10 @@
   return m_Kd;
 }
 
+double PIDController::GetIZone() const {
+  return m_iZone;
+}
+
 units::second_t PIDController::GetPeriod() const {
   return m_period;
 }
@@ -78,11 +90,12 @@
 
 void PIDController::SetSetpoint(double setpoint) {
   m_setpoint = setpoint;
+  m_haveSetpoint = true;
 
   if (m_continuous) {
     double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
     m_positionError =
-        frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+        InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
   } else {
     m_positionError = m_setpoint - m_measurement;
   }
@@ -95,7 +108,8 @@
 }
 
 bool PIDController::AtSetpoint() const {
-  return std::abs(m_positionError) < m_positionTolerance &&
+  return m_haveMeasurement && m_haveSetpoint &&
+         std::abs(m_positionError) < m_positionTolerance &&
          std::abs(m_velocityError) < m_velocityTolerance;
 }
 
@@ -137,18 +151,23 @@
 double PIDController::Calculate(double measurement) {
   m_measurement = measurement;
   m_prevError = m_positionError;
+  m_haveMeasurement = true;
 
   if (m_continuous) {
     double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
     m_positionError =
-        frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+        InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
   } else {
     m_positionError = m_setpoint - m_measurement;
   }
 
   m_velocityError = (m_positionError - m_prevError) / m_period.value();
 
-  if (m_Ki != 0) {
+  // If the absolute value of the position error is outside of IZone, reset the
+  // total error
+  if (std::abs(m_positionError) > m_iZone) {
+    m_totalError = 0;
+  } else if (m_Ki != 0) {
     m_totalError =
         std::clamp(m_totalError + m_positionError * m_period.value(),
                    m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
@@ -159,6 +178,7 @@
 
 double PIDController::Calculate(double measurement, double setpoint) {
   m_setpoint = setpoint;
+  m_haveSetpoint = true;
   return Calculate(measurement);
 }
 
@@ -167,6 +187,7 @@
   m_prevError = 0;
   m_totalError = 0;
   m_velocityError = 0;
+  m_haveMeasurement = false;
 }
 
 void PIDController::InitSendable(wpi::SendableBuilder& builder) {
@@ -178,6 +199,9 @@
   builder.AddDoubleProperty(
       "d", [this] { return GetD(); }, [this](double value) { SetD(value); });
   builder.AddDoubleProperty(
+      "izone", [this] { return GetIZone(); },
+      [this](double value) { SetIZone(value); });
+  builder.AddDoubleProperty(
       "setpoint", [this] { return GetSetpoint(); },
       [this](double value) { SetSetpoint(value); });
 }
diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
index 39e0d8c..213ba99 100644
--- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
@@ -4,40 +4,8 @@
 
 #include "frc/estimator/DifferentialDrivePoseEstimator.h"
 
-#include <wpi/timestamp.h>
-
-#include "frc/StateSpaceUtil.h"
-#include "frc/estimator/AngleStatistics.h"
-
 using namespace frc;
 
-DifferentialDrivePoseEstimator::InterpolationRecord
-DifferentialDrivePoseEstimator::InterpolationRecord::Interpolate(
-    DifferentialDriveKinematics& kinematics, InterpolationRecord endValue,
-    double i) const {
-  if (i < 0) {
-    return *this;
-  } else if (i > 1) {
-    return endValue;
-  } else {
-    // Find the interpolated left distance.
-    auto left = wpi::Lerp(this->leftDistance, endValue.leftDistance, i);
-    // Find the interpolated right distance.
-    auto right = wpi::Lerp(this->rightDistance, endValue.rightDistance, i);
-
-    // Find the new gyro angle.
-    auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
-
-    // Create a twist to represent this changed based on the interpolated
-    // sensor inputs.
-    auto twist =
-        kinematics.ToTwist2d(left - leftDistance, right - rightDistance);
-    twist.dtheta = (gyro - gyroAngle).Radians();
-
-    return {pose.Exp(twist), gyro, left, right};
-  }
-}
-
 DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
     DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
     units::meter_t leftDistance, units::meter_t rightDistance,
@@ -51,114 +19,7 @@
     units::meter_t leftDistance, units::meter_t rightDistance,
     const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
     const wpi::array<double, 3>& visionMeasurementStdDevs)
-    : m_kinematics{kinematics},
-      m_odometry{gyroAngle, leftDistance, rightDistance, initialPose} {
-  for (size_t i = 0; i < 3; ++i) {
-    m_q[i] = stateStdDevs[i] * stateStdDevs[i];
-  }
-
-  SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-}
-
-void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
-    const wpi::array<double, 3>& visionMeasurementStdDevs) {
-  wpi::array<double, 3> r{wpi::empty_array};
-  for (size_t i = 0; i < 3; ++i) {
-    r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
-  }
-
-  // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
-  // and C = I. See wpimath/algorithms.md.
-  for (size_t row = 0; row < 3; ++row) {
-    if (m_q[row] == 0.0) {
-      m_visionK(row, row) = 0.0;
-    } else {
-      m_visionK(row, row) =
-          m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
-    }
-  }
-}
-
-void DifferentialDrivePoseEstimator::ResetPosition(const Rotation2d& gyroAngle,
-                                                   units::meter_t leftDistance,
-                                                   units::meter_t rightDistance,
-                                                   const Pose2d& pose) {
-  // Reset state estimate and error covariance
-  m_odometry.ResetPosition(gyroAngle, leftDistance, rightDistance, pose);
-  m_poseBuffer.Clear();
-}
-
-Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
-  return m_odometry.GetPose();
-}
-
-void DifferentialDrivePoseEstimator::AddVisionMeasurement(
-    const Pose2d& visionRobotPose, units::second_t timestamp) {
-  // Step 1: Get the estimated pose from when the vision measurement was made.
-  auto sample = m_poseBuffer.Sample(timestamp);
-
-  if (!sample.has_value()) {
-    return;
-  }
-
-  // Step 2: Measure the twist between the odometry pose and the vision pose.
-  auto twist = sample.value().pose.Log(visionRobotPose);
-
-  // Step 3: We should not trust the twist entirely, so instead we scale this
-  // twist by a Kalman gain matrix representing how much we trust vision
-  // measurements compared to our current pose.
-  frc::Vectord<3> k_times_twist =
-      m_visionK *
-      frc::Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
-
-  // Step 4: Convert back to Twist2d.
-  Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
-                      units::meter_t{k_times_twist(1)},
-                      units::radian_t{k_times_twist(2)}};
-
-  // Step 5: Reset Odometry to state at sample with vision adjustment.
-  m_odometry.ResetPosition(
-      sample.value().gyroAngle, sample.value().leftDistance,
-      sample.value().rightDistance, sample.value().pose.Exp(scaledTwist));
-
-  // Step 6: Replay odometry inputs between sample time and latest recorded
-  // sample to update the pose buffer and correct odometry.
-  auto internal_buf = m_poseBuffer.GetInternalBuffer();
-
-  auto first_newer_record =
-      std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp,
-                       [](const auto& pair, auto t) { return t > pair.first; });
-
-  for (auto entry = first_newer_record + 1; entry != internal_buf.end();
-       entry++) {
-    UpdateWithTime(entry->first, entry->second.gyroAngle,
-                   entry->second.leftDistance, entry->second.rightDistance);
-  }
-}
-
-Pose2d DifferentialDrivePoseEstimator::Update(const Rotation2d& gyroAngle,
-                                              units::meter_t leftDistance,
-                                              units::meter_t rightDistance) {
-  return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
-                        leftDistance, rightDistance);
-}
-
-Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
-    units::second_t currentTime, const Rotation2d& gyroAngle,
-    units::meter_t leftDistance, units::meter_t rightDistance) {
-  m_odometry.Update(gyroAngle, leftDistance, rightDistance);
-
-  // fmt::print("odo, {}, {}, {}, {}, {}, {}\n",
-  //   gyroAngle.Radians(),
-  //   leftDistance,
-  //   rightDistance,
-  //   GetEstimatedPosition().X(),
-  //   GetEstimatedPosition().Y(),
-  //   GetEstimatedPosition().Rotation().Radians()
-  // );
-
-  m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle,
-                                       leftDistance, rightDistance});
-
-  return GetEstimatedPosition();
-}
+    : PoseEstimator<DifferentialDriveWheelSpeeds,
+                    DifferentialDriveWheelPositions>(
+          kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+      m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {}
diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
index 9642e08..7c75e71 100644
--- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
@@ -8,49 +8,10 @@
 
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/AngleStatistics.h"
+#include "wpimath/MathShared.h"
 
 using namespace frc;
 
-frc::MecanumDrivePoseEstimator::InterpolationRecord
-frc::MecanumDrivePoseEstimator::InterpolationRecord::Interpolate(
-    MecanumDriveKinematics& kinematics, InterpolationRecord endValue,
-    double i) const {
-  if (i < 0) {
-    return *this;
-  } else if (i > 1) {
-    return endValue;
-  } else {
-    // Find the new wheel distance measurements.
-    MecanumDriveWheelPositions wheels_lerp{
-        wpi::Lerp(this->wheelPositions.frontLeft,
-                  endValue.wheelPositions.frontLeft, i),
-        wpi::Lerp(this->wheelPositions.frontRight,
-                  endValue.wheelPositions.frontRight, i),
-        wpi::Lerp(this->wheelPositions.rearLeft,
-                  endValue.wheelPositions.rearLeft, i),
-        wpi::Lerp(this->wheelPositions.rearRight,
-                  endValue.wheelPositions.rearRight, i)};
-
-    // Find the distance between this measurement and the
-    // interpolated measurement.
-    MecanumDriveWheelPositions wheels_delta{
-        wheels_lerp.frontLeft - this->wheelPositions.frontLeft,
-        wheels_lerp.frontRight - this->wheelPositions.frontRight,
-        wheels_lerp.rearLeft - this->wheelPositions.rearLeft,
-        wheels_lerp.rearRight - this->wheelPositions.rearRight};
-
-    // Find the new gyro angle.
-    auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
-
-    // Create a twist to represent this changed based on the interpolated
-    // sensor inputs.
-    auto twist = kinematics.ToTwist2d(wheels_delta);
-    twist.dtheta = (gyro - gyroAngle).Radians();
-
-    return {pose.Exp(twist), gyro, wheels_lerp};
-  }
-}
-
 frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
     MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
     const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
@@ -63,107 +24,6 @@
     const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose,
     const wpi::array<double, 3>& stateStdDevs,
     const wpi::array<double, 3>& visionMeasurementStdDevs)
-    : m_kinematics{kinematics},
-      m_odometry{kinematics, gyroAngle, wheelPositions, initialPose} {
-  for (size_t i = 0; i < 3; ++i) {
-    m_q[i] = stateStdDevs[i] * stateStdDevs[i];
-  }
-
-  SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-}
-
-void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
-    const wpi::array<double, 3>& visionMeasurementStdDevs) {
-  wpi::array<double, 3> r{wpi::empty_array};
-  for (size_t i = 0; i < 3; ++i) {
-    r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
-  }
-
-  // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
-  // and C = I. See wpimath/algorithms.md.
-  for (size_t row = 0; row < 3; ++row) {
-    if (m_q[row] == 0.0) {
-      m_visionK(row, row) = 0.0;
-    } else {
-      m_visionK(row, row) =
-          m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
-    }
-  }
-}
-
-void frc::MecanumDrivePoseEstimator::ResetPosition(
-    const Rotation2d& gyroAngle,
-    const MecanumDriveWheelPositions& wheelPositions, const Pose2d& pose) {
-  // Reset state estimate and error covariance
-  m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
-  m_poseBuffer.Clear();
-}
-
-Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
-  return m_odometry.GetPose();
-}
-
-void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
-    const Pose2d& visionRobotPose, units::second_t timestamp) {
-  // Step 1: Get the estimated pose from when the vision measurement was made.
-  auto sample = m_poseBuffer.Sample(timestamp);
-
-  if (!sample.has_value()) {
-    return;
-  }
-
-  // Step 2: Measure the twist between the odometry pose and the vision pose
-  auto twist = sample.value().pose.Log(visionRobotPose);
-
-  // Step 3: We should not trust the twist entirely, so instead we scale this
-  // twist by a Kalman gain matrix representing how much we trust vision
-  // measurements compared to our current pose.
-  frc::Vectord<3> k_times_twist =
-      m_visionK *
-      frc::Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
-
-  // Step 4: Convert back to Twist2d
-  Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
-                      units::meter_t{k_times_twist(1)},
-                      units::radian_t{k_times_twist(2)}};
-
-  // Step 5: Reset Odometry to state at sample with vision adjustment.
-  m_odometry.ResetPosition(sample.value().gyroAngle,
-                           sample.value().wheelPositions,
-                           sample.value().pose.Exp(scaledTwist));
-
-  // Step 6: Replay odometry inputs between sample time and latest recorded
-  // sample to update the pose buffer and correct odometry.
-  auto internal_buf = m_poseBuffer.GetInternalBuffer();
-
-  auto upper_bound =
-      std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp,
-                       [](const auto& pair, auto t) { return t > pair.first; });
-
-  for (auto entry = upper_bound; entry != internal_buf.end(); entry++) {
-    UpdateWithTime(entry->first, entry->second.gyroAngle,
-                   entry->second.wheelPositions);
-  }
-}
-
-Pose2d frc::MecanumDrivePoseEstimator::Update(
-    const Rotation2d& gyroAngle,
-    const MecanumDriveWheelPositions& wheelPositions) {
-  return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
-                        wheelPositions);
-}
-
-Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
-    units::second_t currentTime, const Rotation2d& gyroAngle,
-    const MecanumDriveWheelPositions& wheelPositions) {
-  m_odometry.Update(gyroAngle, wheelPositions);
-
-  MecanumDriveWheelPositions internalWheelPositions{
-      wheelPositions.frontLeft, wheelPositions.frontRight,
-      wheelPositions.rearLeft, wheelPositions.rearRight};
-
-  m_poseBuffer.AddSample(
-      currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions});
-
-  return GetEstimatedPosition();
-}
+    : PoseEstimator<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
+          kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+      m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {}
diff --git a/wpimath/src/main/native/cpp/filter/Debouncer.cpp b/wpimath/src/main/native/cpp/filter/Debouncer.cpp
index 4e909a2..841768e 100644
--- a/wpimath/src/main/native/cpp/filter/Debouncer.cpp
+++ b/wpimath/src/main/native/cpp/filter/Debouncer.cpp
@@ -4,6 +4,8 @@
 
 #include "frc/filter/Debouncer.h"
 
+#include "wpimath/MathShared.h"
+
 using namespace frc;
 
 Debouncer::Debouncer(units::second_t debounceTime, DebounceType type)
@@ -21,11 +23,12 @@
 }
 
 void Debouncer::ResetTimer() {
-  m_prevTime = units::microsecond_t(wpi::Now());
+  m_prevTime = wpi::math::MathSharedStore::GetTimestamp();
 }
 
 bool Debouncer::HasElapsed() const {
-  return units::microsecond_t(wpi::Now()) - m_prevTime >= m_debounceTime;
+  return wpi::math::MathSharedStore::GetTimestamp() - m_prevTime >=
+         m_debounceTime;
 }
 
 bool Debouncer::Calculate(bool input) {
diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp
index d1ee93d..2a8c9db 100644
--- a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp
+++ b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp
@@ -8,7 +8,8 @@
 #include <stdexcept>
 #include <utility>
 
-#include "Eigen/QR"
+#include <Eigen/Core>
+#include <Eigen/QR>
 
 using namespace frc;
 
@@ -18,7 +19,7 @@
   // Construct a change of basis matrix from the source coordinate system to the
   // NWU coordinate system. Each column vector in the change of basis matrix is
   // one of the old basis vectors mapped to its representation in the new basis.
-  Matrixd<3, 3> R;
+  Eigen::Matrix3d R;
   R.block<3, 1>(0, 0) = positiveX.m_axis;
   R.block<3, 1>(0, 1) = positiveY.m_axis;
   R.block<3, 1>(0, 2) = positiveZ.m_axis;
@@ -68,6 +69,8 @@
 Transform3d CoordinateSystem::Convert(const Transform3d& transform,
                                       const CoordinateSystem& from,
                                       const CoordinateSystem& to) {
-  return Transform3d{Convert(transform.Translation(), from, to),
-                     Convert(transform.Rotation(), from, to)};
+  const auto coordRot = from.m_rotation - to.m_rotation;
+  return Transform3d{
+      Convert(transform.Translation(), from, to),
+      (-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))};
 }
diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
index 2648a90..2e15216 100644
--- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
@@ -8,6 +8,9 @@
 
 #include <wpi/json.h>
 
+#include "frc/MathUtil.h"
+#include "geometry2d.pb.h"
+
 using namespace frc;
 
 Transform2d Pose2d::operator-(const Pose2d& other) const {
@@ -67,6 +70,36 @@
   return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}};
 }
 
+Pose2d Pose2d::Nearest(std::span<const Pose2d> poses) const {
+  return *std::min_element(
+      poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) {
+        auto aDistance = this->Translation().Distance(a.Translation());
+        auto bDistance = this->Translation().Distance(b.Translation());
+
+        // If the distances are equal sort by difference in rotation
+        if (aDistance == bDistance) {
+          return std::abs((this->Rotation() - a.Rotation()).Radians().value()) <
+                 std::abs((this->Rotation() - b.Rotation()).Radians().value());
+        }
+        return aDistance < bDistance;
+      });
+}
+
+Pose2d Pose2d::Nearest(std::initializer_list<Pose2d> poses) const {
+  return *std::min_element(
+      poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) {
+        auto aDistance = this->Translation().Distance(a.Translation());
+        auto bDistance = this->Translation().Distance(b.Translation());
+
+        // If the distances are equal sort by difference in rotation
+        if (aDistance == bDistance) {
+          return std::abs((this->Rotation() - a.Rotation()).Radians().value()) <
+                 std::abs((this->Rotation() - b.Rotation()).Radians().value());
+        }
+        return aDistance < bDistance;
+      });
+}
+
 void frc::to_json(wpi::json& json, const Pose2d& pose) {
   json = wpi::json{{"translation", pose.Translation()},
                    {"rotation", pose.Rotation()}};
@@ -76,3 +109,23 @@
   pose = Pose2d{json.at("translation").get<Translation2d>(),
                 json.at("rotation").get<Rotation2d>()};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Pose2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose2d>(
+      arena);
+}
+
+frc::Pose2d wpi::Protobuf<frc::Pose2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufPose2d*>(&msg);
+  return Pose2d{wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
+                wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation())};
+}
+
+void wpi::Protobuf<frc::Pose2d>::Pack(google::protobuf::Message* msg,
+                                      const frc::Pose2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufPose2d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
index 4732c4d..ffbaecb 100644
--- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
@@ -6,8 +6,11 @@
 
 #include <cmath>
 
+#include <Eigen/Core>
 #include <wpi/json.h>
 
+#include "geometry3d.pb.h"
+
 using namespace frc;
 
 namespace {
@@ -21,14 +24,14 @@
  * @param rotation The rotation vector.
  * @return The rotation vector as a 3x3 rotation matrix.
  */
-Matrixd<3, 3> RotationVectorToMatrix(const Vectord<3>& rotation) {
+Eigen::Matrix3d RotationVectorToMatrix(const Eigen::Vector3d& rotation) {
   // Given a rotation vector <a, b, c>,
   //         [ 0 -c  b]
   // Omega = [ c  0 -a]
   //         [-b  a  0]
-  return Matrixd<3, 3>{{0.0, -rotation(2), rotation(1)},
-                       {rotation(2), 0.0, -rotation(0)},
-                       {-rotation(1), rotation(0), 0.0}};
+  return Eigen::Matrix3d{{0.0, -rotation(2), rotation(1)},
+                         {rotation(2), 0.0, -rotation(0)},
+                         {-rotation(1), rotation(0), 0.0}};
 }
 }  // namespace
 
@@ -60,6 +63,10 @@
   return *this * (1.0 / scalar);
 }
 
+Pose3d Pose3d::RotateBy(const Rotation3d& other) const {
+  return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
+}
+
 Pose3d Pose3d::TransformBy(const Transform3d& other) const {
   return {m_translation + (other.Translation().RotateBy(m_rotation)),
           other.Rotation() + m_rotation};
@@ -71,73 +78,102 @@
 }
 
 Pose3d Pose3d::Exp(const Twist3d& twist) const {
-  Matrixd<3, 3> Omega = RotationVectorToMatrix(
-      Vectord<3>{twist.rx.value(), twist.ry.value(), twist.rz.value()});
-  Matrixd<3, 3> OmegaSq = Omega * Omega;
+  // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
+  Eigen::Vector3d u{twist.dx.value(), twist.dy.value(), twist.dz.value()};
+  Eigen::Vector3d rvec{twist.rx.value(), twist.ry.value(), twist.rz.value()};
+  Eigen::Matrix3d omega = RotationVectorToMatrix(rvec);
+  Eigen::Matrix3d omegaSq = omega * omega;
+  double theta = rvec.norm();
+  double thetaSq = theta * theta;
 
-  double thetaSq =
-      (twist.rx * twist.rx + twist.ry * twist.ry + twist.rz * twist.rz).value();
-
-  // Get left Jacobian of SO3. See first line in right column of
-  // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
-  Matrixd<3, 3> J;
-  if (thetaSq < 1E-9 * 1E-9) {
-    // V = I + 0.5ω
-    J = Matrixd<3, 3>::Identity() + 0.5 * Omega;
+  double A;
+  double B;
+  double C;
+  if (std::abs(theta) < 1E-7) {
+    // Taylor Expansions around θ = 0
+    // A = 1/1! - θ²/3! + θ⁴/5!
+    // B = 1/2! - θ²/4! + θ⁴/6!
+    // C = 1/3! - θ²/5! + θ⁴/7!
+    // sources:
+    // A:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
+    // B:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+    // C:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+    A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120;
+    B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720;
+    C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
   } else {
-    double theta = std::sqrt(thetaSq);
-    // J = I + (1 − std::cos(θ))/θ² ω + (θ − std::sin(θ))/θ³ ω²
-    J = Matrixd<3, 3>::Identity() + (1.0 - std::cos(theta)) / thetaSq * Omega +
-        (theta - std::sin(theta)) / (thetaSq * theta) * OmegaSq;
+    // A = std::sin(θ)/θ
+    // B = (1 - std::cos(θ)) / θ²
+    // C = (1 - A) / θ²
+    A = std::sin(theta) / theta;
+    B = (1 - std::cos(theta)) / thetaSq;
+    C = (1 - A) / thetaSq;
   }
 
-  // Get translation component
-  Vectord<3> translation =
-      J * Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()};
+  Eigen::Matrix3d R = Eigen::Matrix3d::Identity() + A * omega + B * omegaSq;
+  Eigen::Matrix3d V = Eigen::Matrix3d::Identity() + B * omega + C * omegaSq;
 
-  const Transform3d transform{Translation3d{units::meter_t{translation(0)},
-                                            units::meter_t{translation(1)},
-                                            units::meter_t{translation(2)}},
-                              Rotation3d{twist.rx, twist.ry, twist.rz}};
+  auto translation_component = V * u;
+  const Transform3d transform{
+      Translation3d{units::meter_t{translation_component(0)},
+                    units::meter_t{translation_component(1)},
+                    units::meter_t{translation_component(2)}},
+      Rotation3d{R}};
 
   return *this + transform;
 }
 
 Twist3d Pose3d::Log(const Pose3d& end) const {
+  // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
   const auto transform = end.RelativeTo(*this);
 
-  Vectord<3> rotVec = transform.Rotation().GetQuaternion().ToRotationVector();
+  Eigen::Vector3d u{transform.X().value(), transform.Y().value(),
+                    transform.Z().value()};
+  Eigen::Vector3d rvec =
+      transform.Rotation().GetQuaternion().ToRotationVector();
 
-  Matrixd<3, 3> Omega = RotationVectorToMatrix(rotVec);
-  Matrixd<3, 3> OmegaSq = Omega * Omega;
+  Eigen::Matrix3d omega = RotationVectorToMatrix(rvec);
+  Eigen::Matrix3d omegaSq = omega * omega;
+  double theta = rvec.norm();
+  double thetaSq = theta * theta;
 
-  double thetaSq = rotVec.squaredNorm();
-
-  // Get left Jacobian inverse of SO3. See fourth line in right column of
-  // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
-  Matrixd<3, 3> Jinv;
-  if (thetaSq < 1E-9 * 1E-9) {
-    // J⁻¹ = I − 0.5ω + 1/12 ω²
-    Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega + 1.0 / 12.0 * OmegaSq;
+  double C;
+  if (std::abs(theta) < 1E-7) {
+    // Taylor Expansions around θ = 0
+    // A = 1/1! - θ²/3! + θ⁴/5!
+    // B = 1/2! - θ²/4! + θ⁴/6!
+    // C = 1/6 * (1/2 + θ²/5! + θ⁴/7!)
+    // sources:
+    // A:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
+    // B:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+    // C:
+    // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5BDivide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2C2Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0
+    C = 1 / 12.0 + thetaSq / 720 + thetaSq * thetaSq / 30240;
   } else {
-    double theta = std::sqrt(thetaSq);
-    double halfTheta = 0.5 * theta;
-
-    // J⁻¹ = I − 0.5ω + (1 − 0.5θ std::cos(θ/2) / std::sin(θ/2))/θ² ω²
-    Jinv = Matrixd<3, 3>::Identity() - 0.5 * Omega +
-           (1.0 - 0.5 * theta * std::cos(halfTheta) / std::sin(halfTheta)) /
-               thetaSq * OmegaSq;
+    // A = std::sin(θ)/θ
+    // B = (1 - std::cos(θ)) / θ²
+    // C = (1 - A/(2*B)) / θ²
+    double A = std::sin(theta) / theta;
+    double B = (1 - std::cos(theta)) / thetaSq;
+    C = (1 - A / (2 * B)) / thetaSq;
   }
 
-  // Get dtranslation component
-  Vectord<3> dtranslation =
-      Jinv * Vectord<3>{transform.X().value(), transform.Y().value(),
-                        transform.Z().value()};
+  Eigen::Matrix3d V_inv =
+      Eigen::Matrix3d::Identity() - 0.5 * omega + C * omegaSq;
 
-  return Twist3d{
-      units::meter_t{dtranslation(0)}, units::meter_t{dtranslation(1)},
-      units::meter_t{dtranslation(2)}, units::radian_t{rotVec(0)},
-      units::radian_t{rotVec(1)},      units::radian_t{rotVec(2)}};
+  Eigen::Vector3d translation_component = V_inv * u;
+
+  return Twist3d{units::meter_t{translation_component(0)},
+                 units::meter_t{translation_component(1)},
+                 units::meter_t{translation_component(2)},
+                 units::radian_t{rvec(0)},
+                 units::radian_t{rvec(1)},
+                 units::radian_t{rvec(2)}};
 }
 
 Pose2d Pose3d::ToPose2d() const {
@@ -153,3 +189,23 @@
   pose = Pose3d{json.at("translation").get<Translation3d>(),
                 json.at("rotation").get<Rotation3d>()};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Pose3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose3d>(
+      arena);
+}
+
+frc::Pose3d wpi::Protobuf<frc::Pose3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufPose3d*>(&msg);
+  return Pose3d{wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
+                wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation())};
+}
+
+void wpi::Protobuf<frc::Pose3d>::Pack(google::protobuf::Message* msg,
+                                      const frc::Pose3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufPose3d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
index 9c2ceda..37afbb8 100644
--- a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
@@ -4,13 +4,53 @@
 
 #include "frc/geometry/Quaternion.h"
 
+#include <numbers>
+
 #include <wpi/json.h>
 
+#include "geometry3d.pb.h"
+
 using namespace frc;
 
 Quaternion::Quaternion(double w, double x, double y, double z)
     : m_r{w}, m_v{x, y, z} {}
 
+Quaternion Quaternion::operator+(const Quaternion& other) const {
+  return Quaternion{
+      m_r + other.m_r,
+      m_v(0) + other.m_v(0),
+      m_v(1) + other.m_v(1),
+      m_v(2) + other.m_v(2),
+  };
+}
+
+Quaternion Quaternion::operator-(const Quaternion& other) const {
+  return Quaternion{
+      m_r - other.m_r,
+      m_v(0) - other.m_v(0),
+      m_v(1) - other.m_v(1),
+      m_v(2) - other.m_v(2),
+  };
+}
+
+Quaternion Quaternion::operator*(const double other) const {
+  return Quaternion{
+      m_r * other,
+      m_v(0) * other,
+      m_v(1) * other,
+      m_v(2) * other,
+  };
+}
+
+Quaternion Quaternion::operator/(const double other) const {
+  return Quaternion{
+      m_r / other,
+      m_v(0) / other,
+      m_v(1) / other,
+      m_v(2) / other,
+  };
+}
+
 Quaternion Quaternion::operator*(const Quaternion& other) const {
   // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
   const auto& r1 = m_r;
@@ -26,26 +66,102 @@
   // v = r₁v₂ + r₂v₁ + v₁ x v₂
   Eigen::Vector3d v = r1 * v2 + r2 * v1 + cross;
 
-  return Quaternion{r1 * r2 - v1.dot(v2), v(0), v(1), v(2)};
+  return Quaternion{// r = r₁r₂ − v₁ ⋅ v₂
+                    r1 * r2 - v1.dot(v2),
+                    // v = r₁v₂ + r₂v₁ + v₁ x v₂
+                    v(0), v(1), v(2)};
 }
 
 bool Quaternion::operator==(const Quaternion& other) const {
-  return std::abs(m_r * other.m_r + m_v.dot(other.m_v)) > 1.0 - 1E-9;
+  return std::abs(Dot(other) - Norm() * other.Norm()) < 1e-9 &&
+         std::abs(Norm() - other.Norm()) < 1e-9;
+}
+
+Quaternion Quaternion::Conjugate() const {
+  return Quaternion{W(), -X(), -Y(), -Z()};
+}
+
+double Quaternion::Dot(const Quaternion& other) const {
+  return W() * other.W() + m_v.dot(other.m_v);
 }
 
 Quaternion Quaternion::Inverse() const {
-  return Quaternion{m_r, -m_v(0), -m_v(1), -m_v(2)};
+  double norm = Norm();
+  return Conjugate() / (norm * norm);
+}
+
+double Quaternion::Norm() const {
+  return std::sqrt(Dot(*this));
 }
 
 Quaternion Quaternion::Normalize() const {
-  double norm = std::sqrt(W() * W() + X() * X() + Y() * Y() + Z() * Z());
+  double norm = Norm();
   if (norm == 0.0) {
     return Quaternion{};
   } else {
-    return Quaternion{W() / norm, X() / norm, Y() / norm, Z() / norm};
+    return Quaternion{W(), X(), Y(), Z()} / norm;
   }
 }
 
+Quaternion Quaternion::Pow(const double other) const {
+  return (Log() * other).Exp();
+}
+
+Quaternion Quaternion::Exp(const Quaternion& other) const {
+  return other.Exp() * *this;
+}
+
+Quaternion Quaternion::Exp() const {
+  double scalar = std::exp(m_r);
+
+  double axial_magnitude = m_v.norm();
+  double cosine = std::cos(axial_magnitude);
+
+  double axial_scalar;
+
+  if (axial_magnitude < 1e-9) {
+    // Taylor series of sin(x)/x near x=0: 1 − x²/6 + x⁴/120 + O(n⁶)
+    double axial_magnitude_sq = axial_magnitude * axial_magnitude;
+    double axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq;
+    axial_scalar =
+        1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0;
+  } else {
+    axial_scalar = std::sin(axial_magnitude) / axial_magnitude;
+  }
+
+  return Quaternion(cosine * scalar, X() * axial_scalar * scalar,
+                    Y() * axial_scalar * scalar, Z() * axial_scalar * scalar);
+}
+
+Quaternion Quaternion::Log(const Quaternion& other) const {
+  return (other * Inverse()).Log();
+}
+
+Quaternion Quaternion::Log() const {
+  double scalar = std::log(Norm());
+
+  double v_norm = m_v.norm();
+
+  double s_norm = W() / Norm();
+
+  if (std::abs(s_norm + 1) < 1e-9) {
+    return Quaternion{scalar, -std::numbers::pi, 0, 0};
+  }
+
+  double v_scalar;
+
+  if (v_norm < 1e-9) {
+    // Taylor series expansion of atan2(y / x) / y around y = 0 = 1/x -
+    // y^2/3*x^3 + O(y^4)
+    v_scalar = 1.0 / W() - 1.0 / 3.0 * v_norm * v_norm / (W() * W() * W());
+  } else {
+    v_scalar = std::atan2(v_norm, W()) / v_norm;
+  }
+
+  return Quaternion{scalar, v_scalar * m_v(0), v_scalar * m_v(1),
+                    v_scalar * m_v(2)};
+}
+
 double Quaternion::W() const {
   return m_r;
 }
@@ -80,6 +196,30 @@
   }
 }
 
+Quaternion Quaternion::FromRotationVector(const Eigen::Vector3d& rvec) {
+  // 𝑣⃗ = θ * v̂
+  // v̂ = 𝑣⃗ / θ
+
+  // 𝑞 = std::cos(θ/2) + std::sin(θ/2) * v̂
+  // 𝑞 = std::cos(θ/2) + std::sin(θ/2) / θ * 𝑣⃗
+
+  double theta = rvec.norm();
+  double cos = std::cos(theta / 2);
+
+  double axial_scalar;
+
+  if (theta < 1e-9) {
+    // taylor series expansion of sin(θ/2) / θ around θ = 0 = 1/2 - θ²/48 +
+    // O(θ⁴)
+    axial_scalar = 1.0 / 2.0 - theta * theta / 48.0;
+  } else {
+    axial_scalar = std::sin(theta / 2) / theta;
+  }
+
+  return Quaternion{cos, axial_scalar * rvec(0), axial_scalar * rvec(1),
+                    axial_scalar * rvec(2)};
+}
+
 void frc::to_json(wpi::json& json, const Quaternion& quaternion) {
   json = wpi::json{{"W", quaternion.W()},
                    {"X", quaternion.X()},
@@ -92,3 +232,24 @@
       Quaternion{json.at("W").get<double>(), json.at("X").get<double>(),
                  json.at("Y").get<double>(), json.at("Z").get<double>()};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Quaternion>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufQuaternion>(
+      arena);
+}
+
+frc::Quaternion wpi::Protobuf<frc::Quaternion>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufQuaternion*>(&msg);
+  return frc::Quaternion{m->w(), m->x(), m->y(), m->z()};
+}
+
+void wpi::Protobuf<frc::Quaternion>::Pack(google::protobuf::Message* msg,
+                                          const frc::Quaternion& value) {
+  auto m = static_cast<wpi::proto::ProtobufQuaternion*>(msg);
+  m->set_w(value.W());
+  m->set_x(value.X());
+  m->set_y(value.Y());
+  m->set_z(value.Z());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
index 921e1f8..05a644e 100644
--- a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
@@ -8,6 +8,7 @@
 
 #include <wpi/json.h>
 
+#include "geometry2d.pb.h"
 #include "units/math.h"
 
 using namespace frc;
@@ -19,3 +20,21 @@
 void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
   rotation = Rotation2d{units::radian_t{json.at("radians").get<double>()}};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Rotation2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation2d>(
+      arena);
+}
+
+frc::Rotation2d wpi::Protobuf<frc::Rotation2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufRotation2d*>(&msg);
+  return frc::Rotation2d{units::radian_t{m->value()}};
+}
+
+void wpi::Protobuf<frc::Rotation2d>::Pack(google::protobuf::Message* msg,
+                                          const frc::Rotation2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufRotation2d*>(msg);
+  m->set_value(value.Radians().value());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
index 298f0e6..3a68870 100644
--- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
@@ -7,12 +7,13 @@
 #include <cmath>
 #include <numbers>
 
+#include <Eigen/Core>
+#include <Eigen/LU>
+#include <Eigen/QR>
 #include <wpi/json.h>
 
-#include "Eigen/Core"
-#include "Eigen/LU"
-#include "Eigen/QR"
 #include "frc/fmt/Eigen.h"
+#include "geometry3d.pb.h"
 #include "units/math.h"
 #include "wpimath/MathShared.h"
 
@@ -38,23 +39,26 @@
                    cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy};
 }
 
-Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) {
+Rotation3d::Rotation3d(const Eigen::Vector3d& rvec)
+    : Rotation3d{rvec, units::radian_t{rvec.norm()}} {}
+
+Rotation3d::Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle) {
   double norm = axis.norm();
   if (norm == 0.0) {
     return;
   }
 
   // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
-  Vectord<3> v = axis / norm * units::math::sin(angle / 2.0);
+  Eigen::Vector3d v = axis / norm * units::math::sin(angle / 2.0);
   m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
 }
 
-Rotation3d::Rotation3d(const Matrixd<3, 3>& rotationMatrix) {
+Rotation3d::Rotation3d(const Eigen::Matrix3d& rotationMatrix) {
   const auto& R = rotationMatrix;
 
   // Require that the rotation matrix is special orthogonal. This is true if the
   // matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
-  if ((R * R.transpose() - Matrixd<3, 3>::Identity()).norm() > 1e-9) {
+  if ((R * R.transpose() - Eigen::Matrix3d::Identity()).norm() > 1e-9) {
     std::string msg =
         fmt::format("Rotation matrix isn't orthogonal\n\nR =\n{}\n", R);
 
@@ -109,7 +113,8 @@
   m_q = Quaternion{w, x, y, z};
 }
 
-Rotation3d::Rotation3d(const Vectord<3>& initial, const Vectord<3>& final) {
+Rotation3d::Rotation3d(const Eigen::Vector3d& initial,
+                       const Eigen::Vector3d& final) {
   double dot = initial.dot(final);
   double normProduct = initial.norm() * final.norm();
   double dotNorm = dot / normProduct;
@@ -170,6 +175,11 @@
   return *this * (1.0 / scalar);
 }
 
+bool Rotation3d::operator==(const Rotation3d& other) const {
+  return std::abs(std::abs(m_q.Dot(other.m_q)) -
+                  m_q.Norm() * other.m_q.Norm()) < 1e-9;
+}
+
 Rotation3d Rotation3d::RotateBy(const Rotation3d& other) const {
   return Rotation3d{other.m_q * m_q};
 }
@@ -184,9 +194,15 @@
   double y = m_q.Y();
   double z = m_q.Z();
 
-  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
-  return units::radian_t{
-      std::atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))};
+  // wpimath/algorithms.md
+  double cxcy = 1.0 - 2.0 * (x * x + y * y);
+  double sxcy = 2.0 * (w * x + y * z);
+  double cy_sq = cxcy * cxcy + sxcy * sxcy;
+  if (cy_sq > 1e-20) {
+    return units::radian_t{std::atan2(sxcy, cxcy)};
+  } else {
+    return 0_rad;
+  }
 }
 
 units::radian_t Rotation3d::Y() const {
@@ -195,7 +211,7 @@
   double y = m_q.Y();
   double z = m_q.Z();
 
-  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
   double ratio = 2.0 * (w * y - z * x);
   if (std::abs(ratio) >= 1.0) {
     return units::radian_t{std::copysign(std::numbers::pi / 2.0, ratio)};
@@ -210,12 +226,18 @@
   double y = m_q.Y();
   double z = m_q.Z();
 
-  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
-  return units::radian_t{
-      std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))};
+  // wpimath/algorithms.md
+  double cycz = 1.0 - 2.0 * (y * y + z * z);
+  double cysz = 2.0 * (w * z + x * y);
+  double cy_sq = cycz * cycz + cysz * cysz;
+  if (cy_sq > 1e-20) {
+    return units::radian_t{std::atan2(cysz, cycz)};
+  } else {
+    return units::radian_t{std::atan2(2.0 * w * z, w * w - z * z)};
+  }
 }
 
-Vectord<3> Rotation3d::Axis() const {
+Eigen::Vector3d Rotation3d::Axis() const {
   double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
   if (norm == 0.0) {
     return {0.0, 0.0, 0.0};
@@ -240,3 +262,21 @@
 void frc::from_json(const wpi::json& json, Rotation3d& rotation) {
   rotation = Rotation3d{json.at("quaternion").get<Quaternion>()};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Rotation3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation3d>(
+      arena);
+}
+
+frc::Rotation3d wpi::Protobuf<frc::Rotation3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufRotation3d*>(&msg);
+  return Rotation3d{wpi::UnpackProtobuf<frc::Quaternion>(m->q())};
+}
+
+void wpi::Protobuf<frc::Rotation3d>::Pack(google::protobuf::Message* msg,
+                                          const frc::Rotation3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufRotation3d*>(msg);
+  wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
index 25b0590..77f3cee 100644
--- a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
@@ -5,6 +5,7 @@
 #include "frc/geometry/Transform2d.h"
 
 #include "frc/geometry/Pose2d.h"
+#include "geometry2d.pb.h"
 
 using namespace frc;
 
@@ -21,3 +22,23 @@
 Transform2d Transform2d::operator+(const Transform2d& other) const {
   return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Transform2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTransform2d>(arena);
+}
+
+frc::Transform2d wpi::Protobuf<frc::Transform2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTransform2d*>(&msg);
+  return Transform2d{wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
+                     wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation())};
+}
+
+void wpi::Protobuf<frc::Transform2d>::Pack(google::protobuf::Message* msg,
+                                           const frc::Transform2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTransform2d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
index 1cfabaa..de6c253 100644
--- a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
@@ -5,6 +5,7 @@
 #include "frc/geometry/Transform3d.h"
 
 #include "frc/geometry/Pose3d.h"
+#include "geometry3d.pb.h"
 
 using namespace frc;
 
@@ -21,6 +22,10 @@
 Transform3d::Transform3d(Translation3d translation, Rotation3d rotation)
     : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
 
+Transform3d::Transform3d(units::meter_t x, units::meter_t y, units::meter_t z,
+                         Rotation3d rotation)
+    : m_translation(x, y, z), m_rotation(std::move(rotation)) {}
+
 Transform3d Transform3d::Inverse() const {
   // We are rotating the difference between the translations
   // using a clockwise rotation matrix. This transforms the global
@@ -31,3 +36,23 @@
 Transform3d Transform3d::operator+(const Transform3d& other) const {
   return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Transform3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTransform3d>(arena);
+}
+
+frc::Transform3d wpi::Protobuf<frc::Transform3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTransform3d*>(&msg);
+  return Transform3d{wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
+                     wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation())};
+}
+
+void wpi::Protobuf<frc::Transform3d>::Pack(google::protobuf::Message* msg,
+                                           const frc::Transform3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTransform3d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
index d463696..6d5f315 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
@@ -6,6 +6,7 @@
 
 #include <wpi/json.h>
 
+#include "geometry2d.pb.h"
 #include "units/math.h"
 
 using namespace frc;
@@ -23,6 +24,22 @@
          units::math::abs(m_y - other.m_y) < 1E-9_m;
 }
 
+Translation2d Translation2d::Nearest(
+    std::span<const Translation2d> translations) const {
+  return *std::min_element(translations.begin(), translations.end(),
+                           [this](Translation2d a, Translation2d b) {
+                             return this->Distance(a) < this->Distance(b);
+                           });
+}
+
+Translation2d Translation2d::Nearest(
+    std::initializer_list<Translation2d> translations) const {
+  return *std::min_element(translations.begin(), translations.end(),
+                           [this](Translation2d a, Translation2d b) {
+                             return this->Distance(a) < this->Distance(b);
+                           });
+}
+
 void frc::to_json(wpi::json& json, const Translation2d& translation) {
   json =
       wpi::json{{"x", translation.X().value()}, {"y", translation.Y().value()}};
@@ -32,3 +49,22 @@
   translation = Translation2d{units::meter_t{json.at("x").get<double>()},
                               units::meter_t{json.at("y").get<double>()}};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Translation2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTranslation2d>(arena);
+}
+
+frc::Translation2d wpi::Protobuf<frc::Translation2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTranslation2d*>(&msg);
+  return frc::Translation2d{units::meter_t{m->x()}, units::meter_t{m->y()}};
+}
+
+void wpi::Protobuf<frc::Translation2d>::Pack(google::protobuf::Message* msg,
+                                             const frc::Translation2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTranslation2d*>(msg);
+  m->set_x(value.X().value());
+  m->set_y(value.Y().value());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
index 2c53791..90e94ae 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
@@ -6,6 +6,7 @@
 
 #include <wpi/json.h>
 
+#include "geometry3d.pb.h"
 #include "units/length.h"
 #include "units/math.h"
 
@@ -52,3 +53,24 @@
                               units::meter_t{json.at("y").get<double>()},
                               units::meter_t{json.at("z").get<double>()}};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Translation3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTranslation3d>(arena);
+}
+
+frc::Translation3d wpi::Protobuf<frc::Translation3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTranslation3d*>(&msg);
+  return frc::Translation3d{units::meter_t{m->x()}, units::meter_t{m->y()},
+                            units::meter_t{m->z()}};
+}
+
+void wpi::Protobuf<frc::Translation3d>::Pack(google::protobuf::Message* msg,
+                                             const frc::Translation3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTranslation3d*>(msg);
+  m->set_x(value.X().value());
+  m->set_y(value.Y().value());
+  m->set_z(value.Z().value());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Twist2d.cpp b/wpimath/src/main/native/cpp/geometry/Twist2d.cpp
new file mode 100644
index 0000000..6c106eb
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Twist2d.cpp
@@ -0,0 +1,30 @@
+// 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 "frc/geometry/Twist2d.h"
+
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+google::protobuf::Message* wpi::Protobuf<frc::Twist2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist2d>(
+      arena);
+}
+
+frc::Twist2d wpi::Protobuf<frc::Twist2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTwist2d*>(&msg);
+  return frc::Twist2d{units::meter_t{m->dx()}, units::meter_t{m->dy()},
+                      units::radian_t{m->dtheta()}};
+}
+
+void wpi::Protobuf<frc::Twist2d>::Pack(google::protobuf::Message* msg,
+                                       const frc::Twist2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTwist2d*>(msg);
+  m->set_dx(value.dx.value());
+  m->set_dy(value.dy.value());
+  m->set_dtheta(value.dtheta.value());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Twist3d.cpp b/wpimath/src/main/native/cpp/geometry/Twist3d.cpp
new file mode 100644
index 0000000..4f4ce86
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Twist3d.cpp
@@ -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.
+
+#include "frc/geometry/Twist3d.h"
+
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+google::protobuf::Message* wpi::Protobuf<frc::Twist3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist3d>(
+      arena);
+}
+
+frc::Twist3d wpi::Protobuf<frc::Twist3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTwist3d*>(&msg);
+  return frc::Twist3d{units::meter_t{m->dx()},  units::meter_t{m->dy()},
+                      units::meter_t{m->dz()},  units::radian_t{m->rx()},
+                      units::radian_t{m->ry()}, units::radian_t{m->rz()}};
+}
+
+void wpi::Protobuf<frc::Twist3d>::Pack(google::protobuf::Message* msg,
+                                       const frc::Twist3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTwist3d*>(msg);
+  m->set_dx(value.dx.value());
+  m->set_dy(value.dy.value());
+  m->set_dz(value.dz.value());
+  m->set_rx(value.rx.value());
+  m->set_ry(value.ry.value());
+  m->set_rz(value.rz.value());
+}
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
deleted file mode 100644
index fd8a223..0000000
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
+++ /dev/null
@@ -1,367 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-#include <jni.h>
-
-#include <exception>
-
-#include <wpi/jni_util.h>
-
-#include "Eigen/Cholesky"
-#include "Eigen/Core"
-#include "Eigen/Eigenvalues"
-#include "Eigen/QR"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-#include "edu_wpi_first_math_WPIMathJNI.h"
-#include "frc/trajectory/TrajectoryUtil.h"
-#include "unsupported/Eigen/MatrixFunctions"
-
-using namespace wpi::java;
-
-/**
- * Returns true if (A, B) is a stabilizable pair.
- *
- * (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
- * any, have absolute values less than one, where an eigenvalue is
- * uncontrollable if rank(λI - A, B) < n where n is the number of states.
- *
- * @param A System matrix.
- * @param B Input matrix.
- */
-bool check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
-                        const Eigen::Ref<const Eigen::MatrixXd>& B) {
-  int states = B.rows();
-  int inputs = B.cols();
-  Eigen::EigenSolver<Eigen::MatrixXd> es{A};
-  for (int i = 0; i < states; ++i) {
-    if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
-            es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
-        1) {
-      continue;
-    }
-
-    Eigen::MatrixXcd E{states, states + inputs};
-    E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(states, states) - A,
-        B;
-    Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
-    if (qr.rank() < states) {
-      return false;
-    }
-  }
-
-  return true;
-}
-
-std::vector<double> GetElementsFromTrajectory(
-    const frc::Trajectory& trajectory) {
-  std::vector<double> elements;
-  elements.reserve(trajectory.States().size() * 7);
-
-  for (auto&& state : trajectory.States()) {
-    elements.push_back(state.t.value());
-    elements.push_back(state.velocity.value());
-    elements.push_back(state.acceleration.value());
-    elements.push_back(state.pose.X().value());
-    elements.push_back(state.pose.Y().value());
-    elements.push_back(state.pose.Rotation().Radians().value());
-    elements.push_back(state.curvature.value());
-  }
-
-  return elements;
-}
-
-frc::Trajectory CreateTrajectoryFromElements(std::span<const double> elements) {
-  // Make sure that the elements have the correct length.
-  assert(elements.size() % 7 == 0);
-
-  // Create a vector of states from the elements.
-  std::vector<frc::Trajectory::State> states;
-  states.reserve(elements.size() / 7);
-
-  for (size_t i = 0; i < elements.size(); i += 7) {
-    states.emplace_back(frc::Trajectory::State{
-        units::second_t{elements[i]},
-        units::meters_per_second_t{elements[i + 1]},
-        units::meters_per_second_squared_t{elements[i + 2]},
-        frc::Pose2d{units::meter_t{elements[i + 3]},
-                    units::meter_t{elements[i + 4]},
-                    units::radian_t{elements[i + 5]}},
-        units::curvature_t{elements[i + 6]}});
-  }
-
-  return frc::Trajectory(states);
-}
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    discreteAlgebraicRiccatiEquation
- * Signature: ([D[D[D[DII[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_discreteAlgebraicRiccatiEquation
-  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
-   jdoubleArray R, jint states, jint inputs, jdoubleArray S)
-{
-  jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
-  jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
-  jdouble* nativeQ = env->GetDoubleArrayElements(Q, nullptr);
-  jdouble* nativeR = env->GetDoubleArrayElements(R, nullptr);
-
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Amat{nativeA, states, states};
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Bmat{nativeB, states, inputs};
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Qmat{nativeQ, states, states};
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Rmat{nativeR, inputs, inputs};
-
-  try {
-    Eigen::MatrixXd result =
-        drake::math::DiscreteAlgebraicRiccatiEquation(Amat, Bmat, Qmat, Rmat);
-
-    env->ReleaseDoubleArrayElements(A, nativeA, 0);
-    env->ReleaseDoubleArrayElements(B, nativeB, 0);
-    env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
-    env->ReleaseDoubleArrayElements(R, nativeR, 0);
-
-    env->SetDoubleArrayRegion(S, 0, states * states, result.data());
-  } catch (const std::runtime_error& e) {
-    jclass cls = env->FindClass("java/lang/RuntimeException");
-    if (cls) {
-      env->ThrowNew(cls, e.what());
-    }
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    exp
- * Signature: ([DI[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_exp
-  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst)
-{
-  jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr);
-
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Amat{arrayBody, rows, rows};
-  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Aexp =
-      Amat.exp();
-
-  env->ReleaseDoubleArrayElements(src, arrayBody, 0);
-  env->SetDoubleArrayRegion(dst, 0, rows * rows, Aexp.data());
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    pow
- * Signature: ([DID[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_pow
-  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdouble exponent,
-   jdoubleArray dst)
-{
-  jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr);
-
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Amat{arrayBody, rows, rows};  // NOLINT
-  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Apow =
-      Amat.pow(exponent);
-
-  env->ReleaseDoubleArrayElements(src, arrayBody, 0);
-  env->SetDoubleArrayRegion(dst, 0, rows * rows, Apow.data());
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    isStabilizable
- * Signature: (II[D[D)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
-  (JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc,
-   jdoubleArray bSrc)
-{
-  jdouble* nativeA = env->GetDoubleArrayElements(aSrc, nullptr);
-  jdouble* nativeB = env->GetDoubleArrayElements(bSrc, nullptr);
-
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      A{nativeA, states, states};
-
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      B{nativeB, states, inputs};
-
-  bool isStabilizable = check_stabilizable(A, B);
-
-  env->ReleaseDoubleArrayElements(aSrc, nativeA, 0);
-  env->ReleaseDoubleArrayElements(bSrc, nativeB, 0);
-
-  return isStabilizable;
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    fromPathweaverJson
- * Signature: (Ljava/lang/String;)[D
- */
-JNIEXPORT jdoubleArray JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
-  (JNIEnv* env, jclass, jstring path)
-{
-  try {
-    auto trajectory =
-        frc::TrajectoryUtil::FromPathweaverJson(JStringRef{env, path}.c_str());
-    std::vector<double> elements = GetElementsFromTrajectory(trajectory);
-    return MakeJDoubleArray(env, elements);
-  } catch (std::exception& e) {
-    jclass cls = env->FindClass("java/io/IOException");
-    if (cls) {
-      env->ThrowNew(cls, e.what());
-    }
-    return nullptr;
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    toPathweaverJson
- * Signature: ([DLjava/lang/String;)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
-  (JNIEnv* env, jclass, jdoubleArray elements, jstring path)
-{
-  try {
-    auto trajectory =
-        CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
-    frc::TrajectoryUtil::ToPathweaverJson(trajectory,
-                                          JStringRef{env, path}.c_str());
-  } catch (std::exception& e) {
-    jclass cls = env->FindClass("java/io/IOException");
-    if (cls) {
-      env->ThrowNew(cls, e.what());
-    }
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    deserializeTrajectory
- * Signature: (Ljava/lang/String;)[D
- */
-JNIEXPORT jdoubleArray JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
-  (JNIEnv* env, jclass, jstring json)
-{
-  try {
-    auto trajectory = frc::TrajectoryUtil::DeserializeTrajectory(
-        JStringRef{env, json}.c_str());
-    std::vector<double> elements = GetElementsFromTrajectory(trajectory);
-    return MakeJDoubleArray(env, elements);
-  } catch (std::exception& e) {
-    jclass cls = env->FindClass(
-        "edu/wpi/first/math/trajectory/TrajectoryUtil$"
-        "TrajectorySerializationException");
-    if (cls) {
-      env->ThrowNew(cls, e.what());
-    }
-    return nullptr;
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    serializeTrajectory
- * Signature: ([D)Ljava/lang/String;
- */
-JNIEXPORT jstring JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
-  (JNIEnv* env, jclass, jdoubleArray elements)
-{
-  try {
-    auto trajectory =
-        CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
-    return MakeJString(env,
-                       frc::TrajectoryUtil::SerializeTrajectory(trajectory));
-  } catch (std::exception& e) {
-    jclass cls = env->FindClass(
-        "edu/wpi/first/math/trajectory/TrajectoryUtil$"
-        "TrajectorySerializationException");
-    if (cls) {
-      env->ThrowNew(cls, e.what());
-    }
-    return nullptr;
-  }
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    rankUpdate
- * Signature: ([DI[DDZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_rankUpdate
-  (JNIEnv* env, jclass, jdoubleArray mat, jint rows, jdoubleArray vec,
-   jdouble sigma, jboolean lowerTriangular)
-{
-  jdouble* matBody = env->GetDoubleArrayElements(mat, nullptr);
-  jdouble* vecBody = env->GetDoubleArrayElements(vec, nullptr);
-
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      L{matBody, rows, rows};
-  Eigen::Map<Eigen::Vector<double, Eigen::Dynamic>> v{vecBody, rows};
-
-  if (lowerTriangular == JNI_TRUE) {
-    Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(L, v, sigma);
-  } else {
-    Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(L, v, sigma);
-  }
-
-  env->ReleaseDoubleArrayElements(mat, matBody, 0);
-  env->ReleaseDoubleArrayElements(vec, vecBody, 0);
-}
-
-/*
- * Class:     edu_wpi_first_math_WPIMathJNI
- * Method:    solveFullPivHouseholderQr
- * Signature: ([DII[DII[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_math_WPIMathJNI_solveFullPivHouseholderQr
-  (JNIEnv* env, jclass, jdoubleArray A, jint Arows, jint Acols, jdoubleArray B,
-   jint Brows, jint Bcols, jdoubleArray dst)
-{
-  jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
-  jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
-
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Amat{nativeA, Arows, Acols};
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Bmat{nativeB, Brows, Bcols};
-
-  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Xmat =
-      Amat.fullPivHouseholderQr().solve(Bmat);
-
-  env->ReleaseDoubleArrayElements(A, nativeA, 0);
-  env->ReleaseDoubleArrayElements(B, nativeB, 0);
-  env->SetDoubleArrayRegion(dst, 0, Brows * Bcols, Xmat.data());
-}
-
-}  // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
new file mode 100644
index 0000000..e5c3ac5
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
@@ -0,0 +1,177 @@
+// 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 <jni.h>
+
+#include <stdexcept>
+
+#include <wpi/jni_util.h>
+
+#include "WPIMathJNI_Exceptions.h"
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/DARE.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    dareDetailABQR
+ * Signature: ([D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQR
+  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+   jdoubleArray R, jint states, jint inputs, jdoubleArray S)
+{
+  JSpan<const jdouble> nativeA{env, A};
+  JSpan<const jdouble> nativeB{env, B};
+  JSpan<const jdouble> nativeQ{env, Q};
+  JSpan<const jdouble> nativeR{env, R};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.data(), states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Qmat{nativeQ.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Rmat{nativeR.data(), inputs, inputs};
+
+  Eigen::MatrixXd RmatCopy{Rmat};
+  auto R_llt = RmatCopy.llt();
+
+  Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
+      Amat, Bmat, Qmat, R_llt);
+
+  env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    dareDetailABQRN
+ * Signature: ([D[D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQRN
+  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+   jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
+{
+  JSpan<const jdouble> nativeA{env, A};
+  JSpan<const jdouble> nativeB{env, B};
+  JSpan<const jdouble> nativeQ{env, Q};
+  JSpan<const jdouble> nativeR{env, R};
+  JSpan<const jdouble> nativeN{env, N};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.data(), states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Qmat{nativeQ.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Rmat{nativeR.data(), inputs, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Nmat{nativeN.data(), states, inputs};
+
+  Eigen::MatrixXd Rcopy{Rmat};
+  auto R_llt = Rcopy.llt();
+
+  Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
+      Amat, Bmat, Qmat, R_llt, Nmat);
+
+  env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    dareABQR
+ * Signature: ([D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareABQR
+  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+   jdoubleArray R, jint states, jint inputs, jdoubleArray S)
+{
+  JSpan<const jdouble> nativeA{env, A};
+  JSpan<const jdouble> nativeB{env, B};
+  JSpan<const jdouble> nativeQ{env, Q};
+  JSpan<const jdouble> nativeR{env, R};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.data(), states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Qmat{nativeQ.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Rmat{nativeR.data(), inputs, inputs};
+
+  try {
+    Eigen::MatrixXd result =
+        frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat);
+
+    env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+  } catch (const std::invalid_argument& e) {
+    illegalArgEx.Throw(env, e.what());
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    dareABQRN
+ * Signature: ([D[D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareABQRN
+  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+   jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
+{
+  JSpan<const jdouble> nativeA{env, A};
+  JSpan<const jdouble> nativeB{env, B};
+  JSpan<const jdouble> nativeQ{env, Q};
+  JSpan<const jdouble> nativeR{env, R};
+  JSpan<const jdouble> nativeN{env, N};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.data(), states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Qmat{nativeQ.data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Rmat{nativeR.data(), inputs, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Nmat{nativeN.data(), states, inputs};
+
+  try {
+    Eigen::MatrixXd result =
+        frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat, Nmat);
+
+    env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+  } catch (const std::invalid_argument& e) {
+    illegalArgEx.Throw(env, e.what());
+  }
+}
+
+}  // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp
new file mode 100644
index 0000000..642a2ba
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp
@@ -0,0 +1,112 @@
+// 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 <jni.h>
+
+#include <Eigen/Cholesky>
+#include <Eigen/Core>
+#include <Eigen/QR>
+#include <unsupported/Eigen/MatrixFunctions>
+#include <wpi/jni_util.h>
+
+#include "edu_wpi_first_math_WPIMathJNI.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    exp
+ * Signature: ([DI[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_exp
+  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst)
+{
+  JSpan<const jdouble> arrayBody{env, src};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{arrayBody.data(), rows, rows};
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Aexp =
+      Amat.exp();
+
+  env->SetDoubleArrayRegion(dst, 0, rows * rows, Aexp.data());
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    pow
+ * Signature: ([DID[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_pow
+  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdouble exponent,
+   jdoubleArray dst)
+{
+  JSpan<const jdouble> arrayBody{env, src};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{arrayBody.data(), rows, rows};
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Apow =
+      Amat.pow(exponent);
+
+  env->SetDoubleArrayRegion(dst, 0, rows * rows, Apow.data());
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    rankUpdate
+ * Signature: ([DI[DDZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_rankUpdate
+  (JNIEnv* env, jclass, jdoubleArray mat, jint rows, jdoubleArray vec,
+   jdouble sigma, jboolean lowerTriangular)
+{
+  JSpan<jdouble> matBody{env, mat};
+  JSpan<const jdouble> vecBody{env, vec};
+
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      L{matBody.data(), rows, rows};
+  Eigen::Map<const Eigen::Vector<double, Eigen::Dynamic>> v{vecBody.data(),
+                                                            rows};
+
+  if (lowerTriangular == JNI_TRUE) {
+    Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(L, v, sigma);
+  } else {
+    Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(L, v, sigma);
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    solveFullPivHouseholderQr
+ * Signature: ([DII[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_solveFullPivHouseholderQr
+  (JNIEnv* env, jclass, jdoubleArray A, jint Arows, jint Acols, jdoubleArray B,
+   jint Brows, jint Bcols, jdoubleArray dst)
+{
+  JSpan<const jdouble> nativeA{env, A};
+  JSpan<const jdouble> nativeB{env, B};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.data(), Arows, Acols};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.data(), Brows, Bcols};
+
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Xmat =
+      Amat.fullPivHouseholderQr().solve(Bmat);
+
+  env->SetDoubleArrayRegion(dst, 0, Brows * Bcols, Xmat.data());
+}
+
+}  // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp
new file mode 100644
index 0000000..7f5a03f
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp
@@ -0,0 +1,56 @@
+// 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 "WPIMathJNI_Exceptions.h"
+
+#include <jni.h>
+
+using namespace wpi::java;
+
+//
+// Globals and load/unload
+//
+
+JException illegalArgEx;
+JException ioEx;
+JException trajectorySerializationEx;
+
+static const JExceptionInit exceptions[] = {
+    {"java/lang/IllegalArgumentException", &illegalArgEx},
+    {"java/io/IOException", &ioEx},
+    {"edu/wpi/first/math/trajectory/"
+     "TrajectoryUtil$TrajectorySerializationException",
+     &trajectorySerializationEx}};
+
+extern "C" {
+
+JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
+  JNIEnv* env;
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
+    return JNI_ERR;
+  }
+
+  // Cache references to exceptions
+  for (auto& c : exceptions) {
+    *c.cls = JException(env, c.name);
+    if (!*c.cls) {
+      return JNI_ERR;
+    }
+  }
+
+  return JNI_VERSION_1_6;
+}
+
+JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) {
+  JNIEnv* env;
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
+    return;
+  }
+  // Delete global references
+  for (auto& c : exceptions) {
+    c.cls->free(env);
+  }
+}
+
+}  // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.h b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.h
new file mode 100644
index 0000000..cf0e149
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.h
@@ -0,0 +1,11 @@
+// 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 <wpi/jni_util.h>
+
+extern wpi::java::JException illegalArgEx;
+extern wpi::java::JException ioEx;
+extern wpi::java::JException trajectorySerializationEx;
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp
new file mode 100644
index 0000000..f985231
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp
@@ -0,0 +1,70 @@
+// 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 <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/geometry/Pose3d.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    expPose3d
+ * Signature: (DDDDDDDDDDDDD)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_expPose3d
+  (JNIEnv* env, jclass, jdouble poseX, jdouble poseY, jdouble poseZ,
+   jdouble poseQw, jdouble poseQx, jdouble poseQy, jdouble poseQz,
+   jdouble twistDx, jdouble twistDy, jdouble twistDz, jdouble twistRx,
+   jdouble twistRy, jdouble twistRz)
+{
+  frc::Pose3d pose{
+      units::meter_t{poseX}, units::meter_t{poseY}, units::meter_t{poseZ},
+      frc::Rotation3d{frc::Quaternion{poseQw, poseQx, poseQy, poseQz}}};
+  frc::Twist3d twist{units::meter_t{twistDx},  units::meter_t{twistDy},
+                     units::meter_t{twistDz},  units::radian_t{twistRx},
+                     units::radian_t{twistRy}, units::radian_t{twistRz}};
+
+  frc::Pose3d result = pose.Exp(twist);
+
+  const auto& resultQuaternion = result.Rotation().GetQuaternion();
+  return MakeJDoubleArray(
+      env, {{result.X().value(), result.Y().value(), result.Z().value(),
+             resultQuaternion.W(), resultQuaternion.X(), resultQuaternion.Y(),
+             resultQuaternion.Z()}});
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    logPose3d
+ * Signature: (DDDDDDDDDDDDDD)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_logPose3d
+  (JNIEnv* env, jclass, jdouble startX, jdouble startY, jdouble startZ,
+   jdouble startQw, jdouble startQx, jdouble startQy, jdouble startQz,
+   jdouble endX, jdouble endY, jdouble endZ, jdouble endQw, jdouble endQx,
+   jdouble endQy, jdouble endQz)
+{
+  frc::Pose3d startPose{
+      units::meter_t{startX}, units::meter_t{startY}, units::meter_t{startZ},
+      frc::Rotation3d{frc::Quaternion{startQw, startQx, startQy, startQz}}};
+  frc::Pose3d endPose{
+      units::meter_t{endX}, units::meter_t{endY}, units::meter_t{endZ},
+      frc::Rotation3d{frc::Quaternion{endQw, endQx, endQy, endQz}}};
+
+  frc::Twist3d result = startPose.Log(endPose);
+
+  return MakeJDoubleArray(
+      env, {{result.dx.value(), result.dy.value(), result.dz.value(),
+             result.rx.value(), result.ry.value(), result.rz.value()}});
+}
+
+}  // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp
new file mode 100644
index 0000000..dc3dfdd
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp
@@ -0,0 +1,44 @@
+// 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 <jni.h>
+
+#include <Eigen/Core>
+#include <wpi/jni_util.h>
+
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/StateSpaceUtil.h"
+
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    isStabilizable
+ * Signature: (II[D[D)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
+  (JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc,
+   jdoubleArray bSrc)
+{
+  JSpan<const jdouble> nativeA{env, aSrc};
+  JSpan<const jdouble> nativeB{env, bSrc};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      A{nativeA.data(), states, states};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      B{nativeB.data(), states, inputs};
+
+  bool isStabilizable =
+      frc::IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(A, B);
+
+  return isStabilizable;
+}
+
+}  // extern "C"
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp
new file mode 100644
index 0000000..3359da9
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp
@@ -0,0 +1,138 @@
+// 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 <jni.h>
+
+#include <exception>
+
+#include <wpi/jni_util.h>
+
+#include "WPIMathJNI_Exceptions.h"
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/trajectory/TrajectoryUtil.h"
+
+using namespace wpi::java;
+
+std::vector<double> GetElementsFromTrajectory(
+    const frc::Trajectory& trajectory) {
+  std::vector<double> elements;
+  elements.reserve(trajectory.States().size() * 7);
+
+  for (auto&& state : trajectory.States()) {
+    elements.push_back(state.t.value());
+    elements.push_back(state.velocity.value());
+    elements.push_back(state.acceleration.value());
+    elements.push_back(state.pose.X().value());
+    elements.push_back(state.pose.Y().value());
+    elements.push_back(state.pose.Rotation().Radians().value());
+    elements.push_back(state.curvature.value());
+  }
+
+  return elements;
+}
+
+frc::Trajectory CreateTrajectoryFromElements(std::span<const double> elements) {
+  // Make sure that the elements have the correct length.
+  assert(elements.size() % 7 == 0);
+
+  // Create a vector of states from the elements.
+  std::vector<frc::Trajectory::State> states;
+  states.reserve(elements.size() / 7);
+
+  for (size_t i = 0; i < elements.size(); i += 7) {
+    states.emplace_back(frc::Trajectory::State{
+        units::second_t{elements[i]},
+        units::meters_per_second_t{elements[i + 1]},
+        units::meters_per_second_squared_t{elements[i + 2]},
+        frc::Pose2d{units::meter_t{elements[i + 3]},
+                    units::meter_t{elements[i + 4]},
+                    units::radian_t{elements[i + 5]}},
+        units::curvature_t{elements[i + 6]}});
+  }
+
+  return frc::Trajectory(states);
+}
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    fromPathweaverJson
+ * Signature: (Ljava/lang/String;)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
+  (JNIEnv* env, jclass, jstring path)
+{
+  try {
+    auto trajectory =
+        frc::TrajectoryUtil::FromPathweaverJson(JStringRef{env, path}.c_str());
+    std::vector<double> elements = GetElementsFromTrajectory(trajectory);
+    return MakeJDoubleArray(env, elements);
+  } catch (std::exception& e) {
+    ioEx.Throw(env, e.what());
+    return nullptr;
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    toPathweaverJson
+ * Signature: ([DLjava/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
+  (JNIEnv* env, jclass, jdoubleArray elements, jstring path)
+{
+  try {
+    auto trajectory =
+        CreateTrajectoryFromElements(JSpan<const jdouble>{env, elements});
+    frc::TrajectoryUtil::ToPathweaverJson(trajectory,
+                                          JStringRef{env, path}.c_str());
+  } catch (std::exception& e) {
+    ioEx.Throw(env, e.what());
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    deserializeTrajectory
+ * Signature: (Ljava/lang/String;)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
+  (JNIEnv* env, jclass, jstring json)
+{
+  try {
+    auto trajectory = frc::TrajectoryUtil::DeserializeTrajectory(
+        JStringRef{env, json}.c_str());
+    std::vector<double> elements = GetElementsFromTrajectory(trajectory);
+    return MakeJDoubleArray(env, elements);
+  } catch (std::exception& e) {
+    trajectorySerializationEx.Throw(env, e.what());
+    return nullptr;
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    serializeTrajectory
+ * Signature: ([D)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
+  (JNIEnv* env, jclass, jdoubleArray elements)
+{
+  try {
+    auto trajectory =
+        CreateTrajectoryFromElements(JSpan<const jdouble>{env, elements});
+    return MakeJString(env,
+                       frc::TrajectoryUtil::SerializeTrajectory(trajectory));
+  } catch (std::exception& e) {
+    trajectorySerializationEx.Throw(env, e.what());
+    return nullptr;
+  }
+}
+
+}  // extern "C"
diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
index 1ff7a8a..346a232 100644
--- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
@@ -11,32 +11,9 @@
 DifferentialDriveOdometry::DifferentialDriveOdometry(
     const Rotation2d& gyroAngle, units::meter_t leftDistance,
     units::meter_t rightDistance, const Pose2d& initialPose)
-    : m_pose(initialPose),
-      m_prevLeftDistance(leftDistance),
-      m_prevRightDistance(rightDistance) {
-  m_previousAngle = m_pose.Rotation();
-  m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    : Odometry<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions>(
+          m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance},
+          initialPose) {
   wpi::math::MathSharedStore::ReportUsage(
       wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1);
 }
-
-const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
-                                                units::meter_t leftDistance,
-                                                units::meter_t rightDistance) {
-  auto deltaLeftDistance = leftDistance - m_prevLeftDistance;
-  auto deltaRightDistance = rightDistance - m_prevRightDistance;
-
-  m_prevLeftDistance = leftDistance;
-  m_prevRightDistance = rightDistance;
-
-  auto averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
-  auto angle = gyroAngle + m_gyroOffset;
-
-  auto newPose = m_pose.Exp(
-      {averageDeltaDistance, 0_m, (angle - m_previousAngle).Radians()});
-
-  m_previousAngle = angle;
-  m_pose = {newPose.Translation(), angle};
-
-  return m_pose;
-}
diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
index 298dd7f..c21a7b2 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
@@ -25,7 +25,7 @@
                                       chassisSpeeds.vy.value(),
                                       chassisSpeeds.omega.value()};
 
-  Vectord<4> wheelsVector = m_inverseKinematics * chassisSpeedsVector;
+  Eigen::Vector4d wheelsVector = m_inverseKinematics * chassisSpeedsVector;
 
   MecanumDriveWheelSpeeds wheelSpeeds;
   wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsVector(0)};
@@ -37,7 +37,7 @@
 
 ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
     const MecanumDriveWheelSpeeds& wheelSpeeds) const {
-  Vectord<4> wheelSpeedsVector{
+  Eigen::Vector4d wheelSpeedsVector{
       wheelSpeeds.frontLeft.value(), wheelSpeeds.frontRight.value(),
       wheelSpeeds.rearLeft.value(), wheelSpeeds.rearRight.value()};
 
@@ -50,15 +50,30 @@
 }
 
 Twist2d MecanumDriveKinematics::ToTwist2d(
+    const MecanumDriveWheelPositions& start,
+    const MecanumDriveWheelPositions& end) const {
+  Eigen::Vector4d wheelDeltasVector{
+      end.frontLeft.value() - start.frontLeft.value(),
+      end.frontRight.value() - start.frontRight.value(),
+      end.rearLeft.value() - start.rearLeft.value(),
+      end.rearRight.value() - start.rearRight.value()};
+
+  Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
+
+  return {units::meter_t{twistVector(0)}, units::meter_t{twistVector(1)},
+          units::radian_t{twistVector(2)}};
+}
+
+Twist2d MecanumDriveKinematics::ToTwist2d(
     const MecanumDriveWheelPositions& wheelDeltas) const {
-  Vectord<4> wheelDeltasVector{
+  Eigen::Vector4d wheelDeltasVector{
       wheelDeltas.frontLeft.value(), wheelDeltas.frontRight.value(),
       wheelDeltas.rearLeft.value(), wheelDeltas.rearRight.value()};
 
   Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
 
-  return {units::meter_t{twistVector(0)},  // NOLINT
-          units::meter_t{twistVector(1)}, units::radian_t{twistVector(2)}};
+  return {units::meter_t{twistVector(0)}, units::meter_t{twistVector(1)},
+          units::radian_t{twistVector(2)}};
 }
 
 void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
index 394adaf..55055be 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
@@ -11,35 +11,9 @@
 MecanumDriveOdometry::MecanumDriveOdometry(
     MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
     const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
-    : m_kinematics(kinematics),
-      m_pose(initialPose),
-      m_previousWheelPositions(wheelPositions) {
-  m_previousAngle = m_pose.Rotation();
-  m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    : Odometry<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
+          m_kinematicsImpl, gyroAngle, wheelPositions, initialPose),
+      m_kinematicsImpl(kinematics) {
   wpi::math::MathSharedStore::ReportUsage(
       wpi::math::MathUsageId::kOdometry_MecanumDrive, 1);
 }
-
-const Pose2d& MecanumDriveOdometry::Update(
-    const Rotation2d& gyroAngle,
-    const MecanumDriveWheelPositions& wheelPositions) {
-  auto angle = gyroAngle + m_gyroOffset;
-
-  MecanumDriveWheelPositions wheelDeltas{
-      wheelPositions.frontLeft - m_previousWheelPositions.frontLeft,
-      wheelPositions.frontRight - m_previousWheelPositions.frontRight,
-      wheelPositions.rearLeft - m_previousWheelPositions.rearLeft,
-      wheelPositions.rearRight - m_previousWheelPositions.rearRight,
-  };
-
-  auto twist = m_kinematics.ToTwist2d(wheelDeltas);
-  twist.dtheta = (angle - m_previousAngle).Radians();
-
-  auto newPose = m_pose.Exp(twist);
-
-  m_previousAngle = angle;
-  m_previousWheelPositions = wheelPositions;
-  m_pose = {newPose.Translation(), angle};
-
-  return m_pose;
-}
diff --git a/wpimath/src/main/native/cpp/kinematics/SwerveModulePosition.cpp b/wpimath/src/main/native/cpp/kinematics/SwerveModulePosition.cpp
new file mode 100644
index 0000000..5343de9
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/SwerveModulePosition.cpp
@@ -0,0 +1,15 @@
+// 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 "frc/kinematics/SwerveModulePosition.h"
+
+#include "frc/kinematics/SwerveModuleState.h"
+#include "units/math.h"
+
+using namespace frc;
+
+bool SwerveModulePosition::operator==(const SwerveModulePosition& other) const {
+  return units::math::abs(distance - other.distance) < 1E-9_m &&
+         angle == other.angle;
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/SwerveModuleState.cpp b/wpimath/src/main/native/cpp/kinematics/SwerveModuleState.cpp
new file mode 100644
index 0000000..071d53a
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/SwerveModuleState.cpp
@@ -0,0 +1,24 @@
+// 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 "frc/kinematics/SwerveModuleState.h"
+
+#include "units/math.h"
+
+using namespace frc;
+
+bool SwerveModuleState::operator==(const SwerveModuleState& other) const {
+  return units::math::abs(speed - other.speed) < 1E-9_mps &&
+         angle == other.angle;
+}
+
+SwerveModuleState SwerveModuleState::Optimize(
+    const SwerveModuleState& desiredState, const Rotation2d& currentAngle) {
+  auto delta = desiredState.angle - currentAngle;
+  if (units::math::abs(delta.Degrees()) > 90_deg) {
+    return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}};
+  } else {
+    return {desiredState.speed, desiredState.angle};
+  }
+}
diff --git a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
index de47547..5393f83 100644
--- a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
@@ -5,6 +5,7 @@
 #include "frc/trajectory/Trajectory.h"
 
 #include <algorithm>
+#include <stdexcept>
 
 #include <wpi/MathExtras.h>
 #include <wpi/json.h>
@@ -54,10 +55,20 @@
 }
 
 Trajectory::Trajectory(const std::vector<State>& states) : m_states(states) {
+  if (m_states.empty()) {
+    throw std::invalid_argument(
+        "Trajectory manually initialized with no states.");
+  }
+
   m_totalTime = states.back().t;
 }
 
 Trajectory::State Trajectory::Sample(units::second_t t) const {
+  if (m_states.empty()) {
+    throw std::runtime_error(
+        "Trajectory cannot be sampled if it has no states.");
+  }
+
   if (t <= m_states.front().t) {
     return m_states.front();
   }
diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
index 92d52ed..4c0a55e 100644
--- a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
@@ -223,10 +223,9 @@
 
     if (minMaxAccel.minAcceleration > minMaxAccel.maxAcceleration) {
       throw std::runtime_error(
-          "The constraint's min acceleration was greater than its max "
-          "acceleration. To debug this, remove all constraints from the config "
-          "and add each one individually. If the offending constraint was "
-          "packaged with WPILib, please file a bug report.");
+          "There was an infeasible trajectory constraint. To determine which "
+          "one, remove all constraints from the TrajectoryConfig and add them "
+          "back one-by-one.");
     }
 
     state->minAcceleration = units::math::max(
diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
index 169b642..da9c955 100644
--- a/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
@@ -7,9 +7,8 @@
 #include <system_error>
 
 #include <fmt/format.h>
-#include <wpi/SmallString.h>
+#include <wpi/MemoryBuffer.h>
 #include <wpi/json.h>
-#include <wpi/raw_istream.h>
 #include <wpi/raw_ostream.h>
 
 using namespace frc;
@@ -29,15 +28,14 @@
 }
 
 Trajectory TrajectoryUtil::FromPathweaverJson(std::string_view path) {
-  std::error_code error_code;
-
-  wpi::raw_fd_istream input{path, error_code};
-  if (error_code) {
+  std::error_code ec;
+  std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
+      wpi::MemoryBuffer::GetFile(path, ec);
+  if (fileBuffer == nullptr || ec) {
     throw std::runtime_error(fmt::format("Cannot open file: {}", path));
   }
 
-  wpi::json json;
-  input >> json;
+  wpi::json json = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
 
   return Trajectory{json.get<std::vector<Trajectory::State>>()};
 }
diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
index 2a308db..460ff8b 100644
--- a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
@@ -7,9 +7,8 @@
 using namespace frc;
 
 DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint(
-    const DifferentialDriveKinematics& kinematics,
-    units::meters_per_second_t maxSpeed)
-    : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+    DifferentialDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
+    : m_kinematics(std::move(kinematics)), m_maxSpeed(maxSpeed) {}
 
 units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
     const Pose2d& pose, units::curvature_t curvature,
diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
index d60b4b6..46c306e 100644
--- a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
@@ -15,9 +15,9 @@
 
 DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint(
     const SimpleMotorFeedforward<units::meter>& feedforward,
-    const DifferentialDriveKinematics& kinematics, units::volt_t maxVoltage)
+    DifferentialDriveKinematics kinematics, units::volt_t maxVoltage)
     : m_feedforward(feedforward),
-      m_kinematics(kinematics),
+      m_kinematics(std::move(kinematics)),
       m_maxVoltage(maxVoltage) {}
 
 units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(
diff --git a/wpimath/src/main/native/include/frc/DARE.h b/wpimath/src/main/native/include/frc/DARE.h
new file mode 100644
index 0000000..6a3104e
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/DARE.h
@@ -0,0 +1,406 @@
+// 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 <stdexcept>
+#include <string>
+
+#include <Eigen/Cholesky>
+#include <Eigen/Core>
+#include <Eigen/LU>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/fmt/Eigen.h"
+
+// Works cited:
+//
+// [1] E. K.-W. Chu, H.-Y. Fan, W.-W. Lin & C.-S. Wang "Structure-Preserving
+//     Algorithms for Periodic Discrete-Time Algebraic Riccati Equations",
+//     International Journal of Control, 77:8, 767-788, 2004.
+//     DOI: 10.1080/00207170410001714988
+
+namespace frc {
+
+namespace detail {
+
+/**
+ * Checks the preconditions of A, B, and Q for the DARE solver.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @param A The system matrix.
+ * @param B The input matrix.
+ * @param Q The state cost matrix.
+ * @throws std::invalid_argument if Q isn't symmetric positive semidefinite.
+ * @throws std::invalid_argument if the (A, B) pair isn't stabilizable.
+ * @throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't
+ *   detectable.
+ */
+template <int States, int Inputs>
+void CheckDARE_ABQ(const Eigen::Matrix<double, States, States>& A,
+                   const Eigen::Matrix<double, States, Inputs>& B,
+                   const Eigen::Matrix<double, States, States>& Q) {
+  // Require Q be symmetric
+  if ((Q - Q.transpose()).norm() > 1e-10) {
+    std::string msg = fmt::format("Q isn't symmetric!\n\nQ =\n{}\n", Q);
+    throw std::invalid_argument(msg);
+  }
+
+  // Require Q be positive semidefinite
+  //
+  // If Q is a symmetric matrix with a decomposition LDLᵀ, the number of
+  // positive, negative, and zero diagonal entries in D equals the number of
+  // positive, negative, and zero eigenvalues respectively in Q (see
+  // https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
+  //
+  // Therefore, D having no negative diagonal entries is sufficient to prove Q
+  // is positive semidefinite.
+  auto Q_ldlt = Q.ldlt();
+  if (Q_ldlt.info() != Eigen::Success ||
+      (Q_ldlt.vectorD().array() < 0.0).any()) {
+    std::string msg =
+        fmt::format("Q isn't positive semidefinite!\n\nQ =\n{}\n", Q);
+    throw std::invalid_argument(msg);
+  }
+
+  // Require (A, B) pair be stabilizable
+  if (!IsStabilizable<States, Inputs>(A, B)) {
+    std::string msg = fmt::format(
+        "The (A, B) pair isn't stabilizable!\n\nA =\n{}\nB =\n{}\n", A, B);
+    throw std::invalid_argument(msg);
+  }
+
+  // Require (A, C) pair be detectable where Q = CᵀC
+  //
+  // Q = CᵀC = PᵀLDLᵀP
+  // Cᵀ = PᵀL√(D)
+  // C = (PᵀL√(D))ᵀ
+  {
+    Eigen::Matrix<double, States, States> C =
+        (Q_ldlt.transpositionsP().transpose() *
+         Eigen::Matrix<double, States, States>{Q_ldlt.matrixL()} *
+         Q_ldlt.vectorD().cwiseSqrt().asDiagonal())
+            .transpose();
+
+    if (!IsDetectable<States, States>(A, C)) {
+      std::string msg = fmt::format(
+          "The (A, C) pair where Q = CᵀC isn't detectable!\n\nA =\n{}\nQ "
+          "=\n{}\n",
+          A, Q);
+      throw std::invalid_argument(msg);
+    }
+  }
+}
+
+/**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic
+ * Riccati equation:
+ *
+ *   AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ *
+ * This internal function skips expensive precondition checks for increased
+ * performance. The solver may hang if any of the following occur:
+ * <ul>
+ *   <li>Q isn't symmetric positive semidefinite</li>
+ *   <li>R isn't symmetric positive definite</li>
+ *   <li>The (A, B) pair isn't stabilizable</li>
+ *   <li>The (A, C) pair where Q = CᵀC isn't detectable</li>
+ * </ul>
+ * Only use this function if you're sure the preconditions are met.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @param A The system matrix.
+ * @param B The input matrix.
+ * @param Q The state cost matrix.
+ * @param R_llt The LLT decomposition of the input cost matrix.
+ */
+template <int States, int Inputs>
+Eigen::Matrix<double, States, States> DARE(
+    const Eigen::Matrix<double, States, States>& A,
+    const Eigen::Matrix<double, States, Inputs>& B,
+    const Eigen::Matrix<double, States, States>& Q,
+    const Eigen::LLT<Eigen::Matrix<double, Inputs, Inputs>>& R_llt) {
+  using StateMatrix = Eigen::Matrix<double, States, States>;
+
+  // Implements the SDA algorithm on page 5 of [1].
+
+  // A₀ = A
+  StateMatrix A_k = A;
+
+  // G₀ = BR⁻¹Bᵀ
+  //
+  // See equation (4) of [1].
+  StateMatrix G_k = B * R_llt.solve(B.transpose());
+
+  // H₀ = Q
+  //
+  // See equation (4) of [1].
+  StateMatrix H_k;
+  StateMatrix H_k1 = Q;
+
+  do {
+    H_k = H_k1;
+
+    // W = I + GₖHₖ
+    StateMatrix W = StateMatrix::Identity(H_k.rows(), H_k.cols()) + G_k * H_k;
+
+    auto W_solver = W.lu();
+
+    // Solve WV₁ = Aₖ for V₁
+    StateMatrix V_1 = W_solver.solve(A_k);
+
+    // Solve V₂Wᵀ = Gₖ for V₂
+    //
+    // We want to put V₂Wᵀ = Gₖ into Ax = b form so we can solve it more
+    // efficiently.
+    //
+    // V₂Wᵀ = Gₖ
+    // (V₂Wᵀ)ᵀ = Gₖᵀ
+    // WV₂ᵀ = Gₖᵀ
+    //
+    // The solution of Ax = b can be found via x = A.solve(b).
+    //
+    // V₂ᵀ = W.solve(Gₖᵀ)
+    // V₂ = W.solve(Gₖᵀ)ᵀ
+    StateMatrix V_2 = W_solver.solve(G_k.transpose()).transpose();
+
+    // Gₖ₊₁ = Gₖ + AₖV₂Aₖᵀ
+    G_k += A_k * V_2 * A_k.transpose();
+
+    // Hₖ₊₁ = Hₖ + V₁ᵀHₖAₖ
+    H_k1 = H_k + V_1.transpose() * H_k * A_k;
+
+    // Aₖ₊₁ = AₖV₁
+    A_k *= V_1;
+
+    // while |Hₖ₊₁ − Hₖ| > ε |Hₖ₊₁|
+  } while ((H_k1 - H_k).norm() > 1e-10 * H_k1.norm());
+
+  return H_k1;
+}
+
+/**
+Computes the unique stabilizing solution X to the discrete-time algebraic
+Riccati equation:
+
+  AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+
+This is equivalent to solving the original DARE:
+
+  A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+
+where A₂ and Q₂ are a change of variables:
+
+  A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+
+This overload of the DARE is useful for finding the control law uₖ that
+minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+
+@verbatim
+    ∞ [xₖ]ᵀ[Q  N][xₖ]
+J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   k=0
+@endverbatim
+
+This is a more general form of the following. The linear-quadratic regulator
+is the feedback control law uₖ that minimizes the following cost function
+subject to xₖ₊₁ = Axₖ + Buₖ:
+
+@verbatim
+    ∞
+J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   k=0
+@endverbatim
+
+This can be refactored as:
+
+@verbatim
+    ∞ [xₖ]ᵀ[Q 0][xₖ]
+J = Σ [uₖ] [0 R][uₖ] ΔT
+   k=0
+@endverbatim
+
+This internal function skips expensive precondition checks for increased
+performance. The solver may hang if any of the following occur:
+<ul>
+  <li>Q₂ isn't symmetric positive semidefinite</li>
+  <li>R isn't symmetric positive definite</li>
+  <li>The (A₂, B) pair isn't stabilizable</li>
+  <li>The (A₂, C) pair where Q₂ = CᵀC isn't detectable</li>
+</ul>
+Only use this function if you're sure the preconditions are met.
+
+@tparam States Number of states.
+@tparam Inputs Number of inputs.
+@param A The system matrix.
+@param B The input matrix.
+@param Q The state cost matrix.
+@param R_llt The LLT decomposition of the input cost matrix.
+@param N The state-input cross cost matrix.
+*/
+template <int States, int Inputs>
+Eigen::Matrix<double, States, States> DARE(
+    const Eigen::Matrix<double, States, States>& A,
+    const Eigen::Matrix<double, States, Inputs>& B,
+    const Eigen::Matrix<double, States, States>& Q,
+    const Eigen::LLT<Eigen::Matrix<double, Inputs, Inputs>>& R_llt,
+    const Eigen::Matrix<double, Inputs, Inputs>& N) {
+  // This is a change of variables to make the DARE that includes Q, R, and N
+  // cost matrices fit the form of the DARE that includes only Q and R cost
+  // matrices.
+  //
+  // This is equivalent to solving the original DARE:
+  //
+  //   A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+  //
+  // where A₂ and Q₂ are a change of variables:
+  //
+  //   A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+  return detail::DARE<States, Inputs>(A - B * R_llt.solve(N.transpose()), B,
+                                      Q - N * R_llt.solve(N.transpose()),
+                                      R_llt);
+}
+
+}  // namespace detail
+
+/**
+ * Computes the unique stabilizing solution X to the discrete-time algebraic
+ * Riccati equation:
+ *
+ *   AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @param A The system matrix.
+ * @param B The input matrix.
+ * @param Q The state cost matrix.
+ * @param R The input cost matrix.
+ * @throws std::invalid_argument if Q isn't symmetric positive semidefinite.
+ * @throws std::invalid_argument if R isn't symmetric positive definite.
+ * @throws std::invalid_argument if the (A, B) pair isn't stabilizable.
+ * @throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't
+ *   detectable.
+ */
+template <int States, int Inputs>
+Eigen::Matrix<double, States, States> DARE(
+    const Eigen::Matrix<double, States, States>& A,
+    const Eigen::Matrix<double, States, Inputs>& B,
+    const Eigen::Matrix<double, States, States>& Q,
+    const Eigen::Matrix<double, Inputs, Inputs>& R) {
+  // Require R be symmetric
+  if ((R - R.transpose()).norm() > 1e-10) {
+    std::string msg = fmt::format("R isn't symmetric!\n\nR =\n{}\n", R);
+    throw std::invalid_argument(msg);
+  }
+
+  // Require R be positive definite
+  auto R_llt = R.llt();
+  if (R_llt.info() != Eigen::Success) {
+    std::string msg = fmt::format("R isn't positive definite!\n\nR =\n{}\n", R);
+    throw std::invalid_argument(msg);
+  }
+
+  detail::CheckDARE_ABQ<States, Inputs>(A, B, Q);
+
+  return detail::DARE<States, Inputs>(A, B, Q, R_llt);
+}
+
+/**
+Computes the unique stabilizing solution X to the discrete-time algebraic
+Riccati equation:
+
+  AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+
+This is equivalent to solving the original DARE:
+
+  A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+
+where A₂ and Q₂ are a change of variables:
+
+  A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+
+This overload of the DARE is useful for finding the control law uₖ that
+minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+
+@verbatim
+    ∞ [xₖ]ᵀ[Q  N][xₖ]
+J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   k=0
+@endverbatim
+
+This is a more general form of the following. The linear-quadratic regulator
+is the feedback control law uₖ that minimizes the following cost function
+subject to xₖ₊₁ = Axₖ + Buₖ:
+
+@verbatim
+    ∞
+J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   k=0
+@endverbatim
+
+This can be refactored as:
+
+@verbatim
+    ∞ [xₖ]ᵀ[Q 0][xₖ]
+J = Σ [uₖ] [0 R][uₖ] ΔT
+   k=0
+@endverbatim
+
+@tparam States Number of states.
+@tparam Inputs Number of inputs.
+@param A The system matrix.
+@param B The input matrix.
+@param Q The state cost matrix.
+@param R The input cost matrix.
+@param N The state-input cross cost matrix.
+@throws std::invalid_argument if Q₂ isn't symmetric positive semidefinite.
+@throws std::invalid_argument if R isn't symmetric positive definite.
+@throws std::invalid_argument if the (A₂, B) pair isn't stabilizable.
+@throws std::invalid_argument if the (A₂, C) pair where Q₂ = CᵀC isn't
+  detectable.
+*/
+template <int States, int Inputs>
+Eigen::Matrix<double, States, States> DARE(
+    const Eigen::Matrix<double, States, States>& A,
+    const Eigen::Matrix<double, States, Inputs>& B,
+    const Eigen::Matrix<double, States, States>& Q,
+    const Eigen::Matrix<double, Inputs, Inputs>& R,
+    const Eigen::Matrix<double, States, Inputs>& N) {
+  // Require R be symmetric
+  if ((R - R.transpose()).norm() > 1e-10) {
+    std::string msg = fmt::format("R isn't symmetric!\n\nR =\n{}\n", R);
+    throw std::invalid_argument(msg);
+  }
+
+  // Require R be positive definite
+  auto R_llt = R.llt();
+  if (R_llt.info() != Eigen::Success) {
+    std::string msg = fmt::format("R isn't positive definite!\n\nR =\n{}\n", R);
+    throw std::invalid_argument(msg);
+  }
+
+  // This is a change of variables to make the DARE that includes Q, R, and N
+  // cost matrices fit the form of the DARE that includes only Q and R cost
+  // matrices.
+  //
+  // This is equivalent to solving the original DARE:
+  //
+  //   A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+  //
+  // where A₂ and Q₂ are a change of variables:
+  //
+  //   A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+  Eigen::Matrix<double, States, States> A_2 =
+      A - B * R_llt.solve(N.transpose());
+  Eigen::Matrix<double, States, States> Q_2 =
+      Q - N * R_llt.solve(N.transpose());
+
+  detail::CheckDARE_ABQ<States, Inputs>(A_2, B, Q_2);
+
+  return detail::DARE<States, Inputs>(A_2, B, Q_2, R_llt);
+}
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/EigenCore.h b/wpimath/src/main/native/include/frc/EigenCore.h
index b33e9e2..1604a0d 100644
--- a/wpimath/src/main/native/include/frc/EigenCore.h
+++ b/wpimath/src/main/native/include/frc/EigenCore.h
@@ -4,7 +4,7 @@
 
 #pragma once
 
-#include "Eigen/Core"
+#include <Eigen/Core>
 
 namespace frc {
 
diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h
index 24bf857..26f106e 100644
--- a/wpimath/src/main/native/include/frc/MathUtil.h
+++ b/wpimath/src/main/native/include/frc/MathUtil.h
@@ -5,6 +5,7 @@
 #pragma once
 
 #include <numbers>
+#include <type_traits>
 
 #include <wpi/SymbolExports.h>
 
@@ -25,12 +26,11 @@
  * be infinite.
  * @return The value after the deadband is applied.
  */
-template <typename T,
-          typename = std::enable_if_t<std::disjunction_v<
-              std::is_floating_point<T>, units::traits::is_unit_t<T>>>>
+template <typename T>
+  requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
 T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
   T magnitude;
-  if constexpr (std::is_floating_point_v<T>) {
+  if constexpr (std::is_arithmetic_v<T>) {
     magnitude = std::abs(value);
   } else {
     magnitude = units::math::abs(value);
@@ -106,6 +106,58 @@
 }
 
 /**
+ * Checks if the given value matches an expected value within a certain
+ * tolerance.
+ *
+ * @param expected The expected value
+ * @param actual The actual value
+ * @param tolerance The allowed difference between the actual and the expected
+ * value
+ * @return Whether or not the actual value is within the allowed tolerance
+ */
+template <typename T>
+  requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
+constexpr bool IsNear(T expected, T actual, T tolerance) {
+  if constexpr (std::is_arithmetic_v<T>) {
+    return std::abs(expected - actual) < tolerance;
+  } else {
+    return units::math::abs(expected - actual) < tolerance;
+  }
+}
+
+/**
+ * Checks if the given value matches an expected value within a certain
+ * tolerance. Supports continuous input for cases like absolute encoders.
+ *
+ * Continuous input means that the min and max value are considered to be the
+ * same point, and tolerances can be checked across them. A common example
+ * would be for absolute encoders: calling isNear(2, 359, 5, 0, 360) returns
+ * true because 359 is 1 away from 360 (which is treated as the same as 0) and
+ * 2 is 2 away from 0, adding up to an error of 3 degrees, which is within the
+ * given tolerance of 5.
+ *
+ * @param expected The expected value
+ * @param actual The actual value
+ * @param tolerance The allowed difference between the actual and the expected
+ * value
+ * @param min Smallest value before wrapping around to the largest value
+ * @param max Largest value before wrapping around to the smallest value
+ * @return Whether or not the actual value is within the allowed tolerance
+ */
+template <typename T>
+  requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
+constexpr bool IsNear(T expected, T actual, T tolerance, T min, T max) {
+  T errorBound = (max - min) / 2.0;
+  T error = frc::InputModulus<T>(expected - actual, -errorBound, errorBound);
+
+  if constexpr (std::is_arithmetic_v<T>) {
+    return std::abs(error) < tolerance;
+  } else {
+    return units::math::abs(error) < tolerance;
+  }
+}
+
+/**
  * Wraps an angle to the range -pi to pi radians (-180 to 180 degrees).
  *
  * @param angle Angle to wrap.
diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
index 0345b46..3aa2e75 100644
--- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h
+++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
@@ -6,31 +6,21 @@
 
 #include <array>
 #include <cmath>
+#include <concepts>
 #include <limits>
 #include <random>
-#include <type_traits>
 
+#include <Eigen/Eigenvalues>
+#include <Eigen/QR>
 #include <wpi/SymbolExports.h>
 #include <wpi/deprecated.h>
 
-#include "Eigen/Eigenvalues"
-#include "Eigen/QR"
 #include "frc/EigenCore.h"
 #include "frc/geometry/Pose2d.h"
 
 namespace frc {
 namespace detail {
 
-template <int Rows, int Cols, typename Matrix, typename T, typename... Ts>
-void MatrixImpl(Matrix& result, T elem, Ts... elems) {
-  constexpr int count = Rows * Cols - (sizeof...(Ts) + 1);
-
-  result(count / Cols, count % Cols) = elem;
-  if constexpr (sizeof...(Ts) > 0) {
-    MatrixImpl<Rows, Cols>(result, elems...);
-  }
-}
-
 template <typename Matrix, typename T, typename... Ts>
 void CostMatrixImpl(Matrix& result, T elem, Ts... elems) {
   if (elem == std::numeric_limits<double>::infinity()) {
@@ -66,26 +56,37 @@
 template <int States, int Inputs>
 bool IsStabilizableImpl(const Matrixd<States, States>& A,
                         const Matrixd<States, Inputs>& B) {
-  Eigen::EigenSolver<Matrixd<States, States>> es{A};
+  Eigen::EigenSolver<Matrixd<States, States>> es{A, false};
 
-  for (int i = 0; i < States; ++i) {
-    if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
-            es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
-        1) {
+  for (int i = 0; i < A.rows(); ++i) {
+    if (std::norm(es.eigenvalues()[i]) < 1) {
       continue;
     }
 
-    Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
-    E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
-                                             States>::Identity() -
-             A,
-        B;
+    if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) {
+      Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
+      E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
+                                               States>::Identity() -
+               A,
+          B;
 
-    Eigen::ColPivHouseholderQR<
-        Eigen::Matrix<std::complex<double>, States, States + Inputs>>
-        qr{E};
-    if (qr.rank() < States) {
-      return false;
+      Eigen::ColPivHouseholderQR<
+          Eigen::Matrix<std::complex<double>, States, States + Inputs>>
+          qr{E};
+      if (qr.rank() < States) {
+        return false;
+      }
+    } else {
+      Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
+      E << es.eigenvalues()[i] *
+                   Eigen::MatrixXcd::Identity(A.rows(), A.rows()) -
+               A,
+          B;
+
+      Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
+      if (qr.rank() < A.rows()) {
+        return false;
+      }
     }
   }
   return true;
@@ -106,8 +107,7 @@
  *                   of the control inputs from no actuation.
  * @return State excursion or control effort cost matrix.
  */
-template <typename... Ts, typename = std::enable_if_t<
-                              std::conjunction_v<std::is_same<double, Ts>...>>>
+template <std::same_as<double>... Ts>
 Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances) {
   Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
   detail::CostMatrixImpl(result.diagonal(), tolerances...);
@@ -126,8 +126,7 @@
  *                output measurement.
  * @return Process noise or measurement noise covariance matrix.
  */
-template <typename... Ts, typename = std::enable_if_t<
-                              std::conjunction_v<std::is_same<double, Ts>...>>>
+template <std::same_as<double>... Ts>
 Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
   Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
   detail::CovMatrixImpl(result.diagonal(), stdDevs...);
@@ -138,7 +137,8 @@
  * Creates a cost matrix from the given vector for use with LQR.
  *
  * The cost matrix is constructed using Bryson's rule. The inverse square of
- * each element in the input is taken and placed on the cost matrix diagonal.
+ * each element in the input is placed on the cost matrix diagonal. If a
+ * tolerance is infinity, its cost matrix entry is set to zero.
  *
  * @param costs An array. For a Q matrix, its elements are the maximum allowed
  *              excursions of the states from the reference. For an R matrix,
@@ -151,7 +151,11 @@
   Eigen::DiagonalMatrix<double, N> result;
   auto& diag = result.diagonal();
   for (size_t i = 0; i < N; ++i) {
-    diag(i) = 1.0 / std::pow(costs[i], 2);
+    if (costs[i] == std::numeric_limits<double>::infinity()) {
+      diag(i) = 0.0;
+    } else {
+      diag(i) = 1.0 / std::pow(costs[i], 2);
+    }
   }
   return result;
 }
@@ -178,8 +182,7 @@
   return result;
 }
 
-template <typename... Ts, typename = std::enable_if_t<
-                              std::conjunction_v<std::is_same<double, Ts>...>>>
+template <std::same_as<double>... Ts>
 Matrixd<sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
   Matrixd<sizeof...(Ts), 1> result;
   detail::WhiteNoiseVectorImpl(result, stdDevs...);
@@ -221,7 +224,7 @@
  * @return The vector.
  */
 WPILIB_DLLEXPORT
-Vectord<3> PoseTo3dVector(const Pose2d& pose);
+Eigen::Vector3d PoseTo3dVector(const Pose2d& pose);
 
 /**
  * Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
@@ -231,14 +234,14 @@
  * @return The vector.
  */
 WPILIB_DLLEXPORT
-Vectord<4> PoseTo4dVector(const Pose2d& pose);
+Eigen::Vector4d PoseTo4dVector(const Pose2d& pose);
 
 /**
  * Returns true if (A, B) is a stabilizable pair.
  *
  * (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
  * any, have absolute values less than one, where an eigenvalue is
- * uncontrollable if rank(λI - A, B) < n where n is the number of states.
+ * uncontrollable if rank([λI - A, B]) < n where n is the number of states.
  *
  * @tparam States The number of states.
  * @tparam Inputs The number of inputs.
@@ -256,7 +259,7 @@
  *
  * (A, C) is detectable if and only if the unobservable eigenvalues of A, if
  * any, have absolute values less than one, where an eigenvalue is unobservable
- * if rank(λI - A; C) < n where n is the number of states.
+ * if rank([λI - A; C]) < n where n is the number of states.
  *
  * @tparam States The number of states.
  * @tparam Outputs The number of outputs.
@@ -282,6 +285,12 @@
 WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
                                            const Matrixd<2, 1>& B);
 
+// Template specializations are used here to make common state-input pairs
+// compile faster.
+template <>
+WPILIB_DLLEXPORT bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(
+    const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
+
 /**
  * Converts a Pose2d into a vector of [x, y, theta].
  *
@@ -290,7 +299,7 @@
  * @return The vector.
  */
 WPILIB_DLLEXPORT
-Vectord<3> PoseToVector(const Pose2d& pose);
+Eigen::Vector3d PoseToVector(const Pose2d& pose);
 
 /**
  * Clamps input vector between system's minimum and maximum allowable input.
diff --git a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
index 21aad25..6306457 100644
--- a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
@@ -7,7 +7,8 @@
 #include <array>
 #include <functional>
 
-#include "Eigen/QR"
+#include <Eigen/QR>
+
 #include "frc/EigenCore.h"
 #include "frc/system/NumericalJacobian.h"
 #include "units/time.h"
@@ -165,6 +166,9 @@
   InputVector Calculate(const StateVector& r, const StateVector& nextR) {
     StateVector rDot = (nextR - r) / m_dt.value();
 
+    // ṙ = f(r) + Bu
+    // Bu = ṙ − f(r)
+    // u = B⁺(ṙ − f(r))
     m_uff = m_B.householderQr().solve(rDot - m_f(r, InputVector::Zero()));
 
     m_r = nextR;
diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h
index 3a5148a..d84e3ce 100644
--- a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h
+++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h
@@ -4,9 +4,9 @@
 
 #pragma once
 
+#include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 
-#include "Eigen/Core"
 #include "frc/controller/DifferentialDriveWheelVoltages.h"
 #include "frc/system/LinearSystem.h"
 #include "units/acceleration.h"
diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
index bbe7720..62a7bad 100644
--- a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
@@ -6,6 +6,9 @@
 
 #include <wpi/MathExtras.h>
 
+#include "frc/EigenCore.h"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/system/plant/LinearSystemId.h"
 #include "units/length.h"
 #include "units/time.h"
 #include "units/voltage.h"
@@ -54,6 +57,50 @@
     return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
   }
 
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param currentVelocity The current velocity setpoint, in distance per
+   *                        second.
+   * @param nextVelocity    The next velocity setpoint, in distance per second.
+   * @param dt              Time between velocity setpoints in seconds.
+   * @return The computed feedforward, in volts.
+   */
+  units::volt_t Calculate(units::unit_t<Velocity> currentVelocity,
+                          units::unit_t<Velocity> nextVelocity,
+                          units::second_t dt) const {
+    // Discretize the affine model.
+    //
+    //   dx/dt = Ax + Bu + c
+    //   dx/dt = Ax + B(u + B⁺c)
+    //   xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
+    //   xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
+    //   xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ
+    //
+    // Solve for uₖ.
+    //
+    //   B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
+    //
+    // For an elevator with the model
+    // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x),
+    // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B
+    // assuming sgn(x) is a constant for the duration of the step.
+    //
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x)))
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x))
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x)
+    auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
+    LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
+
+    Vectord<1> r{currentVelocity.value()};
+    Vectord<1> nextR{nextVelocity.value()};
+
+    return kG + kS * wpi::sgn(currentVelocity.value()) +
+           units::volt_t{feedforward.Calculate(r, nextR)(0)};
+  }
+
   // Rearranging the main equation from the calculate() method yields the
   // formulas for the methods below:
 
diff --git a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
index 9a749c6..6f9568e 100644
--- a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
+++ b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
@@ -42,7 +42,7 @@
    * angle.
    */
   HolonomicDriveController(
-      frc2::PIDController xController, frc2::PIDController yController,
+      PIDController xController, PIDController yController,
       ProfiledPIDController<units::radian> thetaController);
 
   HolonomicDriveController(const HolonomicDriveController&) = default;
@@ -103,14 +103,29 @@
    */
   void SetEnabled(bool enabled);
 
+  /**
+   * Returns the rotation ProfiledPIDController
+   */
+  ProfiledPIDController<units::radian>& getThetaController();
+
+  /**
+   * Returns the X PIDController
+   */
+  PIDController& getXController();
+
+  /**
+   * Returns the Y PIDController
+   */
+  PIDController& getYController();
+
  private:
   Pose2d m_poseError;
   Rotation2d m_rotationError;
   Pose2d m_poseTolerance;
   bool m_enabled = true;
 
-  frc2::PIDController m_xController;
-  frc2::PIDController m_yController;
+  PIDController m_xController;
+  PIDController m_yController;
   ProfiledPIDController<units::radian> m_thetaController;
 
   bool m_firstRun = true;
diff --git a/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
index eef1fc0..3a1230d 100644
--- a/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
+++ b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
@@ -6,7 +6,8 @@
 
 #include <frc/system/LinearSystem.h>
 
-#include "Eigen/QR"
+#include <Eigen/QR>
+
 #include "frc/EigenCore.h"
 #include "units/time.h"
 
diff --git a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h
index 0a336aa..94f3fa3 100644
--- a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h
+++ b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h
@@ -23,9 +23,17 @@
 /**
  * The linear time-varying differential drive controller has a similar form to
  * the LQR, but the model used to compute the controller gain is the nonlinear
- * model linearized around the drivetrain's current state. We precomputed gains
- * for important places in our state-space, then interpolated between them with
- * a LUT to save computational resources.
+ * differential drive model linearized around the drivetrain's current state. We
+ * precompute gains for important places in our state-space, then interpolate
+ * between them with a lookup table to save computational resources.
+ *
+ * This controller has a flat hierarchy with pose and wheel velocity references
+ * and voltage outputs. This is different from a Ramsete controller's nested
+ * hierarchy where the top-level controller has a pose reference and chassis
+ * velocity command outputs, and the low-level controller has wheel velocity
+ * references and voltage outputs. Flat hierarchies are easier to tune in one
+ * shot. Furthermore, this controller is more optimal in the "least-squares
+ * error" sense than a controller based on Ramsete.
  *
  * See section 8.7 in Controls Engineering in FRC for a derivation of the
  * control law we used shown in theorem 8.7.4.
@@ -35,12 +43,18 @@
   /**
    * Constructs a linear time-varying differential drive controller.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param plant      The differential drive velocity plant.
    * @param trackwidth The distance between the differential drive's left and
    *                   right wheels.
    * @param Qelems     The maximum desired error tolerance for each state.
    * @param Relems     The maximum desired control effort for each input.
    * @param dt         Discretization timestep.
+   * @throws std::domain_error if max velocity of plant with 12 V input <= 0 m/s
+   *     or >= 15 m/s.
    */
   LTVDifferentialDriveController(const frc::LinearSystem<2, 2, 2>& plant,
                                  units::meter_t trackwidth,
diff --git a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h
index 83cfe4b..38c4287 100644
--- a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h
+++ b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h
@@ -20,8 +20,11 @@
 
 /**
  * The linear time-varying unicycle controller has a similar form to the LQR,
- * but the model used to compute the controller gain is the nonlinear model
- * linearized around the drivetrain's current state.
+ * but the model used to compute the controller gain is the nonlinear unicycle
+ * model linearized around the drivetrain's current state.
+ *
+ * This controller is a roughly drop-in replacement for RamseteController with
+ * more optimal feedback gains in the "least-squares error" sense.
  *
  * See section 8.9 in Controls Engineering in FRC for a derivation of the
  * control law we used shown in theorem 8.9.1.
@@ -36,6 +39,7 @@
    * @param dt Discretization timestep.
    * @param maxVelocity The maximum velocity for the controller gain lookup
    *                    table.
+   * @throws std::domain_error if maxVelocity &lt;= 0.
    */
   explicit LTVUnicycleController(
       units::second_t dt, units::meters_per_second_t maxVelocity = 9_mps);
@@ -43,11 +47,16 @@
   /**
    * Constructs a linear time-varying unicycle controller.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param Qelems The maximum desired error tolerance for each state.
    * @param Relems The maximum desired control effort for each input.
    * @param dt     Discretization timestep.
    * @param maxVelocity The maximum velocity for the controller gain lookup
    *                    table.
+   * @throws std::domain_error if maxVelocity <= 0 m/s or >= 15 m/s.
    */
   LTVUnicycleController(const wpi::array<double, 3>& Qelems,
                         const wpi::array<double, 2>& Relems, units::second_t dt,
diff --git a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
index d4cc3c4..1d905e2 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
@@ -7,7 +7,8 @@
 #include <array>
 #include <functional>
 
-#include "Eigen/QR"
+#include <Eigen/QR>
+
 #include "frc/EigenCore.h"
 #include "frc/system/Discretization.h"
 #include "frc/system/LinearSystem.h"
@@ -137,6 +138,9 @@
    * @return The calculated feedforward.
    */
   InputVector Calculate(const StateVector& r, const StateVector& nextR) {
+    // rₖ₊₁ = Arₖ + Buₖ
+    // Buₖ = rₖ₊₁ − Arₖ
+    // uₖ = B⁺(rₖ₊₁ − Arₖ)
     m_uff = m_B.householderQr().solve(nextR - (m_A * r));
     m_r = nextR;
     return m_uff;
diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
index 50d6566..979e98a 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
+++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
@@ -36,6 +36,10 @@
   /**
    * Constructs a controller with the given coefficients and plant.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param plant  The plant being controlled.
    * @param Qelems The maximum desired error tolerance for each state.
    * @param Relems The maximum desired control effort for each input.
@@ -50,6 +54,10 @@
   /**
    * Constructs a controller with the given coefficients and plant.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
+   * for how to select the tolerances.
+   *
    * @param A      Continuous system matrix of the plant being controlled.
    * @param B      Continuous input matrix of the plant being controlled.
    * @param Qelems The maximum desired error tolerance for each state.
diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
index 87d37ec..1871244 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
+++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
@@ -7,14 +7,14 @@
 #include <stdexcept>
 #include <string>
 
-#include "Eigen/Cholesky"
-#include "Eigen/Eigenvalues"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include <Eigen/Cholesky>
+#include <unsupported/Eigen/MatrixFunctions>
+
+#include "frc/DARE.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/controller/LinearQuadraticRegulator.h"
 #include "frc/fmt/Eigen.h"
 #include "frc/system/Discretization.h"
-#include "unsupported/Eigen/MatrixFunctions"
 #include "wpimath/MathShared.h"
 
 namespace frc {
@@ -52,8 +52,7 @@
     throw std::invalid_argument(msg);
   }
 
-  Matrixd<States, States> S =
-      drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
+  Matrixd<States, States> S = DARE<States, Inputs>(discA, discB, Q, R);
 
   // K = (BᵀSB + R)⁻¹BᵀSA
   m_K = (discB.transpose() * S * discB + R)
@@ -72,8 +71,7 @@
   Matrixd<States, Inputs> discB;
   DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
 
-  Matrixd<States, States> S =
-      drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
+  Matrixd<States, States> S = DARE<States, Inputs>(discA, discB, Q, R, N);
 
   // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
   m_K = (discB.transpose() * S * discB + R)
diff --git a/wpimath/src/main/native/include/frc/controller/PIDController.h b/wpimath/src/main/native/include/frc/controller/PIDController.h
index d6a41d1..0d5b0a3 100644
--- a/wpimath/src/main/native/include/frc/controller/PIDController.h
+++ b/wpimath/src/main/native/include/frc/controller/PIDController.h
@@ -13,7 +13,7 @@
 
 #include "units/time.h"
 
-namespace frc2 {
+namespace frc {
 
 /**
  * Implements a PID control loop.
@@ -74,6 +74,18 @@
   void SetD(double Kd);
 
   /**
+   * Sets the IZone range. When the absolute value of the position error is
+   * greater than IZone, the total accumulated error will reset to zero,
+   * disabling integral gain until the absolute value of the position error is
+   * less than IZone. This is used to prevent integral windup. Must be
+   * non-negative. Passing a value of zero will effectively disable integral
+   * gain. Passing a value of infinity disables IZone functionality.
+   *
+   * @param iZone Maximum magnitude of error to allow integral control.
+   */
+  void SetIZone(double iZone);
+
+  /**
    * Gets the proportional coefficient.
    *
    * @return proportional coefficient
@@ -95,6 +107,13 @@
   double GetD() const;
 
   /**
+   * Get the IZone range.
+   *
+   * @return Maximum magnitude of error to allow integral control.
+   */
+  double GetIZone() const;
+
+  /**
    * Gets the period of this controller.
    *
    * @return The period of the controller.
@@ -221,6 +240,9 @@
   // Factor for "derivative" control
   double m_Kd;
 
+  // The error range where "integral" control applies
+  double m_iZone = std::numeric_limits<double>::infinity();
+
   // The period (in seconds) of the control loop running this controller
   units::second_t m_period;
 
@@ -252,12 +274,9 @@
 
   double m_setpoint = 0;
   double m_measurement = 0;
+
+  bool m_haveSetpoint = false;
+  bool m_haveMeasurement = false;
 };
 
-}  // namespace frc2
-
-namespace frc {
-
-using frc2::PIDController;
-
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
index 8491118..8f211c6 100644
--- a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -58,7 +58,9 @@
    */
   ProfiledPIDController(double Kp, double Ki, double Kd,
                         Constraints constraints, units::second_t period = 20_ms)
-      : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
+      : m_controller{Kp, Ki, Kd, period},
+        m_constraints{constraints},
+        m_profile{m_constraints} {
     int instances = detail::IncrementAndGetProfiledPIDControllerInstances();
     wpi::math::MathSharedStore::ReportUsage(
         wpi::math::MathUsageId::kController_ProfiledPIDController, instances);
@@ -107,6 +109,18 @@
   void SetD(double Kd) { m_controller.SetD(Kd); }
 
   /**
+   * Sets the IZone range. When the absolute value of the position error is
+   * greater than IZone, the total accumulated error will reset to zero,
+   * disabling integral gain until the absolute value of the position error is
+   * less than IZone. This is used to prevent integral windup. Must be
+   * non-negative. Passing a value of zero will effectively disable integral
+   * gain. Passing a value of infinity disables IZone functionality.
+   *
+   * @param iZone Maximum magnitude of error to allow integral control.
+   */
+  void SetIZone(double iZone) { m_controller.SetIZone(iZone); }
+
+  /**
    * Gets the proportional coefficient.
    *
    * @return proportional coefficient
@@ -128,6 +142,13 @@
   double GetD() const { return m_controller.GetD(); }
 
   /**
+   * Get the IZone range.
+   *
+   * @return Maximum magnitude of error to allow integral control.
+   */
+  double GetIZone() const { return m_controller.GetIZone(); }
+
+  /**
    * Gets the period of this controller.
    *
    * @return The period of the controller.
@@ -183,7 +204,16 @@
    *
    * @param constraints Velocity and acceleration constraints for goal.
    */
-  void SetConstraints(Constraints constraints) { m_constraints = constraints; }
+  void SetConstraints(Constraints constraints) {
+    m_constraints = constraints;
+    m_profile = TrapezoidProfile<Distance>{m_constraints};
+  }
+
+  /**
+   * Get the velocity and acceleration constraints for this controller.
+   * @return Velocity and acceleration constraints.
+   */
+  Constraints GetConstraints() { return m_constraints; }
 
   /**
    * Returns the current setpoint of the ProfiledPIDController.
@@ -292,8 +322,7 @@
       m_setpoint.position = setpointMinDistance + measurement;
     }
 
-    frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
-    m_setpoint = profile.Calculate(GetPeriod());
+    m_setpoint = m_profile.Calculate(GetPeriod(), m_goal, m_setpoint);
     return m_controller.Calculate(measurement.value(),
                                   m_setpoint.position.value());
   }
@@ -372,17 +401,22 @@
     builder.AddDoubleProperty(
         "d", [this] { return GetD(); }, [this](double value) { SetD(value); });
     builder.AddDoubleProperty(
+        "izone", [this] { return GetIZone(); },
+        [this](double value) { SetIZone(value); });
+    builder.AddDoubleProperty(
         "goal", [this] { return GetGoal().position.value(); },
         [this](double value) { SetGoal(Distance_t{value}); });
   }
 
  private:
-  frc2::PIDController m_controller;
+  PIDController m_controller;
   Distance_t m_minimumInput{0};
   Distance_t m_maximumInput{0};
+
+  typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
+  TrapezoidProfile<Distance> m_profile;
   typename frc::TrapezoidProfile<Distance>::State m_goal;
   typename frc::TrapezoidProfile<Distance>::State m_setpoint;
-  typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
 };
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
index 0512c68c..026cc67 100644
--- a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
+++ b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <functional>
 #include <numbers>
 
 #include "frc/EigenCore.h"
diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
index 339ccc9..89dcd35 100644
--- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
@@ -7,13 +7,11 @@
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "frc/EigenCore.h"
+#include "frc/estimator/PoseEstimator.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
-#include "frc/interpolation/TimeInterpolatableBuffer.h"
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/kinematics/DifferentialDriveOdometry.h"
-#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
 #include "units/time.h"
 
 namespace frc {
@@ -32,7 +30,9 @@
  * AddVisionMeasurement() can be called as infrequently as you want; if you
  * never call it, then this class will behave like regular encoder odometry.
  */
-class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
+class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
+    : public PoseEstimator<DifferentialDriveWheelSpeeds,
+                           DifferentialDriveWheelPositions> {
  public:
   /**
    * Constructs a DifferentialDrivePoseEstimator with default standard
@@ -80,19 +80,6 @@
       const wpi::array<double, 3>& visionMeasurementStdDevs);
 
   /**
-   * Sets the pose estimator's trust in vision measurements. This might be used
-   * to change trust in vision measurements after the autonomous period, or to
-   * change trust as distance to a vision target increases.
-   *
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose
-   *     measurement (x position in meters, y position in meters, and heading in
-   *     radians). Increase these numbers to trust the vision pose measurement
-   *     less.
-   */
-  void SetVisionMeasurementStdDevs(
-      const wpi::array<double, 3>& visionMeasurementStdDevs);
-
-  /**
    * Resets the robot's position on the field.
    *
    * @param gyroAngle The current gyro angle.
@@ -101,71 +88,10 @@
    * @param pose The estimated pose of the robot on the field.
    */
   void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
-                     units::meter_t rightDistance, const Pose2d& pose);
-
-  /**
-   * Gets the estimated robot pose.
-   *
-   * @return The estimated robot pose.
-   */
-  Pose2d GetEstimatedPosition() const;
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct
-   * the odometry pose estimate while still accounting for measurement noise.
-   *
-   * This method can be called as infrequently as you want, as long as you are
-   * calling Update() every loop.
-   *
-   * To promote stability of the pose estimate and make it robust to bad vision
-   * data, we recommend only adding vision measurements that are already within
-   * one meter or so of the current pose estimate.
-   *
-   * @param visionRobotPose The pose of the robot as measured by the vision
-   *     camera.
-   * @param timestamp The timestamp of the vision measurement in seconds. Note
-   *     that if you don't use your own time source by calling UpdateWithTime(),
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
-   *     the epoch of this timestamp is the same epoch as
-   *     frc::Timer::GetFPGATimestamp(). This means that you should use
-   *     frc::Timer::GetFPGATimestamp() as your time source in this case.
-   */
-  void AddVisionMeasurement(const Pose2d& visionRobotPose,
-                            units::second_t timestamp);
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct
-   * the odometry pose estimate while still accounting for measurement noise.
-   *
-   * This method can be called as infrequently as you want, as long as you are
-   * calling Update() every loop.
-   *
-   * To promote stability of the pose estimate and make it robust to bad vision
-   * data, we recommend only adding vision measurements that are already within
-   * one meter or so of the current pose estimate.
-   *
-   * Note that the vision measurement standard deviations passed into this
-   * method will continue to apply to future measurements until a subsequent
-   * call to SetVisionMeasurementStdDevs() or this method.
-   *
-   * @param visionRobotPose The pose of the robot as measured by the vision
-   *     camera.
-   * @param timestamp The timestamp of the vision measurement in seconds. Note
-   *     that if you don't use your own time source by calling UpdateWithTime(),
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
-   *     the epoch of this timestamp is the same epoch as
-   *     frc::Timer::GetFPGATimestamp(). This means that you should use
-   *     frc::Timer::GetFPGATimestamp() as your time source in this case.
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose
-   *     measurement (x position in meters, y position in meters, and heading in
-   *     radians). Increase these numbers to trust the vision pose measurement
-   *     less.
-   */
-  void AddVisionMeasurement(
-      const Pose2d& visionRobotPose, units::second_t timestamp,
-      const wpi::array<double, 3>& visionMeasurementStdDevs) {
-    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-    AddVisionMeasurement(visionRobotPose, timestamp);
+                     units::meter_t rightDistance, const Pose2d& pose) {
+    PoseEstimator<DifferentialDriveWheelSpeeds,
+                  DifferentialDriveWheelPositions>::
+        ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose);
   }
 
   /**
@@ -179,7 +105,12 @@
    * @return The estimated pose of the robot.
    */
   Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
-                units::meter_t rightDistance);
+                units::meter_t rightDistance) {
+    return PoseEstimator<
+        DifferentialDriveWheelSpeeds,
+        DifferentialDriveWheelPositions>::Update(gyroAngle,
+                                                 {leftDistance, rightDistance});
+  }
 
   /**
    * Updates the pose estimator with wheel encoder and gyro information. This
@@ -195,61 +126,16 @@
   Pose2d UpdateWithTime(units::second_t currentTime,
                         const Rotation2d& gyroAngle,
                         units::meter_t leftDistance,
-                        units::meter_t rightDistance);
+                        units::meter_t rightDistance) {
+    return PoseEstimator<
+        DifferentialDriveWheelSpeeds,
+        DifferentialDriveWheelPositions>::UpdateWithTime(currentTime, gyroAngle,
+                                                         {leftDistance,
+                                                          rightDistance});
+  }
 
  private:
-  struct InterpolationRecord {
-    // The pose observed given the current sensor inputs and the previous pose.
-    Pose2d pose;
-
-    // The current gyro angle.
-    Rotation2d gyroAngle;
-
-    // The distance traveled by the left encoder.
-    units::meter_t leftDistance;
-
-    // The distance traveled by the right encoder.
-    units::meter_t rightDistance;
-
-    /**
-     * Checks equality between this InterpolationRecord and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are equal.
-     */
-    bool operator==(const InterpolationRecord& other) const = default;
-
-    /**
-     * Checks inequality between this InterpolationRecord and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are not equal.
-     */
-    bool operator!=(const InterpolationRecord& other) const = default;
-
-    /**
-     * Interpolates between two InterpolationRecords.
-     *
-     * @param endValue The end value for the interpolation.
-     * @param i The interpolant (fraction).
-     *
-     * @return The interpolated state.
-     */
-    InterpolationRecord Interpolate(DifferentialDriveKinematics& kinematics,
-                                    InterpolationRecord endValue,
-                                    double i) const;
-  };
-
-  DifferentialDriveKinematics& m_kinematics;
-  DifferentialDriveOdometry m_odometry;
-  wpi::array<double, 3> m_q{wpi::empty_array};
-  Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
-
-  TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
-      1.5_s, [this](const InterpolationRecord& start,
-                    const InterpolationRecord& end, double t) {
-        return start.Interpolate(this->m_kinematics, end, t);
-      }};
+  DifferentialDriveOdometry m_odometryImpl;
 };
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
index b09e8d9..32ed558 100644
--- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
@@ -52,6 +52,10 @@
   /**
    * Constructs an extended Kalman filter.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param f                  A vector-valued function of x and u that returns
    *                           the derivative of the state vector.
    * @param h                  A vector-valued function of x and u that returns
@@ -69,6 +73,10 @@
   /**
    * Constructs an extended Kalman filter.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param f                  A vector-valued function of x and u that returns
    *                           the derivative of the state vector.
    * @param h                  A vector-valued function of x and u that returns
@@ -163,6 +171,33 @@
     Correct<Outputs>(u, y, m_h, m_contR, m_residualFuncY, m_addFuncX);
   }
 
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * This is useful for when the measurement noise covariances vary.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  void Correct(const InputVector& u, const OutputVector& y,
+               const Matrixd<Outputs, Outputs>& R) {
+    Correct<Outputs>(u, y, m_h, R, m_residualFuncY, m_addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * This is useful for when the measurements available during a timestep's
+   * Correct() call vary. The h(x, u) passed to the constructor is used if one
+   * is not provided (the two-argument version of this function).
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement
+   *          vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
   template <int Rows>
   void Correct(
       const InputVector& u, const Vectord<Rows>& y,
@@ -180,7 +215,7 @@
    * @param y             Measurement vector.
    * @param h             A vector-valued function of x and u that returns
    *                      the measurement vector.
-   * @param R             Discrete measurement noise covariance matrix.
+   * @param R             Continuous measurement noise covariance matrix.
    * @param residualFuncY A function that computes the residual of two
    *                      measurement vectors (i.e. it subtracts them.)
    * @param addFuncX      A function that adds two state vectors.
diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
index a56b8b5..0b0e9de 100644
--- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
+++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
@@ -4,8 +4,11 @@
 
 #pragma once
 
-#include "Eigen/Cholesky"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include <functional>
+
+#include <Eigen/Cholesky>
+
+#include "frc/DARE.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/ExtendedKalmanFilter.h"
 #include "frc/system/Discretization.h"
@@ -36,13 +39,13 @@
 
   StateMatrix discA;
   StateMatrix discQ;
-  DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+  DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
 
   Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
 
   if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
-    m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
-        discA.transpose(), C.transpose(), discQ, discR);
+    m_initP =
+        DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
   } else {
     m_initP = StateMatrix::Zero();
   }
@@ -72,13 +75,13 @@
 
   StateMatrix discA;
   StateMatrix discQ;
-  DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+  DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
 
   Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
 
   if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
-    m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
-        discA.transpose(), C.transpose(), discQ, discR);
+    m_initP =
+        DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
   } else {
     m_initP = StateMatrix::Zero();
   }
@@ -95,7 +98,7 @@
   // Find discrete A and Q
   StateMatrix discA;
   StateMatrix discQ;
-  DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+  DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
 
   m_xHat = RK4(m_f, m_xHat, u, dt);
 
diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
index 2121284..f143493 100644
--- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
@@ -46,6 +46,10 @@
   /**
    * Constructs a state-space observer with the given plant.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param plant              The plant used for the prediction step.
    * @param stateStdDevs       Standard deviations of model states.
    * @param measurementStdDevs Standard deviations of measurements.
diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
index ca4f37c..7506c0d 100644
--- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
@@ -10,8 +10,9 @@
 #include <stdexcept>
 #include <string>
 
-#include "Eigen/Cholesky"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include <Eigen/Cholesky>
+
+#include "frc/DARE.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/KalmanFilter.h"
 #include "frc/system/Discretization.h"
@@ -31,7 +32,7 @@
 
   Matrixd<States, States> discA;
   Matrixd<States, States> discQ;
-  DiscretizeAQTaylor<States>(plant.A(), contQ, dt, &discA, &discQ);
+  DiscretizeAQ<States>(plant.A(), contQ, dt, &discA, &discQ);
 
   auto discR = DiscretizeR<Outputs>(contR, dt);
 
@@ -47,8 +48,8 @@
     throw std::invalid_argument(msg);
   }
 
-  Matrixd<States, States> P = drake::math::DiscreteAlgebraicRiccatiEquation(
-      discA.transpose(), C.transpose(), discQ, discR);
+  Matrixd<States, States> P =
+      DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
 
   // S = CPCᵀ + R
   Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
index d1967e9..d748164 100644
--- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
@@ -9,7 +9,7 @@
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "frc/EigenCore.h"
+#include "frc/estimator/PoseEstimator.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
 #include "frc/interpolation/TimeInterpolatableBuffer.h"
@@ -32,7 +32,9 @@
  * never call it, then this class will behave mostly like regular encoder
  * odometry.
  */
-class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
+class WPILIB_DLLEXPORT MecanumDrivePoseEstimator
+    : public PoseEstimator<MecanumDriveWheelSpeeds,
+                           MecanumDriveWheelPositions> {
  public:
   /**
    * Constructs a MecanumDrivePoseEstimator with default standard deviations
@@ -76,172 +78,8 @@
       const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
       const wpi::array<double, 3>& visionMeasurementStdDevs);
 
-  /**
-   * Sets the pose estimator's trust in vision measurements. This might be used
-   * to change trust in vision measurements after the autonomous period, or to
-   * change trust as distance to a vision target increases.
-   *
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose
-   *     measurement (x position in meters, y position in meters, and heading in
-   *     radians). Increase these numbers to trust the vision pose measurement
-   *     less.
-   */
-  void SetVisionMeasurementStdDevs(
-      const wpi::array<double, 3>& visionMeasurementStdDevs);
-
-  /**
-   * Resets the robot's position on the field.
-   *
-   * The gyroscope angle does not need to be reset in the user's robot code.
-   * The library automatically takes care of offsetting the gyro angle.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelPositions The distances measured at each wheel.
-   * @param pose      The position on the field that your robot is at.
-   */
-  void ResetPosition(const Rotation2d& gyroAngle,
-                     const MecanumDriveWheelPositions& wheelPositions,
-                     const Pose2d& pose);
-
-  /**
-   * Gets the estimated robot pose.
-   *
-   * @return The estimated robot pose in meters.
-   */
-  Pose2d GetEstimatedPosition() const;
-
-  /**
-   * Add a vision measurement to the Kalman Filter. This will correct
-   * the odometry pose estimate while still accounting for measurement noise.
-   *
-   * This method can be called as infrequently as you want, as long as you are
-   * calling Update() every loop.
-   *
-   * To promote stability of the pose estimate and make it robust to bad vision
-   * data, we recommend only adding vision measurements that are already within
-   * one meter or so of the current pose estimate.
-   *
-   * @param visionRobotPose The pose of the robot as measured by the vision
-   *     camera.
-   * @param timestamp The timestamp of the vision measurement in seconds. Note
-   *     that if you don't use your own time source by calling UpdateWithTime()
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
-   *     the epoch of this timestamp is the same epoch as
-   *     frc::Timer::GetFPGATimestamp().) This means that you should use
-   *     frc::Timer::GetFPGATimestamp() as your time source or sync the epochs.
-   */
-  void AddVisionMeasurement(const Pose2d& visionRobotPose,
-                            units::second_t timestamp);
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct
-   * the odometry pose estimate while still accounting for measurement noise.
-   *
-   * This method can be called as infrequently as you want, as long as you are
-   * calling Update() every loop.
-   *
-   * To promote stability of the pose estimate and make it robust to bad vision
-   * data, we recommend only adding vision measurements that are already within
-   * one meter or so of the current pose estimate.
-   *
-   * Note that the vision measurement standard deviations passed into this
-   * method will continue to apply to future measurements until a subsequent
-   * call to SetVisionMeasurementStdDevs() or this method.
-   *
-   * @param visionRobotPose The pose of the robot as measured by the vision
-   *     camera.
-   * @param timestamp The timestamp of the vision measurement in seconds. Note
-   *     that if you don't use your own time source by calling UpdateWithTime(),
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
-   *     the epoch of this timestamp is the same epoch as
-   *     frc::Timer::GetFPGATimestamp(). This means that you should use
-   *     frc::Timer::GetFPGATimestamp() as your time source in this case.
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose
-   *     measurement (x position in meters, y position in meters, and heading in
-   *     radians). Increase these numbers to trust the vision pose measurement
-   *     less.
-   */
-  void AddVisionMeasurement(
-      const Pose2d& visionRobotPose, units::second_t timestamp,
-      const wpi::array<double, 3>& visionMeasurementStdDevs) {
-    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-    AddVisionMeasurement(visionRobotPose, timestamp);
-  }
-
-  /**
-   * Updates the pose estimator with wheel encoder and gyro information. This
-   * should be called every loop.
-   *
-   * @param gyroAngle   The current gyro angle.
-   * @param wheelPositions The distances measured at each wheel.
-   * @return The estimated pose of the robot in meters.
-   */
-  Pose2d Update(const Rotation2d& gyroAngle,
-                const MecanumDriveWheelPositions& wheelPositions);
-
-  /**
-   * Updates the pose estimator with wheel encoder and gyro information. This
-   * should be called every loop.
-   *
-   * @param currentTime Time at which this method was called, in seconds.
-   * @param gyroAngle   The current gyroscope angle.
-   * @param wheelPositions The distances measured at each wheel.
-   * @return The estimated pose of the robot in meters.
-   */
-  Pose2d UpdateWithTime(units::second_t currentTime,
-                        const Rotation2d& gyroAngle,
-                        const MecanumDriveWheelPositions& wheelPositions);
-
  private:
-  struct InterpolationRecord {
-    // The pose observed given the current sensor inputs and the previous pose.
-    Pose2d pose;
-
-    // The current gyroscope angle.
-    Rotation2d gyroAngle;
-
-    // The distances measured at each wheel.
-    MecanumDriveWheelPositions wheelPositions;
-
-    /**
-     * Checks equality between this InterpolationRecord and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are equal.
-     */
-    bool operator==(const InterpolationRecord& other) const = default;
-
-    /**
-     * Checks inequality between this InterpolationRecord and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are not equal.
-     */
-    bool operator!=(const InterpolationRecord& other) const = default;
-
-    /**
-     * Interpolates between two InterpolationRecords.
-     *
-     * @param endValue The end value for the interpolation.
-     * @param i The interpolant (fraction).
-     *
-     * @return The interpolated state.
-     */
-    InterpolationRecord Interpolate(MecanumDriveKinematics& kinematics,
-                                    InterpolationRecord endValue,
-                                    double i) const;
-  };
-
-  MecanumDriveKinematics& m_kinematics;
-  MecanumDriveOdometry m_odometry;
-  wpi::array<double, 3> m_q{wpi::empty_array};
-  Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
-
-  TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
-      1.5_s, [this](const InterpolationRecord& start,
-                    const InterpolationRecord& end, double t) {
-        return start.Interpolate(this->m_kinematics, end, t);
-      }};
+  MecanumDriveOdometry m_odometryImpl;
 };
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
new file mode 100644
index 0000000..eb437a4
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
@@ -0,0 +1,228 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <Eigen/Core>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
+#include "frc/kinematics/Kinematics.h"
+#include "frc/kinematics/Odometry.h"
+#include "frc/kinematics/WheelPositions.h"
+#include "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+/**
+ * This class wraps odometry to fuse latency-compensated
+ * vision measurements with encoder measurements. Robot code should not use this
+ * directly- Instead, use the particular type for your drivetrain (e.g.,
+ * DifferentialDrivePoseEstimator). It will correct for noisy vision
+ * measurements and encoder drift. It is intended to be an easy drop-in for
+ * Odometry.
+ *
+ * Update() should be called every robot loop.
+ *
+ * AddVisionMeasurement() can be called as infrequently as you want; if you
+ * never call it, then this class will behave like regular encoder odometry.
+ */
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+class WPILIB_DLLEXPORT PoseEstimator {
+ public:
+  /**
+   * Constructs a PoseEstimator.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param odometry A correctly-configured odometry object for your drivetrain.
+   * @param stateStdDevs Standard deviations of the pose estimate (x position in
+   *     meters, y position in meters, and heading in radians). Increase these
+   *     numbers to trust your state estimate less.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, and heading in
+   *     radians). Increase these numbers to trust the vision pose measurement
+   *     less.
+   */
+  PoseEstimator(Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+                Odometry<WheelSpeeds, WheelPositions>& odometry,
+                const wpi::array<double, 3>& stateStdDevs,
+                const wpi::array<double, 3>& visionMeasurementStdDevs);
+
+  /**
+   * Sets the pose estimator's trust in vision measurements. This might be used
+   * to change trust in vision measurements after the autonomous period, or to
+   * change trust as distance to a vision target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, and heading in
+   *     radians). Increase these numbers to trust the vision pose measurement
+   *     less.
+   */
+  void SetVisionMeasurementStdDevs(
+      const wpi::array<double, 3>& visionMeasurementStdDevs);
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * The gyroscope angle does not need to be reset in the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distances traveled by the encoders.
+   * @param pose The estimated pose of the robot on the field.
+   */
+  void ResetPosition(const Rotation2d& gyroAngle,
+                     const WheelPositions& wheelPositions, const Pose2d& pose);
+
+  /**
+   * Gets the estimated robot pose.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  Pose2d GetEstimatedPosition() const;
+
+  /**
+   * Adds a vision measurement to the Kalman Filter. This will correct
+   * the odometry pose estimate while still accounting for measurement noise.
+   *
+   * This method can be called as infrequently as you want, as long as you are
+   * calling Update() every loop.
+   *
+   * To promote stability of the pose estimate and make it robust to bad vision
+   * data, we recommend only adding vision measurements that are already within
+   * one meter or so of the current pose estimate.
+   *
+   * @param visionRobotPose The pose of the robot as measured by the vision
+   *     camera.
+   * @param timestamp The timestamp of the vision measurement in seconds. Note
+   *     that if you don't use your own time source by calling UpdateWithTime(),
+   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
+   *     the epoch of this timestamp is the same epoch as
+   *     frc::Timer::GetFPGATimestamp(). This means that you should use
+   *     frc::Timer::GetFPGATimestamp() as your time source in this case.
+   */
+  void AddVisionMeasurement(const Pose2d& visionRobotPose,
+                            units::second_t timestamp);
+
+  /**
+   * Adds a vision measurement to the Kalman Filter. This will correct
+   * the odometry pose estimate while still accounting for measurement noise.
+   *
+   * This method can be called as infrequently as you want, as long as you are
+   * calling Update() every loop.
+   *
+   * To promote stability of the pose estimate and make it robust to bad vision
+   * data, we recommend only adding vision measurements that are already within
+   * one meter or so of the current pose estimate.
+   *
+   * Note that the vision measurement standard deviations passed into this
+   * method will continue to apply to future measurements until a subsequent
+   * call to SetVisionMeasurementStdDevs() or this method.
+   *
+   * @param visionRobotPose The pose of the robot as measured by the vision
+   *     camera.
+   * @param timestamp The timestamp of the vision measurement in seconds. Note
+   *     that if you don't use your own time source by calling UpdateWithTime(),
+   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
+   *     the epoch of this timestamp is the same epoch as
+   *     frc::Timer::GetFPGATimestamp(). This means that you should use
+   *     frc::Timer::GetFPGATimestamp() as your time source in this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision pose
+   *     measurement (x position in meters, y position in meters, and heading in
+   *     radians). Increase these numbers to trust the vision pose measurement
+   *     less.
+   */
+  void AddVisionMeasurement(
+      const Pose2d& visionRobotPose, units::second_t timestamp,
+      const wpi::array<double, 3>& visionMeasurementStdDevs) {
+    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    AddVisionMeasurement(visionRobotPose, timestamp);
+  }
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This
+   * should be called every loop.
+   *
+   * @param gyroAngle      The current gyro angle.
+   * @param wheelPositions The distances traveled by the encoders.
+   *
+   * @return The estimated pose of the robot in meters.
+   */
+  Pose2d Update(const Rotation2d& gyroAngle,
+                const WheelPositions& wheelPositions);
+
+  /**
+   * Updates the pose estimator with wheel encoder and gyro information. This
+   * should be called every loop.
+   *
+   * @param currentTime   The time at which this method was called.
+   * @param gyroAngle     The current gyro angle.
+   * @param wheelPositions The distances traveled by the encoders.
+   *
+   * @return The estimated pose of the robot in meters.
+   */
+  Pose2d UpdateWithTime(units::second_t currentTime,
+                        const Rotation2d& gyroAngle,
+                        const WheelPositions& wheelPositions);
+
+ private:
+  struct InterpolationRecord {
+    // The pose observed given the current sensor inputs and the previous pose.
+    Pose2d pose;
+
+    // The current gyroscope angle.
+    Rotation2d gyroAngle;
+
+    // The distances traveled by the wheels.
+    WheelPositions wheelPositions;
+
+    /**
+     * Checks equality between this InterpolationRecord and another object.
+     *
+     * @param other The other object.
+     * @return Whether the two objects are equal.
+     */
+    bool operator==(const InterpolationRecord& other) const = default;
+
+    /**
+     * Checks inequality between this InterpolationRecord and another object.
+     *
+     * @param other The other object.
+     * @return Whether the two objects are not equal.
+     */
+    bool operator!=(const InterpolationRecord& other) const = default;
+
+    /**
+     * Interpolates between two InterpolationRecords.
+     *
+     * @param endValue The end value for the interpolation.
+     * @param i The interpolant (fraction).
+     *
+     * @return The interpolated state.
+     */
+    InterpolationRecord Interpolate(
+        Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+        InterpolationRecord endValue, double i) const;
+  };
+
+  static constexpr units::second_t kBufferDuration = 1.5_s;
+
+  Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
+  Odometry<WheelSpeeds, WheelPositions>& m_odometry;
+  wpi::array<double, 3> m_q{wpi::empty_array};
+  Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
+
+  TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
+      kBufferDuration, [this](const InterpolationRecord& start,
+                              const InterpolationRecord& end, double t) {
+        return start.Interpolate(this->m_kinematics, end, t);
+      }};
+};
+}  // namespace frc
+
+#include "frc/estimator/PoseEstimator.inc"
diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
new file mode 100644
index 0000000..79d71bc
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
@@ -0,0 +1,165 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/estimator/PoseEstimator.h"
+
+namespace frc {
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+PoseEstimator<WheelSpeeds, WheelPositions>::PoseEstimator(
+    Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+    Odometry<WheelSpeeds, WheelPositions>& odometry,
+    const wpi::array<double, 3>& stateStdDevs,
+    const wpi::array<double, 3>& visionMeasurementStdDevs)
+    : m_kinematics(kinematics), m_odometry(odometry) {
+  for (size_t i = 0; i < 3; ++i) {
+    m_q[i] = stateStdDevs[i] * stateStdDevs[i];
+  }
+
+  SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+void PoseEstimator<WheelSpeeds, WheelPositions>::SetVisionMeasurementStdDevs(
+    const wpi::array<double, 3>& visionMeasurementStdDevs) {
+  wpi::array<double, 3> r{wpi::empty_array};
+  for (size_t i = 0; i < 3; ++i) {
+    r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
+  }
+
+  // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
+  // and C = I. See wpimath/algorithms.md.
+  for (size_t row = 0; row < 3; ++row) {
+    if (m_q[row] == 0.0) {
+      m_visionK(row, row) = 0.0;
+    } else {
+      m_visionK(row, row) =
+          m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
+    }
+  }
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPosition(
+    const Rotation2d& gyroAngle, const WheelPositions& wheelPositions,
+    const Pose2d& pose) {
+  // Reset state estimate and error covariance
+  m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
+  m_poseBuffer.Clear();
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
+    const {
+  return m_odometry.GetPose();
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+void PoseEstimator<WheelSpeeds, WheelPositions>::AddVisionMeasurement(
+    const Pose2d& visionRobotPose, units::second_t timestamp) {
+  // Step 0: If this measurement is old enough to be outside the pose buffer's
+  // timespan, skip.
+  if (!m_poseBuffer.GetInternalBuffer().empty() &&
+      m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
+          timestamp) {
+    return;
+  }
+
+  // Step 1: Get the estimated pose from when the vision measurement was made.
+  auto sample = m_poseBuffer.Sample(timestamp);
+
+  if (!sample.has_value()) {
+    return;
+  }
+
+  // Step 2: Measure the twist between the odometry pose and the vision pose
+  auto twist = sample.value().pose.Log(visionRobotPose);
+
+  // Step 3: We should not trust the twist entirely, so instead we scale this
+  // twist by a Kalman gain matrix representing how much we trust vision
+  // measurements compared to our current pose.
+  Eigen::Vector3d k_times_twist =
+      m_visionK *
+      Eigen::Vector3d{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
+
+  // Step 4: Convert back to Twist2d
+  Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
+                      units::meter_t{k_times_twist(1)},
+                      units::radian_t{k_times_twist(2)}};
+
+  // Step 5: Reset Odometry to state at sample with vision adjustment.
+  m_odometry.ResetPosition(sample.value().gyroAngle,
+                           sample.value().wheelPositions,
+                           sample.value().pose.Exp(scaledTwist));
+
+  // Step 6: Record the current pose to allow multiple measurements from the
+  // same timestamp
+  m_poseBuffer.AddSample(timestamp,
+                         {GetEstimatedPosition(), sample.value().gyroAngle,
+                          sample.value().wheelPositions});
+
+  // Step 7: Replay odometry inputs between sample time and latest recorded
+  // sample to update the pose buffer and correct odometry.
+  auto internal_buf = m_poseBuffer.GetInternalBuffer();
+
+  auto upper_bound =
+      std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp,
+                       [](const auto& pair, auto t) { return t > pair.first; });
+
+  for (auto entry = upper_bound; entry != internal_buf.end(); entry++) {
+    UpdateWithTime(entry->first, entry->second.gyroAngle,
+                   entry->second.wheelPositions);
+  }
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::Update(
+    const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) {
+  return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
+                        wheelPositions);
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::UpdateWithTime(
+    units::second_t currentTime, const Rotation2d& gyroAngle,
+    const WheelPositions& wheelPositions) {
+  m_odometry.Update(gyroAngle, wheelPositions);
+
+  WheelPositions internalWheelPositions = wheelPositions;
+
+  m_poseBuffer.AddSample(
+      currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions});
+
+  return GetEstimatedPosition();
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+typename PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord
+PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord::Interpolate(
+    Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+    InterpolationRecord endValue, double i) const {
+  if (i < 0) {
+    return *this;
+  } else if (i > 1) {
+    return endValue;
+  } else {
+    // Find the new wheel distance measurements.
+    WheelPositions wheels_lerp =
+        this->wheelPositions.Interpolate(endValue.wheelPositions, i);
+
+    // Find the new gyro angle.
+    auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
+
+    // Create a twist to represent the change based on the interpolated
+    // sensor inputs.
+    auto twist = kinematics.ToTwist2d(this->wheelPositions, wheels_lerp);
+    twist.dtheta = (gyro - gyroAngle).Radians();
+
+    return {pose.Exp(twist), gyro, wheels_lerp};
+  }
+}
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
index d07bafe..43831d7 100644
--- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
@@ -6,17 +6,15 @@
 
 #include <cmath>
 
-#include <fmt/format.h>
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
-#include <wpi/timestamp.h>
 
-#include "frc/EigenCore.h"
+#include "frc/estimator/PoseEstimator.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
-#include "frc/interpolation/TimeInterpolatableBuffer.h"
 #include "frc/kinematics/SwerveDriveKinematics.h"
 #include "frc/kinematics/SwerveDriveOdometry.h"
+#include "frc/kinematics/SwerveDriveWheelPositions.h"
 #include "units/time.h"
 
 namespace frc {
@@ -33,7 +31,9 @@
  * odometry.
  */
 template <size_t NumModules>
-class SwerveDrivePoseEstimator {
+class SwerveDrivePoseEstimator
+    : public PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
+                           SwerveDriveWheelPositions<NumModules>> {
  public:
   /**
    * Constructs a SwerveDrivePoseEstimator with default standard deviations
@@ -83,14 +83,10 @@
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
       const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
       const wpi::array<double, 3>& visionMeasurementStdDevs)
-      : m_kinematics{kinematics},
-        m_odometry{kinematics, gyroAngle, modulePositions, initialPose} {
-    for (size_t i = 0; i < 3; ++i) {
-      m_q[i] = stateStdDevs[i] * stateStdDevs[i];
-    }
-
-    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-  }
+      : PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
+                      SwerveDriveWheelPositions<NumModules>>(
+            kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
+        m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {}
 
   /**
    * Resets the robot's position on the field.
@@ -107,143 +103,11 @@
       const Rotation2d& gyroAngle,
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
       const Pose2d& pose) {
-    // Reset state estimate and error covariance
-    m_odometry.ResetPosition(gyroAngle, modulePositions, pose);
-    m_poseBuffer.Clear();
-  }
-
-  /**
-   * Gets the estimated robot pose.
-   *
-   * @return The estimated robot pose in meters.
-   */
-  Pose2d GetEstimatedPosition() const { return m_odometry.GetPose(); }
-
-  /**
-   * Sets the pose estimator's trust in vision measurements. This might be used
-   * to change trust in vision measurements after the autonomous period, or to
-   * change trust as distance to a vision target increases.
-   *
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose
-   *     measurement (x position in meters, y position in meters, and heading in
-   *     radians). Increase these numbers to trust the vision pose measurement
-   *     less.
-   */
-  void SetVisionMeasurementStdDevs(
-      const wpi::array<double, 3>& visionMeasurementStdDevs) {
-    wpi::array<double, 3> r{wpi::empty_array};
-    for (size_t i = 0; i < 3; ++i) {
-      r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
-    }
-
-    // Solve for closed form Kalman gain for continuous Kalman filter with A = 0
-    // and C = I. See wpimath/algorithms.md.
-    for (size_t row = 0; row < 3; ++row) {
-      if (m_q[row] == 0.0) {
-        m_visionK(row, row) = 0.0;
-      } else {
-        m_visionK(row, row) =
-            m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
-      }
-    }
-  }
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct the
-   * odometry pose estimate while still accounting for measurement noise.
-   *
-   * This method can be called as infrequently as you want, as long as you are
-   * calling Update() every loop.
-   *
-   * To promote stability of the pose estimate and make it robust to bad vision
-   * data, we recommend only adding vision measurements that are already within
-   * one meter or so of the current pose estimate.
-   *
-   * @param visionRobotPose The pose of the robot as measured by the vision
-   *    camera.
-   * @param timestamp The timestamp of the vision measurement in seconds. Note
-   *     that if you don't use your own time source by calling UpdateWithTime()
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
-   *     the epoch of this timestamp is the same epoch as
-   *     frc::Timer::GetFPGATimestamp().) This means that you should use
-   *     frc::Timer::GetFPGATimestamp() as your time source or sync the epochs.
-   */
-  void AddVisionMeasurement(const Pose2d& visionRobotPose,
-                            units::second_t timestamp) {
-    // Step 1: Get the estimated pose from when the vision measurement was made.
-    auto sample = m_poseBuffer.Sample(timestamp);
-
-    if (!sample.has_value()) {
-      return;
-    }
-
-    // Step 2: Measure the twist between the odometry pose and the vision pose
-    auto twist = sample.value().pose.Log(visionRobotPose);
-
-    // Step 3: We should not trust the twist entirely, so instead we scale this
-    // twist by a Kalman gain matrix representing how much we trust vision
-    // measurements compared to our current pose.
-    frc::Vectord<3> k_times_twist =
-        m_visionK * frc::Vectord<3>{twist.dx.value(), twist.dy.value(),
-                                    twist.dtheta.value()};
-
-    // Step 4: Convert back to Twist2d
-    Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
-                        units::meter_t{k_times_twist(1)},
-                        units::radian_t{k_times_twist(2)}};
-
-    // Step 5: Reset Odometry to state at sample with vision adjustment.
-    m_odometry.ResetPosition(sample.value().gyroAngle,
-                             sample.value().modulePostions,
-                             sample.value().pose.Exp(scaledTwist));
-
-    // Step 6: Replay odometry inputs between sample time and latest recorded
-    // sample to update the pose buffer and correct odometry.
-    auto internal_buf = m_poseBuffer.GetInternalBuffer();
-
-    auto upper_bound = std::lower_bound(
-        internal_buf.begin(), internal_buf.end(), timestamp,
-        [](const auto& pair, auto t) { return t > pair.first; });
-
-    for (auto entry = upper_bound; entry != internal_buf.end(); entry++) {
-      UpdateWithTime(entry->first, entry->second.gyroAngle,
-                     entry->second.modulePostions);
-    }
-  }
-
-  /**
-   * Adds a vision measurement to the Kalman Filter. This will correct the
-   * odometry pose estimate while still accounting for measurement noise.
-   *
-   * This method can be called as infrequently as you want, as long as you are
-   * calling Update() every loop.
-   *
-   * To promote stability of the pose estimate and make it robust to bad vision
-   * data, we recommend only adding vision measurements that are already within
-   * one meter or so of the current pose estimate.
-   *
-   * Note that the vision measurement standard deviations passed into this
-   * method will continue to apply to future measurements until a subsequent
-   * call to SetVisionMeasurementStdDevs() or this method.
-   *
-   * @param visionRobotPose The pose of the robot as measured by the vision
-   *     camera.
-   * @param timestamp The timestamp of the vision measurement in seconds. Note
-   *     that if you don't use your own time source by calling UpdateWithTime(),
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e.,
-   *     the epoch of this timestamp is the same epoch as
-   *     frc::Timer::GetFPGATimestamp(). This means that you should use
-   *     frc::Timer::GetFPGATimestamp() as your time source in this case.
-   * @param visionMeasurementStdDevs Standard deviations of the vision pose
-   *     measurement (x position in meters, y position in meters, and heading in
-   *     radians). Increase these numbers to trust the vision pose measurement
-   *     less.
-   */
-  void AddVisionMeasurement(
-      const Pose2d& visionRobotPose, units::second_t timestamp,
-      const wpi::array<double, 3>& visionMeasurementStdDevs) {
-    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
-    AddVisionMeasurement(visionRobotPose, timestamp);
+    PoseEstimator<
+        SwerveDriveWheelSpeeds<NumModules>,
+        SwerveDriveWheelPositions<NumModules>>::ResetPosition(gyroAngle,
+                                                              {modulePositions},
+                                                              pose);
   }
 
   /**
@@ -258,8 +122,10 @@
   Pose2d Update(
       const Rotation2d& gyroAngle,
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
-    return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
-                          modulePositions);
+    return PoseEstimator<
+        SwerveDriveWheelSpeeds<NumModules>,
+        SwerveDriveWheelPositions<NumModules>>::Update(gyroAngle,
+                                                       {modulePositions});
   }
 
   /**
@@ -275,109 +141,13 @@
   Pose2d UpdateWithTime(
       units::second_t currentTime, const Rotation2d& gyroAngle,
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
-    m_odometry.Update(gyroAngle, modulePositions);
-
-    wpi::array<SwerveModulePosition, NumModules> internalModulePositions{
-        wpi::empty_array};
-
-    for (size_t i = 0; i < NumModules; i++) {
-      internalModulePositions[i].distance = modulePositions[i].distance;
-      internalModulePositions[i].angle = modulePositions[i].angle;
-    }
-
-    m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle,
-                                         internalModulePositions});
-
-    return GetEstimatedPosition();
+    return PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
+                         SwerveDriveWheelPositions<NumModules>>::
+        UpdateWithTime(currentTime, gyroAngle, {modulePositions});
   }
 
  private:
-  struct InterpolationRecord {
-    // The pose observed given the current sensor inputs and the previous pose.
-    Pose2d pose;
-
-    // The current gyroscope angle.
-    Rotation2d gyroAngle;
-
-    // The distances traveled and rotations meaured at each module.
-    wpi::array<SwerveModulePosition, NumModules> modulePostions;
-
-    /**
-     * Checks equality between this InterpolationRecord and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are equal.
-     */
-    bool operator==(const InterpolationRecord& other) const = default;
-
-    /**
-     * Checks inequality between this InterpolationRecord and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are not equal.
-     */
-    bool operator!=(const InterpolationRecord& other) const = default;
-
-    /**
-     * Interpolates between two InterpolationRecords.
-     *
-     * @param endValue The end value for the interpolation.
-     * @param i The interpolant (fraction).
-     *
-     * @return The interpolated state.
-     */
-    InterpolationRecord Interpolate(
-        SwerveDriveKinematics<NumModules>& kinematics,
-        InterpolationRecord endValue, double i) const {
-      if (i < 0) {
-        return *this;
-      } else if (i > 1) {
-        return endValue;
-      } else {
-        // Find the new module distances.
-        wpi::array<SwerveModulePosition, NumModules> modulePositions{
-            wpi::empty_array};
-        // Find the distance between this measurement and the
-        // interpolated measurement.
-        wpi::array<SwerveModulePosition, NumModules> modulesDelta{
-            wpi::empty_array};
-
-        for (size_t i = 0; i < NumModules; i++) {
-          modulePositions[i].distance =
-              wpi::Lerp(this->modulePostions[i].distance,
-                        endValue.modulePostions[i].distance, i);
-          modulePositions[i].angle =
-              wpi::Lerp(this->modulePostions[i].angle,
-                        endValue.modulePostions[i].angle, i);
-
-          modulesDelta[i].distance =
-              modulePositions[i].distance - this->modulePostions[i].distance;
-          modulesDelta[i].angle = modulePositions[i].angle;
-        }
-
-        // Find the new gyro angle.
-        auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
-
-        // Create a twist to represent this changed based on the interpolated
-        // sensor inputs.
-        auto twist = kinematics.ToTwist2d(modulesDelta);
-        twist.dtheta = (gyro - gyroAngle).Radians();
-
-        return {pose.Exp(twist), gyro, modulePositions};
-      }
-    }
-  };
-
-  SwerveDriveKinematics<NumModules>& m_kinematics;
-  SwerveDriveOdometry<NumModules> m_odometry;
-  wpi::array<double, 3> m_q{wpi::empty_array};
-  Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
-
-  TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer{
-      1.5_s, [this](const InterpolationRecord& start,
-                    const InterpolationRecord& end, double t) {
-        return start.Interpolate(this->m_kinematics, end, t);
-      }};
+  SwerveDriveOdometry<NumModules> m_odometryImpl;
 };
 
 extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
index 39ce615..9526f0c 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
@@ -58,6 +58,10 @@
   /**
    * Constructs an unscented Kalman filter.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param f                  A vector-valued function of x and u that returns
    *                           the derivative of the state vector.
    * @param h                  A vector-valued function of x and u that returns
@@ -78,6 +82,10 @@
    * you have angles in the state or measurements, because they allow you to
    * correctly account for the modular nature of angle arithmetic.
    *
+   * See
+   * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices
+   * for how to select the standard deviations.
+   *
    * @param f                  A vector-valued function of x and u that returns
    *                           the derivative of the state vector.
    * @param h                  A vector-valued function of x and u that returns
@@ -205,6 +213,21 @@
   /**
    * Correct the state estimate x-hat using the measurements in y.
    *
+   * This is useful for when the measurement noise covariances vary.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  void Correct(const InputVector& u, const OutputVector& y,
+               const Matrixd<Outputs, Outputs>& R) {
+    Correct<Outputs>(u, y, m_h, R, m_meanFuncY, m_residualFuncY,
+                     m_residualFuncX, m_addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
    * This is useful for when the measurements available during a timestep's
    * Correct() call vary. The h(x, u) passed to the constructor is used if one
    * is not provided (the two-argument version of this function).
@@ -213,7 +236,7 @@
    * @param y Measurement vector.
    * @param h A vector-valued function of x and u that returns the measurement
    *          vector.
-   * @param R Measurement noise covariance matrix (continuous-time).
+   * @param R Continuous measurement noise covariance matrix.
    */
   template <int Rows>
   void Correct(
@@ -232,7 +255,7 @@
    * @param y             Measurement vector.
    * @param h             A vector-valued function of x and u that returns the
    *                      measurement vector.
-   * @param R             Measurement noise covariance matrix (continuous-time).
+   * @param R             Continuous measurement noise covariance matrix.
    * @param meanFuncY     A function that computes the mean of 2 * States + 1
    *                      measurement vectors using a given set of weights.
    * @param residualFuncY A function that computes the residual of two
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
index a6744bf..c693197 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
@@ -4,7 +4,10 @@
 
 #pragma once
 
-#include "Eigen/Cholesky"
+#include <functional>
+
+#include <Eigen/Cholesky>
+
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/UnscentedKalmanFilter.h"
 #include "frc/estimator/UnscentedTransform.h"
@@ -76,7 +79,7 @@
       NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
   StateMatrix discA;
   StateMatrix discQ;
-  DiscretizeAQTaylor<States>(contA, m_contQ, m_dt, &discA, &discQ);
+  DiscretizeAQ<States>(contA, m_contQ, m_dt, &discA, &discQ);
   Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
 
   Matrixd<States, 2 * States + 1> sigmas =
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
index e28f094..bec3bc8 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
@@ -4,9 +4,11 @@
 
 #pragma once
 
+#include <functional>
 #include <tuple>
 
-#include "Eigen/QR"
+#include <Eigen/QR>
+
 #include "frc/EigenCore.h"
 
 namespace frc {
diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
index 0fb4f48..fc558df 100644
--- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h
+++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
@@ -11,10 +11,10 @@
 #include <stdexcept>
 #include <vector>
 
+#include <Eigen/QR>
 #include <wpi/array.h>
 #include <wpi/circular_buffer.h>
 
-#include "Eigen/QR"
 #include "frc/EigenCore.h"
 #include "units/time.h"
 #include "wpimath/MathShared.h"
@@ -95,7 +95,7 @@
     static int instances = 0;
     instances++;
     wpi::math::MathSharedStore::ReportUsage(
-        wpi::math::MathUsageId::kFilter_Linear, 1);
+        wpi::math::MathUsageId::kFilter_Linear, instances);
   }
 
   /**
diff --git a/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h b/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
index 542cd94..17d0d21 100644
--- a/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
+++ b/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
@@ -10,6 +10,7 @@
 #include <wpi/timestamp.h>
 
 #include "units/time.h"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 /**
@@ -45,7 +46,8 @@
       : m_positiveRateLimit{positiveRateLimit},
         m_negativeRateLimit{negativeRateLimit},
         m_prevVal{initialValue},
-        m_prevTime{units::microsecond_t(wpi::Now())} {}
+        m_prevTime{
+            units::microsecond_t(wpi::math::MathSharedStore::GetTimestamp())} {}
 
   /**
    * Creates a new SlewRateLimiter with the given positive rate limit and
@@ -57,19 +59,6 @@
       : SlewRateLimiter(rateLimit, -rateLimit) {}
 
   /**
-   * Creates a new SlewRateLimiter with the given positive rate limit and
-   * negative rate limit of -rateLimit and initial value.
-   *
-   * @param rateLimit The rate-of-change limit.
-   * @param initialValue The initial value of the input.
-   */
-  WPI_DEPRECATED(
-      "Use SlewRateLimiter(Rate_t positiveRateLimit, Rate_t negativeRateLimit, "
-      "Unit_t initalValue) instead")
-  SlewRateLimiter(Rate_t rateLimit, Unit_t initialValue)
-      : SlewRateLimiter(rateLimit, -rateLimit, initialValue) {}
-
-  /**
    * Filters the input to limit its slew rate.
    *
    * @param input The input value whose slew rate is to be limited.
@@ -77,7 +66,7 @@
    * rate.
    */
   Unit_t Calculate(Unit_t input) {
-    units::second_t currentTime = units::microsecond_t(wpi::Now());
+    units::second_t currentTime = wpi::math::MathSharedStore::GetTimestamp();
     units::second_t elapsedTime = currentTime - m_prevTime;
     m_prevVal +=
         std::clamp(input - m_prevVal, m_negativeRateLimit * elapsedTime,
@@ -94,7 +83,7 @@
    */
   void Reset(Unit_t value) {
     m_prevVal = value;
-    m_prevTime = units::microsecond_t(wpi::Now());
+    m_prevTime = wpi::math::MathSharedStore::GetTimestamp();
   }
 
  private:
diff --git a/wpimath/src/main/native/include/frc/fmt/Eigen.h b/wpimath/src/main/native/include/frc/fmt/Eigen.h
index c6b2ee6..2438b9f 100644
--- a/wpimath/src/main/native/include/frc/fmt/Eigen.h
+++ b/wpimath/src/main/native/include/frc/fmt/Eigen.h
@@ -4,120 +4,41 @@
 
 #pragma once
 
+#include <concepts>
+
+#include <Eigen/Core>
+#include <Eigen/SparseCore>
 #include <fmt/format.h>
 
-#include "Eigen/Core"
-#include "Eigen/SparseCore"
-
 /**
- * Formatter for Eigen::Matrix<double, Rows, Cols>.
- *
- * @tparam Rows Number of rows.
- * @tparam Cols Number of columns.
- * @tparam Args Defaulted template arguments to Eigen::Matrix<>.
+ * Formatter for classes derived from Eigen::MatrixBase<Derived> or
+ * Eigen::SparseCompressedBase<Derived>.
  */
-template <int Rows, int Cols, int... Args>
-struct fmt::formatter<Eigen::Matrix<double, Rows, Cols, Args...>> {
-  /**
-   * Storage for format specifier.
-   */
-  char presentation = 'f';
-
-  /**
-   * Format string parser.
-   *
-   * @param ctx Format string context.
-   */
+template <typename Derived, typename CharT>
+  requires std::derived_from<Derived, Eigen::MatrixBase<Derived>> ||
+           std::derived_from<Derived, Eigen::SparseCompressedBase<Derived>>
+struct fmt::formatter<Derived, CharT> {
   constexpr auto parse(fmt::format_parse_context& ctx) {
-    auto it = ctx.begin(), end = ctx.end();
-    if (it != end && (*it == 'f' || *it == 'e')) {
-      presentation = *it++;
-    }
-
-    if (it != end && *it != '}') {
-      throw fmt::format_error("invalid format");
-    }
-
-    return it;
+    return m_underlying.parse(ctx);
   }
 
-  /**
-   * Writes out a formatted matrix.
-   *
-   * @tparam FormatContext Format string context type.
-   * @param mat Matrix to format.
-   * @param ctx Format string context.
-   */
-  template <typename FormatContext>
-  auto format(const Eigen::Matrix<double, Rows, Cols, Args...>& mat,
-              FormatContext& ctx) {
+  auto format(const Derived& mat, fmt::format_context& ctx) const {
     auto out = ctx.out();
-    for (int i = 0; i < mat.rows(); ++i) {
-      for (int j = 0; j < mat.cols(); ++j) {
-        out = fmt::format_to(out, "  {:f}", mat(i, j));
+
+    for (int row = 0; row < mat.rows(); ++row) {
+      for (int col = 0; col < mat.cols(); ++col) {
+        out = fmt::format_to(out, "  ");
+        out = m_underlying.format(mat.coeff(row, col), ctx);
       }
 
-      if (i < mat.rows() - 1) {
+      if (row < mat.rows() - 1) {
         out = fmt::format_to(out, "\n");
       }
     }
 
     return out;
   }
-};
 
-/**
- * Formatter for Eigen::SparseMatrix<double>.
- *
- * @tparam Options Union of bit flags controlling the storage scheme.
- * @tparam StorageIndex The type of the indices.
- */
-template <int Options, typename StorageIndex>
-struct fmt::formatter<Eigen::SparseMatrix<double, Options, StorageIndex>> {
-  /**
-   * Storage for format specifier.
-   */
-  char presentation = 'f';
-
-  /**
-   * Format string parser.
-   *
-   * @param ctx Format string context.
-   */
-  constexpr auto parse(fmt::format_parse_context& ctx) {
-    auto it = ctx.begin(), end = ctx.end();
-    if (it != end && (*it == 'f' || *it == 'e')) {
-      presentation = *it++;
-    }
-
-    if (it != end && *it != '}') {
-      throw fmt::format_error("invalid format");
-    }
-
-    return it;
-  }
-
-  /**
-   * Writes out a formatted matrix.
-   *
-   * @tparam FormatContext Format string context type.
-   * @param mat Matrix to format.
-   * @param ctx Format string context.
-   */
-  template <typename FormatContext>
-  auto format(const Eigen::SparseMatrix<double, Options, StorageIndex>& mat,
-              FormatContext& ctx) {
-    auto out = ctx.out();
-    for (int i = 0; i < mat.rows(); ++i) {
-      for (int j = 0; j < mat.cols(); ++j) {
-        out = fmt::format_to(out, "  {:f}", mat.coeff(i, j));
-      }
-
-      if (i < mat.rows() - 1) {
-        out = fmt::format_to(out, "\n");
-      }
-    }
-
-    return out;
-  }
+ private:
+  fmt::formatter<typename Derived::Scalar, CharT> m_underlying;
 };
diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h b/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h
index 58b966a..2b471b7 100644
--- a/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h
+++ b/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h
@@ -4,9 +4,9 @@
 
 #pragma once
 
+#include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 
-#include "frc/EigenCore.h"
 #include "frc/geometry/Pose3d.h"
 #include "frc/geometry/Rotation3d.h"
 
@@ -67,7 +67,7 @@
  private:
   friend class CoordinateSystem;
 
-  Vectord<3> m_axis;
+  Eigen::Vector3d m_axis;
 };
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
index 232455f..f5e71f2 100644
--- a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
+++ b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
@@ -6,7 +6,6 @@
 
 #include <wpi/SymbolExports.h>
 
-#include "frc/EigenCore.h"
 #include "frc/geometry/CoordinateAxis.h"
 #include "frc/geometry/Pose3d.h"
 #include "frc/geometry/Rotation3d.h"
diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h
index d096e8c..970f792 100644
--- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h
@@ -4,15 +4,18 @@
 
 #pragma once
 
+#include <initializer_list>
+#include <span>
+
 #include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
-#include "Transform2d.h"
-#include "Translation2d.h"
-#include "Twist2d.h"
-
-namespace wpi {
-class json;
-}  // namespace wpi
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Transform2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "frc/geometry/Twist2d.h"
 
 namespace frc {
 
@@ -120,6 +123,15 @@
   constexpr Pose2d operator/(double scalar) const;
 
   /**
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by.
+   *
+   * @return The rotated pose.
+   */
+  constexpr Pose2d RotateBy(const Rotation2d& other) const;
+
+  /**
    * Transforms the pose by the given transformation and returns the new pose.
    * See + operator for the matrix multiplication performed.
    *
@@ -176,6 +188,20 @@
    */
   Twist2d Log(const Pose2d& end) const;
 
+  /**
+   * Returns the nearest Pose2d from a collection of poses
+   * @param poses The collection of poses.
+   * @return The nearest Pose2d from the collection.
+   */
+  Pose2d Nearest(std::span<const Pose2d> poses) const;
+
+  /**
+   * Returns the nearest Pose2d from a collection of poses
+   * @param poses The collection of poses.
+   * @return The nearest Pose2d from the collection.
+   */
+  Pose2d Nearest(std::initializer_list<Pose2d> poses) const;
+
  private:
   Translation2d m_translation;
   Rotation2d m_rotation;
@@ -189,4 +215,38 @@
 
 }  // namespace frc
 
-#include "Pose2d.inc"
+template <>
+struct wpi::Struct<frc::Pose2d> {
+  static constexpr std::string_view kTypeString = "struct:Pose2d";
+  static constexpr size_t kSize = wpi::Struct<frc::Translation2d>::kSize +
+                                  wpi::Struct<frc::Rotation2d>::kSize;
+  static constexpr std::string_view kSchema =
+      "Translation2d translation;Rotation2d rotation";
+  static frc::Pose2d Unpack(std::span<const uint8_t, kSize> data) {
+    return {wpi::UnpackStruct<frc::Translation2d, 0>(data),
+            wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data)};
+  }
+  static void Pack(std::span<uint8_t, kSize> data, const frc::Pose2d& value) {
+    wpi::PackStruct<0>(data, value.Translation());
+    wpi::PackStruct<kRotationOff>(data, value.Rotation());
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation2d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation2d>(fn);
+  }
+
+ private:
+  static constexpr size_t kRotationOff = wpi::Struct<frc::Translation2d>::kSize;
+};
+
+static_assert(wpi::HasNestedStruct<frc::Pose2d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Pose2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Pose2d& value);
+};
+
+#include "frc/geometry/Pose2d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc
index c549f26..2764d16 100644
--- a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc
@@ -31,6 +31,10 @@
   return *this * (1.0 / scalar);
 }
 
+constexpr Pose2d Pose2d::RotateBy(const Rotation2d& other) const {
+  return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
+}
+
 constexpr Pose2d Pose2d::TransformBy(const Transform2d& other) const {
   return {m_translation + (other.Translation().RotateBy(m_rotation)),
           other.Rotation() + m_rotation};
diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h
index 8c7ace3..b5a0e03 100644
--- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h
@@ -5,15 +5,15 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
-#include "Pose2d.h"
-#include "Transform3d.h"
-#include "Translation3d.h"
-#include "Twist3d.h"
-
-namespace wpi {
-class json;
-}  // namespace wpi
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation3d.h"
+#include "frc/geometry/Transform3d.h"
+#include "frc/geometry/Translation3d.h"
+#include "frc/geometry/Twist3d.h"
 
 namespace frc {
 
@@ -56,7 +56,9 @@
 
   /**
    * Transforms the pose by the given transformation and returns the new
-   * transformed pose.
+   * transformed pose. The transform is applied relative to the pose's frame.
+   * Note that this differs from Pose3d::RotateBy(const Rotation3d&), which is
+   * applied relative to the global frame and around the origin.
    *
    * @param other The transform to transform the pose by.
    *
@@ -131,8 +133,20 @@
   Pose3d operator/(double scalar) const;
 
   /**
-   * Transforms the pose by the given transformation and returns the new pose.
-   * See + operator for the matrix multiplication performed.
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by, which is applied
+   * extrinsically (from the global frame).
+   *
+   * @return The rotated pose.
+   */
+  Pose3d RotateBy(const Rotation3d& other) const;
+
+  /**
+   * Transforms the pose by the given transformation and returns the new
+   * transformed pose. The transform is applied relative to the pose's frame.
+   * Note that this differs from Pose3d::RotateBy(const Rotation3d&), which is
+   * applied relative to the global frame and around the origin.
    *
    * @param other The transform to transform the pose by.
    *
@@ -202,3 +216,37 @@
 void from_json(const wpi::json& json, Pose3d& pose);
 
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Pose3d> {
+  static constexpr std::string_view kTypeString = "struct:Pose3d";
+  static constexpr size_t kSize = wpi::Struct<frc::Translation3d>::kSize +
+                                  wpi::Struct<frc::Rotation3d>::kSize;
+  static constexpr std::string_view kSchema =
+      "Translation3d translation;Rotation3d rotation";
+  static frc::Pose3d Unpack(std::span<const uint8_t, kSize> data) {
+    return {wpi::UnpackStruct<frc::Translation3d, 0>(data),
+            wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data)};
+  }
+  static void Pack(std::span<uint8_t, kSize> data, const frc::Pose3d& value) {
+    wpi::PackStruct<0>(data, value.Translation());
+    wpi::PackStruct<kRotationOff>(data, value.Rotation());
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation3d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation3d>(fn);
+  }
+
+ private:
+  static constexpr size_t kRotationOff = wpi::Struct<frc::Translation3d>::kSize;
+};
+
+static_assert(wpi::HasNestedStruct<frc::Pose3d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Pose3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Pose3d& value);
+};
diff --git a/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/wpimath/src/main/native/include/frc/geometry/Quaternion.h
index b5a318b..63a3af2 100644
--- a/wpimath/src/main/native/include/frc/geometry/Quaternion.h
+++ b/wpimath/src/main/native/include/frc/geometry/Quaternion.h
@@ -4,13 +4,11 @@
 
 #pragma once
 
+#include <Eigen/Core>
 #include <wpi/SymbolExports.h>
-
-#include "frc/EigenCore.h"
-
-namespace wpi {
-class json;
-}  // namespace wpi
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 namespace frc {
 
@@ -32,6 +30,34 @@
   Quaternion(double w, double x, double y, double z);
 
   /**
+   * Adds with another quaternion.
+   *
+   * @param other the other quaternion
+   */
+  Quaternion operator+(const Quaternion& other) const;
+
+  /**
+   * Subtracts another quaternion.
+   *
+   * @param other the other quaternion
+   */
+  Quaternion operator-(const Quaternion& other) const;
+
+  /**
+   * Multiples with a scalar value.
+   *
+   * @param other the scalar value
+   */
+  Quaternion operator*(const double other) const;
+
+  /**
+   * Divides by a scalar value.
+   *
+   * @param other the scalar value
+   */
+  Quaternion operator/(const double other) const;
+
+  /**
    * Multiply with another quaternion.
    *
    * @param other The other quaternion.
@@ -47,6 +73,16 @@
   bool operator==(const Quaternion& other) const;
 
   /**
+   * Returns the elementwise product of two quaternions.
+   */
+  double Dot(const Quaternion& other) const;
+
+  /**
+   * Returns the conjugate of the quaternion.
+   */
+  Quaternion Conjugate() const;
+
+  /**
    * Returns the inverse of the quaternion.
    */
   Quaternion Inverse() const;
@@ -57,6 +93,52 @@
   Quaternion Normalize() const;
 
   /**
+   * Calculates the L2 norm of the quaternion.
+   */
+  double Norm() const;
+
+  /**
+   * Calculates this quaternion raised to a power.
+   *
+   * @param t the power to raise this quaternion to.
+   */
+  Quaternion Pow(const double t) const;
+
+  /**
+   * Matrix exponential of a quaternion.
+   *
+   * @param other the "Twist" that will be applied to this quaternion.
+   */
+  Quaternion Exp(const Quaternion& other) const;
+
+  /**
+   * Matrix exponential of a quaternion.
+   *
+   * source: wpimath/algorithms.md
+   *
+   *  If this quaternion is in 𝖘𝖔(3) and you are looking for an element of
+   * SO(3), use FromRotationVector
+   */
+  Quaternion Exp() const;
+
+  /**
+   * Log operator of a quaternion.
+   *
+   * @param other The quaternion to map this quaternion onto
+   */
+  Quaternion Log(const Quaternion& other) const;
+
+  /**
+   * Log operator of a quaternion.
+   *
+   * source:  wpimath/algorithms.md
+   *
+   * If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3),
+   * use ToRotationVector
+   */
+  Quaternion Log() const;
+
+  /**
    * Returns W component of the quaternion.
    */
   double W() const;
@@ -83,8 +165,20 @@
    */
   Eigen::Vector3d ToRotationVector() const;
 
+  /**
+   * Returns the quaternion representation of this rotation vector.
+   *
+   * This is also the exp operator of 𝖘𝖔(3).
+   *
+   * source: wpimath/algorithms.md
+   */
+  static Quaternion FromRotationVector(const Eigen::Vector3d& rvec);
+
  private:
+  // Scalar r in versor form
   double m_r = 1.0;
+
+  // Vector v in versor form
   Eigen::Vector3d m_v{0.0, 0.0, 0.0};
 };
 
@@ -95,3 +189,32 @@
 void from_json(const wpi::json& json, Quaternion& quaternion);
 
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Quaternion> {
+  static constexpr std::string_view kTypeString = "struct:Quaternion";
+  static constexpr size_t kSize = 32;
+  static constexpr std::string_view kSchema =
+      "double w;double x;double y;double z";
+  static frc::Quaternion Unpack(std::span<const uint8_t, 32> data) {
+    return {wpi::UnpackStruct<double, 0>(data),
+            wpi::UnpackStruct<double, 8>(data),
+            wpi::UnpackStruct<double, 16>(data),
+            wpi::UnpackStruct<double, 24>(data)};
+  }
+  static void Pack(std::span<uint8_t, 32> data, const frc::Quaternion& value) {
+    wpi::PackStruct<0>(data, value.W());
+    wpi::PackStruct<8>(data, value.X());
+    wpi::PackStruct<16>(data, value.Y());
+    wpi::PackStruct<24>(data, value.Z());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Quaternion> {
+  static constexpr std::string_view kTypeString = "proto:Quaternion";
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Quaternion Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Quaternion& value);
+};
diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
index 406ef3c..96041a4 100644
--- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
@@ -5,13 +5,12 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 #include "units/angle.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace frc {
 
 /**
@@ -179,4 +178,25 @@
 
 }  // namespace frc
 
-#include "Rotation2d.inc"
+template <>
+struct wpi::Struct<frc::Rotation2d> {
+  static constexpr std::string_view kTypeString = "struct:Rotation2d";
+  static constexpr size_t kSize = 8;
+  static constexpr std::string_view kSchema = "double value";
+  static frc::Rotation2d Unpack(std::span<const uint8_t, 8> data) {
+    return units::radian_t{wpi::UnpackStruct<double>(data)};
+  }
+  static void Pack(std::span<uint8_t, 8> data, const frc::Rotation2d& value) {
+    wpi::PackStruct(data, value.Radians().value());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Rotation2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Rotation2d& value);
+};
+
+#include "frc/geometry/Rotation2d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
index eb31ebd..dbe9e35 100644
--- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
@@ -23,7 +23,8 @@
     : Rotation2d(units::radian_t{value}) {}
 
 constexpr Rotation2d::Rotation2d(double x, double y) {
-  const auto magnitude = gcem::hypot(x, y);
+  double magnitude =
+      std::is_constant_evaluated() ? gcem::hypot(x, y) : std::hypot(x, y);
   if (magnitude > 1e-6) {
     m_sin = y / magnitude;
     m_cos = x / magnitude;
diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
index 7c1a60d..6d04715 100644
--- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
@@ -4,17 +4,16 @@
 
 #pragma once
 
+#include <Eigen/Core>
 #include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
-#include "Quaternion.h"
-#include "Rotation2d.h"
-#include "frc/EigenCore.h"
+#include "frc/geometry/Quaternion.h"
+#include "frc/geometry/Rotation2d.h"
 #include "units/angle.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace frc {
 
 /**
@@ -57,7 +56,16 @@
    * @param axis The rotation axis.
    * @param angle The rotation around the axis.
    */
-  Rotation3d(const Vectord<3>& axis, units::radian_t angle);
+  Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle);
+
+  /**
+   * Constructs a Rotation3d with the given rotation vector representation. This
+   * representation is equivalent to axis-angle, where the normalized axis is
+   * multiplied by the rotation around the axis in radians.
+   *
+   * @param rvec The rotation vector.
+   */
+  explicit Rotation3d(const Eigen::Vector3d& rvec);
 
   /**
    * Constructs a Rotation3d from a rotation matrix.
@@ -65,7 +73,7 @@
    * @param rotationMatrix The rotation matrix.
    * @throws std::domain_error if the rotation matrix isn't special orthogonal.
    */
-  explicit Rotation3d(const Matrixd<3, 3>& rotationMatrix);
+  explicit Rotation3d(const Eigen::Matrix3d& rotationMatrix);
 
   /**
    * Constructs a Rotation3d that rotates the initial vector onto the final
@@ -77,7 +85,7 @@
    * @param initial The initial vector.
    * @param final The final vector.
    */
-  Rotation3d(const Vectord<3>& initial, const Vectord<3>& final);
+  Rotation3d(const Eigen::Vector3d& initial, const Eigen::Vector3d& final);
 
   /**
    * Adds two rotations together.
@@ -126,12 +134,16 @@
   /**
    * Checks equality between this Rotation3d and another object.
    */
-  bool operator==(const Rotation3d&) const = default;
+  bool operator==(const Rotation3d&) const;
 
   /**
-   * Adds the new rotation to the current rotation.
+   * Adds the new rotation to the current rotation. The other rotation is
+   * applied extrinsically, which means that it rotates around the global axes.
+   * For example, Rotation3d{90_deg, 0, 0}.RotateBy(Rotation3d{0, 45_deg, 0})
+   * rotates by 90 degrees around the +X axis and then by 45 degrees around the
+   * global +Y axis. (This is equivalent to Rotation3d{90_deg, 45_deg, 0})
    *
-   * @param other The rotation to rotate by.
+   * @param other The extrinsic rotation to rotate by.
    *
    * @return The new rotated Rotation3d.
    */
@@ -160,7 +172,7 @@
   /**
    * Returns the axis in the axis-angle representation of this rotation.
    */
-  Vectord<3> Axis() const;
+  Eigen::Vector3d Axis() const;
 
   /**
    * Returns the angle in the axis-angle representation of this rotation.
@@ -184,3 +196,31 @@
 void from_json(const wpi::json& json, Rotation3d& rotation);
 
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Rotation3d> {
+  static constexpr std::string_view kTypeString = "struct:Rotation3d";
+  static constexpr size_t kSize = wpi::Struct<frc::Quaternion>::kSize;
+  static constexpr std::string_view kSchema = "Quaternion q";
+  static frc::Rotation3d Unpack(std::span<const uint8_t, kSize> data) {
+    return frc::Rotation3d{wpi::UnpackStruct<frc::Quaternion, 0>(data)};
+  }
+  static void Pack(std::span<uint8_t, kSize> data,
+                   const frc::Rotation3d& value) {
+    wpi::PackStruct<0>(data, value.GetQuaternion());
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Quaternion>(fn);
+  }
+};
+
+static_assert(wpi::HasNestedStruct<frc::Rotation3d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Rotation3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Rotation3d& value);
+};
diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h
index 3c21ec1..593dce0 100644
--- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h
@@ -5,15 +5,17 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
-#include "Translation2d.h"
+#include "frc/geometry/Translation2d.h"
 
 namespace frc {
 
 class WPILIB_DLLEXPORT Pose2d;
 
 /**
- * Represents a transformation for a Pose2d.
+ * Represents a transformation for a Pose2d in the pose's frame.
  */
 class WPILIB_DLLEXPORT Transform2d {
  public:
@@ -34,6 +36,17 @@
   constexpr Transform2d(Translation2d translation, Rotation2d rotation);
 
   /**
+   * Constructs a transform with x and y translations instead of a separate
+   * Translation2d.
+   *
+   * @param x The x component of the translational component of the transform.
+   * @param y The y component of the translational component of the transform.
+   * @param rotation The rotational component of the transform.
+   */
+  constexpr Transform2d(units::meter_t x, units::meter_t y,
+                        Rotation2d rotation);
+
+  /**
    * Constructs the identity transform -- maps an initial pose to itself.
    */
   constexpr Transform2d() = default;
@@ -94,7 +107,8 @@
   }
 
   /**
-   * Composes two transformations.
+   * Composes two transformations. The second transform is applied relative to
+   * the orientation of the first.
    *
    * @param other The transform to compose with this one.
    * @return The composition of the two transformations.
@@ -112,4 +126,40 @@
 };
 }  // namespace frc
 
-#include "Transform2d.inc"
+template <>
+struct wpi::Struct<frc::Transform2d> {
+  static constexpr std::string_view kTypeString = "struct:Transform2d";
+  static constexpr size_t kSize = wpi::Struct<frc::Translation2d>::kSize +
+                                  wpi::Struct<frc::Rotation2d>::kSize;
+  static constexpr std::string_view kSchema =
+      "Translation2d translation;Rotation2d rotation";
+  static frc::Transform2d Unpack(std::span<const uint8_t, kSize> data) {
+    return {wpi::UnpackStruct<frc::Translation2d, 0>(data),
+            wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data)};
+  }
+  static void Pack(std::span<uint8_t, kSize> data,
+                   const frc::Transform2d& value) {
+    wpi::PackStruct<0>(data, value.Translation());
+    wpi::PackStruct<kRotationOff>(data, value.Rotation());
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation2d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation2d>(fn);
+  }
+
+ private:
+  static constexpr size_t kRotationOff = wpi::Struct<frc::Translation2d>::kSize;
+};
+
+static_assert(wpi::HasNestedStruct<frc::Transform2d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Transform2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Transform2d& value);
+};
+
+#include "frc/geometry/Transform2d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.inc b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc
index f851a05..862b8d5 100644
--- a/wpimath/src/main/native/include/frc/geometry/Transform2d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc
@@ -16,6 +16,10 @@
                                    Rotation2d rotation)
     : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
 
+constexpr Transform2d::Transform2d(units::meter_t x, units::meter_t y,
+                                   Rotation2d rotation)
+    : m_translation(x, y), m_rotation(std::move(rotation)) {}
+
 constexpr Transform2d Transform2d::Inverse() const {
   // We are rotating the difference between the translations
   // using a clockwise rotation matrix. This transforms the global
diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h
index 5f50ec2..d5af97d 100644
--- a/wpimath/src/main/native/include/frc/geometry/Transform3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h
@@ -5,15 +5,17 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
-#include "Translation3d.h"
+#include "frc/geometry/Translation3d.h"
 
 namespace frc {
 
 class WPILIB_DLLEXPORT Pose3d;
 
 /**
- * Represents a transformation for a Pose3d.
+ * Represents a transformation for a Pose3d in the pose's frame.
  */
 class WPILIB_DLLEXPORT Transform3d {
  public:
@@ -34,6 +36,18 @@
   Transform3d(Translation3d translation, Rotation3d rotation);
 
   /**
+   * Constructs a transform with x, y, and z translations instead of a separate
+   * Translation3d.
+   *
+   * @param x The x component of the translational component of the transform.
+   * @param y The y component of the translational component of the transform.
+   * @param z The z component of the translational component of the transform.
+   * @param rotation The rotational component of the transform.
+   */
+  Transform3d(units::meter_t x, units::meter_t y, units::meter_t z,
+              Rotation3d rotation);
+
+  /**
    * Constructs the identity transform -- maps an initial pose to itself.
    */
   constexpr Transform3d() = default;
@@ -99,7 +113,8 @@
   Transform3d operator/(double scalar) const { return *this * (1.0 / scalar); }
 
   /**
-   * Composes two transformations.
+   * Composes two transformations. The second transform is applied relative to
+   * the orientation of the first.
    *
    * @param other The transform to compose with this one.
    * @return The composition of the two transformations.
@@ -116,3 +131,39 @@
   Rotation3d m_rotation;
 };
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Transform3d> {
+  static constexpr std::string_view kTypeString = "struct:Transform3d";
+  static constexpr size_t kSize = wpi::Struct<frc::Translation3d>::kSize +
+                                  wpi::Struct<frc::Rotation3d>::kSize;
+  static constexpr std::string_view kSchema =
+      "Translation3d translation;Rotation3d rotation";
+  static frc::Transform3d Unpack(std::span<const uint8_t, kSize> data) {
+    return {wpi::UnpackStruct<frc::Translation3d, 0>(data),
+            wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data)};
+  }
+  static void Pack(std::span<uint8_t, kSize> data,
+                   const frc::Transform3d& value) {
+    wpi::PackStruct<0>(data, value.Translation());
+    wpi::PackStruct<kRotationOff>(data, value.Rotation());
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation3d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation3d>(fn);
+  }
+
+ private:
+  static constexpr size_t kRotationOff = wpi::Struct<frc::Translation3d>::kSize;
+};
+
+static_assert(wpi::HasNestedStruct<frc::Transform3d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Transform3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Transform3d& value);
+};
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
index e168510..1568586 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
@@ -4,15 +4,17 @@
 
 #pragma once
 
+#include <initializer_list>
+#include <span>
+
 #include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
-#include "Rotation2d.h"
+#include "frc/geometry/Rotation2d.h"
 #include "units/length.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace frc {
 
 /**
@@ -170,6 +172,21 @@
    */
   bool operator==(const Translation2d& other) const;
 
+  /**
+   * Returns the nearest Translation2d from a collection of translations
+   * @param translations The collection of translations.
+   * @return The nearest Translation2d from the collection.
+   */
+  Translation2d Nearest(std::span<const Translation2d> translations) const;
+
+  /**
+   * Returns the nearest Translation2d from a collection of translations
+   * @param translations The collection of translations.
+   * @return The nearest Translation2d from the collection.
+   */
+  Translation2d Nearest(
+      std::initializer_list<Translation2d> translations) const;
+
  private:
   units::meter_t m_x = 0_m;
   units::meter_t m_y = 0_m;
@@ -183,4 +200,28 @@
 
 }  // namespace frc
 
-#include "Translation2d.inc"
+template <>
+struct wpi::Struct<frc::Translation2d> {
+  static constexpr std::string_view kTypeString = "struct:Translation2d";
+  static constexpr size_t kSize = 16;
+  static constexpr std::string_view kSchema = "double x;double y";
+  static frc::Translation2d Unpack(std::span<const uint8_t, 16> data) {
+    return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 8>(data)}};
+  }
+  static void Pack(std::span<uint8_t, 16> data,
+                   const frc::Translation2d& value) {
+    wpi::PackStruct<0>(data, value.X().value());
+    wpi::PackStruct<8>(data, value.Y().value());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Translation2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Translation2d& value);
+};
+
+#include "frc/geometry/Translation2d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
index ab641fa..47ba1f6 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
@@ -5,15 +5,14 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
-#include "Rotation3d.h"
-#include "Translation2d.h"
+#include "frc/geometry/Rotation3d.h"
+#include "frc/geometry/Translation2d.h"
 #include "units/length.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace frc {
 
 /**
@@ -186,4 +185,30 @@
 
 }  // namespace frc
 
-#include "Translation3d.inc"
+template <>
+struct wpi::Struct<frc::Translation3d> {
+  static constexpr std::string_view kTypeString = "struct:Translation3d";
+  static constexpr size_t kSize = 24;
+  static constexpr std::string_view kSchema = "double x;double y;double z";
+  static frc::Translation3d Unpack(std::span<const uint8_t, 24> data) {
+    return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 8>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 16>(data)}};
+  }
+  static void Pack(std::span<uint8_t, 24> data,
+                   const frc::Translation3d& value) {
+    wpi::PackStruct<0>(data, value.X().value());
+    wpi::PackStruct<8>(data, value.Y().value());
+    wpi::PackStruct<16>(data, value.Z().value());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Translation3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Translation3d& value);
+};
+
+#include "frc/geometry/Translation3d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Twist2d.h b/wpimath/src/main/native/include/frc/geometry/Twist2d.h
index 620b688..257f1b6 100644
--- a/wpimath/src/main/native/include/frc/geometry/Twist2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Twist2d.h
@@ -5,6 +5,8 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 #include "units/angle.h"
 #include "units/length.h"
@@ -14,7 +16,7 @@
 /**
  * A change in distance along a 2D arc since the last pose update. We can use
  * ideas from differential calculus to create new Pose2ds from a Twist2d and
- * vise versa.
+ * vice versa.
  *
  * A Twist can be used to represent a difference between two poses.
  */
@@ -57,3 +59,28 @@
   }
 };
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Twist2d> {
+  static constexpr std::string_view kTypeString = "struct:Twist2d";
+  static constexpr size_t kSize = 24;
+  static constexpr std::string_view kSchema =
+      "double dx;double dy;double dtheta";
+  static frc::Twist2d Unpack(std::span<const uint8_t, 24> data) {
+    return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 8>(data)},
+            units::radian_t{wpi::UnpackStruct<double, 16>(data)}};
+  }
+  static void Pack(std::span<uint8_t, 24> data, const frc::Twist2d& value) {
+    wpi::PackStruct<0>(data, value.dx.value());
+    wpi::PackStruct<8>(data, value.dy.value());
+    wpi::PackStruct<16>(data, value.dtheta.value());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Twist2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Twist2d& value);
+};
diff --git a/wpimath/src/main/native/include/frc/geometry/Twist3d.h b/wpimath/src/main/native/include/frc/geometry/Twist3d.h
index 3040ab3..4d902df 100644
--- a/wpimath/src/main/native/include/frc/geometry/Twist3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Twist3d.h
@@ -5,6 +5,8 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Rotation3d.h"
 #include "units/angle.h"
@@ -15,7 +17,7 @@
 /**
  * A change in distance along a 3D arc since the last pose update. We can use
  * ideas from differential calculus to create new Pose3ds from a Twist3d and
- * vise versa.
+ * vice versa.
  *
  * A Twist can be used to represent a difference between two poses.
  */
@@ -77,3 +79,35 @@
   }
 };
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Twist3d> {
+  static constexpr std::string_view kTypeString = "struct:Twist3d";
+  static constexpr size_t kSize = 48;
+  static constexpr std::string_view kSchema =
+      "double dx;double dy;double dz;double rx;double ry;double rz";
+  static frc::Twist3d Unpack(std::span<const uint8_t, 48> data) {
+    return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 8>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 16>(data)},
+            units::radian_t{wpi::UnpackStruct<double, 24>(data)},
+            units::radian_t{wpi::UnpackStruct<double, 32>(data)},
+            units::radian_t{wpi::UnpackStruct<double, 40>(data)}};
+  }
+  static void Pack(std::span<uint8_t, 48> data, const frc::Twist3d& value) {
+    wpi::PackStruct<0>(data, value.dx.value());
+    wpi::PackStruct<8>(data, value.dy.value());
+    wpi::PackStruct<16>(data, value.dz.value());
+    wpi::PackStruct<24>(data, value.rx.value());
+    wpi::PackStruct<32>(data, value.ry.value());
+    wpi::PackStruct<40>(data, value.rz.value());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist3d> {
+  static constexpr std::string_view kTypeString = "proto:Twist3d";
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Twist3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Twist3d& value);
+};
diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
index 771fe84..9dd8e62 100644
--- a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
+++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
@@ -68,11 +68,26 @@
     if (m_pastSnapshots.size() == 0 || time > m_pastSnapshots.back().first) {
       m_pastSnapshots.emplace_back(time, sample);
     } else {
-      m_pastSnapshots.insert(
-          std::upper_bound(
-              m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
-              [](auto t, const auto& pair) { return t < pair.first; }),
-          std::pair(time, sample));
+      auto first_after = std::upper_bound(
+          m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
+          [](auto t, const auto& pair) { return t < pair.first; });
+
+      // Don't access this before ensuring first_after isn't first.
+      auto last_not_greater_than = first_after - 1;
+
+      if (first_after == m_pastSnapshots.begin() ||
+          last_not_greater_than == m_pastSnapshots.begin() ||
+          last_not_greater_than->first < time) {
+        // Two cases handled together:
+        // 1. All entries come after the sample
+        // 2. Some entries come before the sample, but none are recorded with
+        // the same time
+        m_pastSnapshots.insert(first_after, std::pair(time, sample));
+      } else {
+        // Final case:
+        // 3. An entry exists with the same recorded time.
+        last_not_greater_than->second = sample;
+      }
     }
     while (time - m_pastSnapshots[0].first > m_historySize) {
       m_pastSnapshots.erase(m_pastSnapshots.begin());
@@ -112,6 +127,10 @@
         m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
         [](const auto& pair, auto t) { return t > pair.first; });
 
+    if (upper_bound == m_pastSnapshots.begin()) {
+      return upper_bound->second;
+    }
+
     auto lower_bound = upper_bound - 1;
 
     double t = ((time - lower_bound->first) /
diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
index 37fe768..93c9044 100644
--- a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
+++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
@@ -6,6 +6,7 @@
 
 #include <wpi/SymbolExports.h>
 
+#include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
 #include "units/angular_velocity.h"
 #include "units/velocity.h"
@@ -15,8 +16,7 @@
  * Represents the speed of a robot chassis. Although this struct contains
  * similar members compared to a Twist2d, they do NOT represent the same thing.
  * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of
- * reference, this ChassisSpeeds struct represents a velocity w.r.t to the robot
- * frame of reference.
+ * reference, a ChassisSpeeds struct represents a robot's velocity.
  *
  * A strictly non-holonomic drivetrain, such as a differential drive, should
  * never have a dy component because it can never move sideways. Holonomic
@@ -24,12 +24,12 @@
  */
 struct WPILIB_DLLEXPORT ChassisSpeeds {
   /**
-   * Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
+   * Velocity along the x-axis. (Fwd is +)
    */
   units::meters_per_second_t vx = 0_mps;
 
   /**
-   * Represents strafe velocity w.r.t the robot frame of reference. (Left is +)
+   * Velocity along the y-axis. (Left is +)
    */
   units::meters_per_second_t vy = 0_mps;
 
@@ -39,6 +39,57 @@
   units::radians_per_second_t omega = 0_rad_per_s;
 
   /**
+   * Disretizes a continuous-time chassis speed.
+   *
+   * This function converts a continuous-time chassis speed into a discrete-time
+   * one such that when the discrete-time chassis speed is applied for one
+   * timestep, the robot moves as if the velocity components are independent
+   * (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the
+   * y-axis, and omega * dt around the z-axis).
+   *
+   * This is useful for compensating for translational skew when translating and
+   * rotating a swerve drivetrain.
+   *
+   * @param vx Forward velocity.
+   * @param vy Sideways velocity.
+   * @param omega Angular velocity.
+   * @param dt The duration of the timestep the speeds should be applied for.
+   *
+   * @return Discretized ChassisSpeeds.
+   */
+  static ChassisSpeeds Discretize(units::meters_per_second_t vx,
+                                  units::meters_per_second_t vy,
+                                  units::radians_per_second_t omega,
+                                  units::second_t dt) {
+    Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt};
+    auto twist = Pose2d{}.Log(desiredDeltaPose);
+    return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt};
+  }
+
+  /**
+   * Disretizes a continuous-time chassis speed.
+   *
+   * This function converts a continuous-time chassis speed into a discrete-time
+   * one such that when the discrete-time chassis speed is applied for one
+   * timestep, the robot moves as if the velocity components are independent
+   * (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the
+   * y-axis, and omega * dt around the z-axis).
+   *
+   * This is useful for compensating for translational skew when translating and
+   * rotating a swerve drivetrain.
+   *
+   * @param continuousSpeeds The continuous speeds.
+   * @param dt The duration of the timestep the speeds should be applied for.
+   *
+   * @return Discretized ChassisSpeeds.
+   */
+  static ChassisSpeeds Discretize(const ChassisSpeeds& continuousSpeeds,
+                                  units::second_t dt) {
+    return Discretize(continuousSpeeds.vx, continuousSpeeds.vy,
+                      continuousSpeeds.omega, dt);
+  }
+
+  /**
    * Converts a user provided field-relative set of speeds into a robot-relative
    * ChassisSpeeds object.
    *
@@ -57,8 +108,12 @@
   static ChassisSpeeds FromFieldRelativeSpeeds(
       units::meters_per_second_t vx, units::meters_per_second_t vy,
       units::radians_per_second_t omega, const Rotation2d& robotAngle) {
-    return {vx * robotAngle.Cos() + vy * robotAngle.Sin(),
-            -vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega};
+    // CW rotation into chassis frame
+    auto rotated =
+        Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
+            .RotateBy(-robotAngle);
+    return {units::meters_per_second_t{rotated.X().value()},
+            units::meters_per_second_t{rotated.Y().value()}, omega};
   }
 
   /**
@@ -81,5 +136,118 @@
                                    fieldRelativeSpeeds.vy,
                                    fieldRelativeSpeeds.omega, robotAngle);
   }
+
+  /**
+   * Converts a user provided robot-relative set of speeds into a field-relative
+   * ChassisSpeeds object.
+   *
+   * @param vx The component of speed in the x direction relative to the robot.
+   * Positive x is towards the robot's front.
+   * @param vy The component of speed in the y direction relative to the robot.
+   * Positive y is towards the robot's left.
+   * @param omega The angular rate of the robot.
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The
+   * robot's angle is considered to be zero when it is facing directly away from
+   * your alliance station wall. Remember that this should be CCW positive.
+   *
+   * @return ChassisSpeeds object representing the speeds in the field's frame
+   * of reference.
+   */
+  static ChassisSpeeds FromRobotRelativeSpeeds(
+      units::meters_per_second_t vx, units::meters_per_second_t vy,
+      units::radians_per_second_t omega, const Rotation2d& robotAngle) {
+    // CCW rotation out of chassis frame
+    auto rotated =
+        Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
+            .RotateBy(robotAngle);
+    return {units::meters_per_second_t{rotated.X().value()},
+            units::meters_per_second_t{rotated.Y().value()}, omega};
+  }
+
+  /**
+   * Converts a user provided robot-relative ChassisSpeeds object into a
+   * field-relative ChassisSpeeds object.
+   *
+   * @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds
+   *    in the robot frame of reference. Positive x is the towards robot's
+   * front. Positive y is towards the robot's left.
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The
+   *    robot's angle is considered to be zero when it is facing directly away
+   *    from your alliance station wall. Remember that this should be CCW
+   *    positive.
+   * @return ChassisSpeeds object representing the speeds in the field's frame
+   *    of reference.
+   */
+  static ChassisSpeeds FromRobotRelativeSpeeds(
+      const ChassisSpeeds& robotRelativeSpeeds, const Rotation2d& robotAngle) {
+    return FromRobotRelativeSpeeds(robotRelativeSpeeds.vx,
+                                   robotRelativeSpeeds.vy,
+                                   robotRelativeSpeeds.omega, robotAngle);
+  }
+
+  /**
+   * Adds two ChassisSpeeds and returns the sum.
+   *
+   * <p>For example, ChassisSpeeds{1.0, 0.5, 1.5} + ChassisSpeeds{2.0, 1.5, 0.5}
+   * = ChassisSpeeds{3.0, 2.0, 2.0}
+   *
+   * @param other The ChassisSpeeds to add.
+   *
+   * @return The sum of the ChassisSpeeds.
+   */
+  constexpr ChassisSpeeds operator+(const ChassisSpeeds& other) const {
+    return {vx + other.vx, vy + other.vy, omega + other.omega};
+  }
+
+  /**
+   * Subtracts the other ChassisSpeeds from the current ChassisSpeeds and
+   * returns the difference.
+   *
+   * <p>For example, ChassisSpeeds{5.0, 4.0, 2.0} - ChassisSpeeds{1.0, 2.0, 1.0}
+   * = ChassisSpeeds{4.0, 2.0, 1.0}
+   *
+   * @param other The ChassisSpeeds to subtract.
+   *
+   * @return The difference between the two ChassisSpeeds.
+   */
+  constexpr ChassisSpeeds operator-(const ChassisSpeeds& other) const {
+    return *this + -other;
+  }
+
+  /**
+   * Returns the inverse of the current ChassisSpeeds.
+   * This is equivalent to negating all components of the ChassisSpeeds.
+   *
+   * @return The inverse of the current ChassisSpeeds.
+   */
+  constexpr ChassisSpeeds operator-() const { return {-vx, -vy, -omega}; }
+
+  /**
+   * Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
+   *
+   * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} * 2
+   * = ChassisSpeeds{4.0, 5.0, 1.0}
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled ChassisSpeeds.
+   */
+  constexpr ChassisSpeeds operator*(double scalar) const {
+    return {scalar * vx, scalar * vy, scalar * omega};
+  }
+
+  /**
+   * Divides the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
+   *
+   * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} / 2
+   * = ChassisSpeeds{1.0, 1.25, 0.5}
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled ChassisSpeeds.
+   */
+  constexpr ChassisSpeeds operator/(double scalar) const {
+    return operator*(1.0 / scalar);
+  }
 };
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
index 930c7a6..95f53fa 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
@@ -8,7 +8,9 @@
 
 #include "frc/geometry/Twist2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/DifferentialDriveWheelPositions.h"
 #include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+#include "frc/kinematics/Kinematics.h"
 #include "units/angle.h"
 #include "units/length.h"
 #include "wpimath/MathShared.h"
@@ -22,7 +24,9 @@
  * velocity components whereas forward kinematics converts left and right
  * component velocities into a linear and angular chassis speed.
  */
-class WPILIB_DLLEXPORT DifferentialDriveKinematics {
+class WPILIB_DLLEXPORT DifferentialDriveKinematics
+    : public Kinematics<DifferentialDriveWheelSpeeds,
+                        DifferentialDriveWheelPositions> {
  public:
   /**
    * Constructs a differential drive kinematics object.
@@ -46,7 +50,7 @@
    * @return The chassis speed.
    */
   constexpr ChassisSpeeds ToChassisSpeeds(
-      const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
+      const DifferentialDriveWheelSpeeds& wheelSpeeds) const override {
     return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
             (wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad};
   }
@@ -60,7 +64,7 @@
    * @return The left and right velocities.
    */
   constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
-      const ChassisSpeeds& chassisSpeeds) const {
+      const ChassisSpeeds& chassisSpeeds) const override {
     return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad,
             chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
   }
@@ -79,6 +83,11 @@
             (rightDistance - leftDistance) / trackWidth * 1_rad};
   }
 
+  Twist2d ToTwist2d(const DifferentialDriveWheelPositions& start,
+                    const DifferentialDriveWheelPositions& end) const override {
+    return ToTwist2d(end.left - start.left, end.right - start.right);
+  }
+
   units::meter_t trackWidth;
 };
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
index cc198ac..279c1dc 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
@@ -7,6 +7,10 @@
 #include <wpi/SymbolExports.h>
 
 #include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/DifferentialDriveWheelPositions.h"
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+#include "frc/kinematics/Odometry.h"
 #include "units/length.h"
 
 namespace frc {
@@ -22,7 +26,9 @@
  * It is important that you reset your encoders to zero before using this class.
  * Any subsequent pose resets also require the encoders to be reset to zero.
  */
-class WPILIB_DLLEXPORT DifferentialDriveOdometry {
+class WPILIB_DLLEXPORT DifferentialDriveOdometry
+    : public Odometry<DifferentialDriveWheelSpeeds,
+                      DifferentialDriveWheelPositions> {
  public:
   /**
    * Constructs a DifferentialDriveOdometry object.
@@ -56,21 +62,14 @@
    */
   void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
                      units::meter_t rightDistance, const Pose2d& pose) {
-    m_pose = pose;
-    m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
-
-    m_prevLeftDistance = leftDistance;
-    m_prevRightDistance = rightDistance;
+    Odometry<DifferentialDriveWheelSpeeds,
+             DifferentialDriveWheelPositions>::ResetPosition(gyroAngle,
+                                                             {leftDistance,
+                                                              rightDistance},
+                                                             pose);
   }
 
   /**
-   * Returns the position of the robot on the field.
-   * @return The pose of the robot.
-   */
-  const Pose2d& GetPose() const { return m_pose; }
-
-  /**
    * Updates the robot position on the field using distance measurements from
    * encoders. This method is more numerically accurate than using velocities to
    * integrate the pose and is also advantageous for teams that are using lower
@@ -82,15 +81,14 @@
    * @return The new pose of the robot.
    */
   const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
-                       units::meter_t rightDistance);
+                       units::meter_t rightDistance) {
+    return Odometry<DifferentialDriveWheelSpeeds,
+                    DifferentialDriveWheelPositions>::Update(gyroAngle,
+                                                             {leftDistance,
+                                                              rightDistance});
+  }
 
  private:
-  Pose2d m_pose;
-
-  Rotation2d m_gyroOffset;
-  Rotation2d m_previousAngle;
-
-  units::meter_t m_prevLeftDistance = 0_m;
-  units::meter_t m_prevRightDistance = 0_m;
+  DifferentialDriveKinematics m_kinematicsImpl{units::meter_t{1}};
 };
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h
new file mode 100644
index 0000000..4dddbea
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h
@@ -0,0 +1,51 @@
+// 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 <wpi/MathExtras.h>
+#include <wpi/SymbolExports.h>
+
+#include "units/length.h"
+
+namespace frc {
+/**
+ * Represents the wheel positions for a differential drive drivetrain.
+ */
+struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions {
+  /**
+   * Distance driven by the left side.
+   */
+  units::meter_t left = 0_m;
+
+  /**
+   * Distance driven by the right side.
+   */
+  units::meter_t right = 0_m;
+
+  /**
+   * Checks equality between this DifferentialDriveWheelPositions and another
+   * object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const DifferentialDriveWheelPositions& other) const = default;
+
+  /**
+   * Checks inequality between this DifferentialDriveWheelPositions and another
+   * object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const DifferentialDriveWheelPositions& other) const = default;
+
+  DifferentialDriveWheelPositions Interpolate(
+      const DifferentialDriveWheelPositions& endValue, double t) const {
+    return {wpi::Lerp(left, endValue.left, t),
+            wpi::Lerp(right, endValue.right, t)};
+  }
+};
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
index fce2b96..9d9e705 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
@@ -36,5 +36,79 @@
    * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
    */
   void Desaturate(units::meters_per_second_t attainableMaxSpeed);
+
+  /**
+   * Adds two DifferentialDriveWheelSpeeds and returns the sum.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{1.0, 0.5} +
+   * DifferentialDriveWheelSpeeds{2.0, 1.5} =
+   * DifferentialDriveWheelSpeeds{3.0, 2.0}
+   *
+   * @param other The DifferentialDriveWheelSpeeds to add.
+   *
+   * @return The sum of the DifferentialDriveWheelSpeeds.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator+(
+      const DifferentialDriveWheelSpeeds& other) const {
+    return {left + other.left, right + other.right};
+  }
+
+  /**
+   * Subtracts the other DifferentialDriveWheelSpeeds from the current
+   * DifferentialDriveWheelSpeeds and returns the difference.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{5.0, 4.0} -
+   * DifferentialDriveWheelSpeeds{1.0, 2.0} =
+   * DifferentialDriveWheelSpeeds{4.0, 2.0}
+   *
+   * @param other The DifferentialDriveWheelSpeeds to subtract.
+   *
+   * @return The difference between the two DifferentialDriveWheelSpeeds.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator-(
+      const DifferentialDriveWheelSpeeds& other) const {
+    return *this + -other;
+  }
+
+  /**
+   * Returns the inverse of the current DifferentialDriveWheelSpeeds.
+   * This is equivalent to negating all components of the
+   * DifferentialDriveWheelSpeeds.
+   *
+   * @return The inverse of the current DifferentialDriveWheelSpeeds.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator-() const {
+    return {-left, -right};
+  }
+
+  /**
+   * Multiplies the DifferentialDriveWheelSpeeds by a scalar and returns the new
+   * DifferentialDriveWheelSpeeds.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} * 2
+   * = DifferentialDriveWheelSpeeds{4.0, 5.0}
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled DifferentialDriveWheelSpeeds.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator*(double scalar) const {
+    return {scalar * left, scalar * right};
+  }
+
+  /**
+   * Divides the DifferentialDriveWheelSpeeds by a scalar and returns the new
+   * DifferentialDriveWheelSpeeds.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} / 2
+   * = DifferentialDriveWheelSpeeds{1.0, 1.25}
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled DifferentialDriveWheelSpeeds.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator/(double scalar) const {
+    return operator*(1.0 / scalar);
+  }
 };
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/Kinematics.h b/wpimath/src/main/native/include/frc/kinematics/Kinematics.h
new file mode 100644
index 0000000..e27cfc6
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/Kinematics.h
@@ -0,0 +1,62 @@
+// 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 <wpi/SymbolExports.h>
+
+#include "frc/geometry/Twist2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+
+namespace frc {
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
+ * into individual wheel speeds. Robot code should not use this directly-
+ * Instead, use the particular type for your drivetrain (e.g.,
+ * DifferentialDriveKinematics).
+ *
+ * Inverse kinematics converts a desired chassis speed into wheel speeds whereas
+ * forward kinematics converts wheel speeds into chassis speed.
+ */
+template <typename WheelSpeeds, typename WheelPositions>
+class WPILIB_DLLEXPORT Kinematics {
+ public:
+  /**
+   * Performs forward kinematics to return the resulting chassis speed from the
+   * wheel speeds. This method is often used for odometry -- determining the
+   * robot's position on the field using data from the real-world speed of each
+   * wheel on the robot.
+   *
+   * @param wheelSpeeds The speeds of the wheels.
+   * @return The chassis speed.
+   */
+  virtual ChassisSpeeds ToChassisSpeeds(
+      const WheelSpeeds& wheelSpeeds) const = 0;
+
+  /**
+   * Performs inverse kinematics to return the wheel speeds from a desired
+   * chassis velocity. This method is often used to convert joystick values into
+   * wheel speeds.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @return The wheel speeds.
+   */
+  virtual WheelSpeeds ToWheelSpeeds(
+      const ChassisSpeeds& chassisSpeeds) const = 0;
+
+  /**
+   * Performs forward kinematics to return the resulting Twist2d from the given
+   * change in wheel positions. This method is often used for odometry --
+   * determining the robot's position on the field using changes in the distance
+   * driven by each wheel on the robot.
+   *
+   * @param start The starting distances driven by the wheels.
+   * @param end The ending distances driven by the wheels.
+   *
+   * @return The resulting Twist2d in the robot's movement.
+   */
+  virtual Twist2d ToTwist2d(const WheelPositions& start,
+                            const WheelPositions& end) const = 0;
+};
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
index 2880cef..fe6eb51 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
@@ -4,13 +4,14 @@
 
 #pragma once
 
+#include <Eigen/QR>
 #include <wpi/SymbolExports.h>
 
-#include "Eigen/QR"
 #include "frc/EigenCore.h"
 #include "frc/geometry/Translation2d.h"
 #include "frc/geometry/Twist2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/Kinematics.h"
 #include "frc/kinematics/MecanumDriveWheelPositions.h"
 #include "frc/kinematics/MecanumDriveWheelSpeeds.h"
 #include "wpimath/MathShared.h"
@@ -39,7 +40,8 @@
  * Forward kinematics is also used for odometry -- determining the position of
  * the robot on the field using encoders and a gyro.
  */
-class WPILIB_DLLEXPORT MecanumDriveKinematics {
+class WPILIB_DLLEXPORT MecanumDriveKinematics
+    : public Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
  public:
   /**
    * Constructs a mecanum drive kinematics object.
@@ -101,7 +103,12 @@
    */
   MecanumDriveWheelSpeeds ToWheelSpeeds(
       const ChassisSpeeds& chassisSpeeds,
-      const Translation2d& centerOfRotation = Translation2d{}) const;
+      const Translation2d& centerOfRotation) const;
+
+  MecanumDriveWheelSpeeds ToWheelSpeeds(
+      const ChassisSpeeds& chassisSpeeds) const override {
+    return ToWheelSpeeds(chassisSpeeds, {});
+  }
 
   /**
    * Performs forward kinematics to return the resulting chassis state from the
@@ -114,7 +121,10 @@
    * @return The resulting chassis speed.
    */
   ChassisSpeeds ToChassisSpeeds(
-      const MecanumDriveWheelSpeeds& wheelSpeeds) const;
+      const MecanumDriveWheelSpeeds& wheelSpeeds) const override;
+
+  Twist2d ToTwist2d(const MecanumDriveWheelPositions& start,
+                    const MecanumDriveWheelPositions& end) const override;
 
   /**
    * Performs forward kinematics to return the resulting Twist2d from the
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
index 5e949ca..0a6f537 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
@@ -9,7 +9,9 @@
 
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveWheelPositions.h"
 #include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+#include "frc/kinematics/Odometry.h"
 #include "units/time.h"
 
 namespace frc {
@@ -23,7 +25,8 @@
  * path following. Furthermore, odometry can be used for latency compensation
  * when using computer-vision systems.
  */
-class WPILIB_DLLEXPORT MecanumDriveOdometry {
+class WPILIB_DLLEXPORT MecanumDriveOdometry
+    : public Odometry<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
  public:
   /**
    * Constructs a MecanumDriveOdometry object.
@@ -38,52 +41,8 @@
       const MecanumDriveWheelPositions& wheelPositions,
       const Pose2d& initialPose = Pose2d{});
 
-  /**
-   * Resets the robot's position on the field.
-   *
-   * The gyroscope angle does not need to be reset here on the user's robot
-   * code. The library automatically takes care of offsetting the gyro angle.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelPositions The current distances measured by each wheel.
-   * @param pose The position on the field that your robot is at.
-   */
-  void ResetPosition(const Rotation2d& gyroAngle,
-                     const MecanumDriveWheelPositions& wheelPositions,
-                     const Pose2d& pose) {
-    m_pose = pose;
-    m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
-    m_previousWheelPositions = wheelPositions;
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   * @return The pose of the robot.
-   */
-  const Pose2d& GetPose() const { return m_pose; }
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method takes in an angle parameter
-   * which is used instead of the angular rate that is calculated from forward
-   * kinematics, in addition to the current distance measurement at each wheel.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelPositions The current distances measured by each wheel.
-   *
-   * @return The new pose of the robot.
-   */
-  const Pose2d& Update(const Rotation2d& gyroAngle,
-                       const MecanumDriveWheelPositions& wheelPositions);
-
  private:
-  MecanumDriveKinematics m_kinematics;
-  Pose2d m_pose;
-
-  MecanumDriveWheelPositions m_previousWheelPositions;
-  Rotation2d m_previousAngle;
-  Rotation2d m_gyroOffset;
+  MecanumDriveKinematics m_kinematicsImpl;
 };
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
index b69aceb..76c8b9d 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
@@ -4,13 +4,14 @@
 
 #pragma once
 
+#include <wpi/MathExtras.h>
 #include <wpi/SymbolExports.h>
 
 #include "units/length.h"
 
 namespace frc {
 /**
- * Represents the wheel speeds for a mecanum drive drivetrain.
+ * Represents the wheel positions for a mecanum drive drivetrain.
  */
 struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
   /**
@@ -49,5 +50,13 @@
    * @return Whether the two objects are not equal.
    */
   bool operator!=(const MecanumDriveWheelPositions& other) const = default;
+
+  MecanumDriveWheelPositions Interpolate(
+      const MecanumDriveWheelPositions& endValue, double t) const {
+    return {wpi::Lerp(frontLeft, endValue.frontLeft, t),
+            wpi::Lerp(frontRight, endValue.frontRight, t),
+            wpi::Lerp(rearLeft, endValue.rearLeft, t),
+            wpi::Lerp(rearRight, endValue.rearRight, t)};
+  }
 };
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
index e698c5f..80e8460 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
@@ -46,5 +46,77 @@
    * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
    */
   void Desaturate(units::meters_per_second_t attainableMaxSpeed);
+
+  /**
+   * Adds two MecanumDriveWheelSpeeds and returns the sum.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{1.0, 0.5, 2.0, 1.5} +
+   * MecanumDriveWheelSpeeds{2.0, 1.5, 0.5, 1.0} =
+   * MecanumDriveWheelSpeeds{3.0, 2.0, 2.5, 2.5}
+   *
+   * @param other The MecanumDriveWheelSpeeds to add.
+   * @return The sum of the MecanumDriveWheelSpeeds.
+   */
+  constexpr MecanumDriveWheelSpeeds operator+(
+      const MecanumDriveWheelSpeeds& other) const {
+    return {frontLeft + other.frontLeft, frontRight + other.frontRight,
+            rearLeft + other.rearLeft, rearRight + other.rearRight};
+  }
+
+  /**
+   * Subtracts the other MecanumDriveWheelSpeeds from the current
+   * MecanumDriveWheelSpeeds and returns the difference.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{5.0, 4.0, 6.0, 2.5} -
+   * MecanumDriveWheelSpeeds{1.0, 2.0, 3.0, 0.5} =
+   * MecanumDriveWheelSpeeds{4.0, 2.0, 3.0, 2.0}
+   *
+   * @param other The MecanumDriveWheelSpeeds to subtract.
+   * @return The difference between the two MecanumDriveWheelSpeeds.
+   */
+  constexpr MecanumDriveWheelSpeeds operator-(
+      const MecanumDriveWheelSpeeds& other) const {
+    return *this + -other;
+  }
+
+  /**
+   * Returns the inverse of the current MecanumDriveWheelSpeeds.
+   * This is equivalent to negating all components of the
+   * MecanumDriveWheelSpeeds.
+   *
+   * @return The inverse of the current MecanumDriveWheelSpeeds.
+   */
+  constexpr MecanumDriveWheelSpeeds operator-() const {
+    return {-frontLeft, -frontRight, -rearLeft, -rearRight};
+  }
+
+  /**
+   * Multiplies the MecanumDriveWheelSpeeds by a scalar and returns the new
+   * MecanumDriveWheelSpeeds.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 3.0, 3.5} * 2 =
+   * MecanumDriveWheelSpeeds{4.0, 5.0, 6.0, 7.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled MecanumDriveWheelSpeeds.
+   */
+  constexpr MecanumDriveWheelSpeeds operator*(double scalar) const {
+    return {scalar * frontLeft, scalar * frontRight, scalar * rearLeft,
+            scalar * rearRight};
+  }
+
+  /**
+   * Divides the MecanumDriveWheelSpeeds by a scalar and returns the new
+   * MecanumDriveWheelSpeeds.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 1.5, 1.0} / 2 =
+   * MecanumDriveWheelSpeeds{1.0, 1.25, 0.75, 0.5}
+   *
+   * @param scalar The scalar to divide by.
+   * @return The scaled MecanumDriveWheelSpeeds.
+   */
+  constexpr MecanumDriveWheelSpeeds operator/(double scalar) const {
+    return operator*(1.0 / scalar);
+  }
 };
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry.h b/wpimath/src/main/native/include/frc/kinematics/Odometry.h
new file mode 100644
index 0000000..55c074c
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/Odometry.h
@@ -0,0 +1,88 @@
+// 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 <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/Kinematics.h"
+#include "frc/kinematics/WheelPositions.h"
+
+namespace frc {
+/**
+ * Class for odometry. Robot code should not use this directly- Instead, use the
+ * particular type for your drivetrain (e.g., DifferentialDriveOdometry).
+ * Odometry allows you to track the robot's position on the field over a course
+ * of a match using readings from your wheel encoders.
+ *
+ * Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ */
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+class WPILIB_DLLEXPORT Odometry {
+ public:
+  /**
+   * Constructs an Odometry object.
+   *
+   * @param kinematics The kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current distances measured by each wheel.
+   * @param initialPose The starting position of the robot on the field.
+   */
+  explicit Odometry(const Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+                    const Rotation2d& gyroAngle,
+                    const WheelPositions& wheelPositions,
+                    const Pose2d& initialPose = Pose2d{});
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * The gyroscope angle does not need to be reset here on the user's robot
+   * code. The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current distances measured by each wheel.
+   * @param pose The position on the field that your robot is at.
+   */
+  void ResetPosition(const Rotation2d& gyroAngle,
+                     const WheelPositions& wheelPositions, const Pose2d& pose) {
+    m_pose = pose;
+    m_previousAngle = pose.Rotation();
+    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    m_previousWheelPositions = wheelPositions;
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   * @return The pose of the robot.
+   */
+  const Pose2d& GetPose() const { return m_pose; }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method takes in an angle parameter
+   * which is used instead of the angular rate that is calculated from forward
+   * kinematics, in addition to the current distance measurement at each wheel.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current distances measured by each wheel.
+   *
+   * @return The new pose of the robot.
+   */
+  const Pose2d& Update(const Rotation2d& gyroAngle,
+                       const WheelPositions& wheelPositions);
+
+ private:
+  const Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
+  Pose2d m_pose;
+
+  WheelPositions m_previousWheelPositions;
+  Rotation2d m_previousAngle;
+  Rotation2d m_gyroOffset;
+};
+}  // namespace frc
+
+#include "Odometry.inc"
diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry.inc b/wpimath/src/main/native/include/frc/kinematics/Odometry.inc
new file mode 100644
index 0000000..688c1bd
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/Odometry.inc
@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/kinematics/Odometry.h"
+
+namespace frc {
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+Odometry<WheelSpeeds, WheelPositions>::Odometry(
+    const Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+    const Rotation2d& gyroAngle, const WheelPositions& wheelPositions,
+    const Pose2d& initialPose)
+    : m_kinematics(kinematics),
+      m_pose(initialPose),
+      m_previousWheelPositions(wheelPositions) {
+  m_previousAngle = m_pose.Rotation();
+  m_gyroOffset = m_pose.Rotation() - gyroAngle;
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+const Pose2d& Odometry<WheelSpeeds, WheelPositions>::Update(
+    const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) {
+  auto angle = gyroAngle + m_gyroOffset;
+
+  auto twist = m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);
+  twist.dtheta = (angle - m_previousAngle).Radians();
+
+  auto newPose = m_pose.Exp(twist);
+
+  m_previousAngle = angle;
+  m_previousWheelPositions = wheelPositions;
+  m_pose = {newPose.Translation(), angle};
+
+  return m_pose;
+}
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
index 97ee233..f6d4e5d 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
@@ -4,23 +4,30 @@
 
 #pragma once
 
+#include <concepts>
 #include <cstddef>
 
+#include <Eigen/QR>
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "Eigen/QR"
 #include "frc/EigenCore.h"
 #include "frc/geometry/Rotation2d.h"
 #include "frc/geometry/Translation2d.h"
 #include "frc/geometry/Twist2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/Kinematics.h"
+#include "frc/kinematics/SwerveDriveWheelPositions.h"
 #include "frc/kinematics/SwerveModulePosition.h"
 #include "frc/kinematics/SwerveModuleState.h"
 #include "units/velocity.h"
 #include "wpimath/MathShared.h"
 
 namespace frc {
+
+template <size_t NumModules>
+using SwerveDriveWheelSpeeds = wpi::array<SwerveModuleState, NumModules>;
+
 /**
  * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
  * into individual module states (speed and angle).
@@ -44,27 +51,25 @@
  * the robot on the field using encoders and a gyro.
  */
 template <size_t NumModules>
-class SwerveDriveKinematics {
+class SwerveDriveKinematics
+    : public Kinematics<SwerveDriveWheelSpeeds<NumModules>,
+                        SwerveDriveWheelPositions<NumModules>> {
  public:
   /**
    * Constructs a swerve drive kinematics object. This takes in a variable
-   * number of wheel locations as Translation2ds. The order in which you pass in
-   * the wheel locations is the same order that you will receive the module
+   * number of module locations as Translation2ds. The order in which you pass
+   * in the module locations is the same order that you will receive the module
    * states when performing inverse kinematics. It is also expected that you
    * pass in the module states in the same order when calling the forward
    * kinematics methods.
    *
-   * @param wheel  The location of the first wheel relative to the physical
-   *               center of the robot.
-   * @param wheels The locations of the other wheels relative to the physical
-   *               center of the robot.
+   * @param moduleTranslations The locations of the modules relative to the
+   *                           physical center of the robot.
    */
-  template <typename... Wheels>
-  explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels)
-      : m_modules{wheel, wheels...}, m_moduleStates(wpi::empty_array) {
-    static_assert(sizeof...(wheels) >= 1,
-                  "A swerve drive requires at least two modules");
-
+  template <std::convertible_to<Translation2d>... ModuleTranslations>
+    requires(sizeof...(ModuleTranslations) == NumModules)
+  explicit SwerveDriveKinematics(ModuleTranslations&&... moduleTranslations)
+      : m_modules{moduleTranslations...}, m_moduleHeadings(wpi::empty_array) {
     for (size_t i = 0; i < NumModules; i++) {
       // clang-format off
       m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
@@ -80,8 +85,8 @@
   }
 
   explicit SwerveDriveKinematics(
-      const wpi::array<Translation2d, NumModules>& wheels)
-      : m_modules{wheels}, m_moduleStates(wpi::empty_array) {
+      const wpi::array<Translation2d, NumModules>& modules)
+      : m_modules{modules}, m_moduleHeadings(wpi::empty_array) {
     for (size_t i = 0; i < NumModules; i++) {
       // clang-format off
       m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
@@ -99,6 +104,25 @@
   SwerveDriveKinematics(const SwerveDriveKinematics&) = default;
 
   /**
+   * Reset the internal swerve module headings.
+   * @param moduleHeadings The swerve module headings. The order of the module
+   * headings should be same as passed into the constructor of this class.
+   */
+  template <std::convertible_to<Rotation2d>... ModuleHeadings>
+    requires(sizeof...(ModuleHeadings) == NumModules)
+  void ResetHeadings(ModuleHeadings&&... moduleHeadings) {
+    return this->ResetHeadings(
+        wpi::array<Rotation2d, NumModules>{moduleHeadings...});
+  }
+
+  /**
+   * Reset the internal swerve module headings.
+   * @param moduleHeadings The swerve module headings. The order of the module
+   * headings should be same as passed into the constructor of this class.
+   */
+  void ResetHeadings(wpi::array<Rotation2d, NumModules> moduleHeadings);
+
+  /**
    * Performs inverse kinematics to return the module states from a desired
    * chassis velocity. This method is often used to convert joystick values into
    * module speeds and angles.
@@ -133,20 +157,29 @@
       const ChassisSpeeds& chassisSpeeds,
       const Translation2d& centerOfRotation = Translation2d{}) const;
 
+  SwerveDriveWheelSpeeds<NumModules> ToWheelSpeeds(
+      const ChassisSpeeds& chassisSpeeds) const override {
+    return ToSwerveModuleStates(chassisSpeeds);
+  }
+
   /**
    * Performs forward kinematics to return the resulting chassis state from the
    * given module states. This method is often used for odometry -- determining
    * the robot's position on the field using data from the real-world speed and
    * angle of each module on the robot.
    *
-   * @param wheelStates The state of the modules (as a SwerveModuleState type)
+   * @param moduleStates The state of the modules (as a SwerveModuleState type)
    * as measured from respective encoders and gyros. The order of the swerve
    * module states should be same as passed into the constructor of this class.
    *
    * @return The resulting chassis speed.
    */
-  template <typename... ModuleStates>
-  ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates) const;
+  template <std::convertible_to<SwerveModuleState>... ModuleStates>
+    requires(sizeof...(ModuleStates) == NumModules)
+  ChassisSpeeds ToChassisSpeeds(ModuleStates&&... moduleStates) const {
+    return this->ToChassisSpeeds(
+        wpi::array<SwerveModuleState, NumModules>{moduleStates...});
+  }
 
   /**
    * Performs forward kinematics to return the resulting chassis state from the
@@ -161,8 +194,8 @@
    *
    * @return The resulting chassis speed.
    */
-  ChassisSpeeds ToChassisSpeeds(
-      wpi::array<SwerveModuleState, NumModules> moduleStates) const;
+  ChassisSpeeds ToChassisSpeeds(const wpi::array<SwerveModuleState, NumModules>&
+                                    moduleStates) const override;
 
   /**
    * Performs forward kinematics to return the resulting Twist2d from the
@@ -170,15 +203,19 @@
    * determining the robot's position on the field using data from the
    * real-world position delta and angle of each module on the robot.
    *
-   * @param wheelDeltas The latest change in position of the modules (as a
+   * @param moduleDeltas The latest change in position of the modules (as a
    * SwerveModulePosition type) as measured from respective encoders and gyros.
    * The order of the swerve module states should be same as passed into the
    * constructor of this class.
    *
    * @return The resulting Twist2d.
    */
-  template <typename... ModuleDeltas>
-  Twist2d ToTwist2d(ModuleDeltas&&... wheelDeltas) const;
+  template <std::convertible_to<SwerveModulePosition>... ModuleDeltas>
+    requires(sizeof...(ModuleDeltas) == NumModules)
+  Twist2d ToTwist2d(ModuleDeltas&&... moduleDeltas) const {
+    return this->ToTwist2d(
+        wpi::array<SwerveModulePosition, NumModules>{moduleDeltas...});
+  }
 
   /**
    * Performs forward kinematics to return the resulting Twist2d from the
@@ -186,7 +223,7 @@
    * determining the robot's position on the field using data from the
    * real-world position delta and angle of each module on the robot.
    *
-   * @param wheelDeltas The latest change in position of the modules (as a
+   * @param moduleDeltas The latest change in position of the modules (as a
    * SwerveModulePosition type) as measured from respective encoders and gyros.
    * The order of the swerve module states should be same as passed into the
    * constructor of this class.
@@ -194,7 +231,20 @@
    * @return The resulting Twist2d.
    */
   Twist2d ToTwist2d(
-      wpi::array<SwerveModulePosition, NumModules> wheelDeltas) const;
+      wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const;
+
+  Twist2d ToTwist2d(
+      const SwerveDriveWheelPositions<NumModules>& start,
+      const SwerveDriveWheelPositions<NumModules>& end) const override {
+    auto result =
+        wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
+    for (size_t i = 0; i < NumModules; i++) {
+      auto startModule = start.positions[i];
+      auto endModule = end.positions[i];
+      result[i] = {endModule.distance - startModule.distance, endModule.angle};
+    }
+    return ToTwist2d(result);
+  }
 
   /**
    * Renormalizes the wheel speeds if any individual speed is above the
@@ -229,7 +279,7 @@
    *
    * @param moduleStates Reference to array of module states. The array will be
    * mutated with the normalized speeds!
-   * @param currentChassisSpeed Current speed of the robot
+   * @param desiredChassisSpeed The desired speed of the robot
    * @param attainableMaxModuleSpeed The absolute max speed a module can reach
    * @param attainableMaxRobotTranslationSpeed The absolute max speed the robot
    * can reach while translating
@@ -238,7 +288,7 @@
    */
   static void DesaturateWheelSpeeds(
       wpi::array<SwerveModuleState, NumModules>* moduleStates,
-      ChassisSpeeds currentChassisSpeed,
+      ChassisSpeeds desiredChassisSpeed,
       units::meters_per_second_t attainableMaxModuleSpeed,
       units::meters_per_second_t attainableMaxRobotTranslationSpeed,
       units::radians_per_second_t attainableMaxRobotRotationSpeed);
@@ -247,7 +297,7 @@
   mutable Matrixd<NumModules * 2, 3> m_inverseKinematics;
   Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>> m_forwardKinematics;
   wpi::array<Translation2d, NumModules> m_modules;
-  mutable wpi::array<SwerveModuleState, NumModules> m_moduleStates;
+  mutable wpi::array<Rotation2d, NumModules> m_moduleHeadings;
 
   mutable Translation2d m_previousCoR;
 };
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
index c7f26e0..db85d99 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
@@ -13,22 +13,32 @@
 
 namespace frc {
 
-template <class... Wheels>
-SwerveDriveKinematics(Translation2d, Wheels...)
-    -> SwerveDriveKinematics<1 + sizeof...(Wheels)>;
+template <typename ModuleTranslation, typename... ModuleTranslations>
+SwerveDriveKinematics(ModuleTranslation, ModuleTranslations...)
+    -> SwerveDriveKinematics<1 + sizeof...(ModuleTranslations)>;
+
+template <size_t NumModules>
+void SwerveDriveKinematics<NumModules>::ResetHeadings(
+    wpi::array<Rotation2d, NumModules> moduleHeadings) {
+  for (size_t i = 0; i < NumModules; i++) {
+    m_moduleHeadings[i] = moduleHeadings[i];
+  }
+}
 
 template <size_t NumModules>
 wpi::array<SwerveModuleState, NumModules>
 SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
     const ChassisSpeeds& chassisSpeeds,
     const Translation2d& centerOfRotation) const {
+  wpi::array<SwerveModuleState, NumModules> moduleStates(wpi::empty_array);
+
   if (chassisSpeeds.vx == 0_mps && chassisSpeeds.vy == 0_mps &&
       chassisSpeeds.omega == 0_rad_per_s) {
     for (size_t i = 0; i < NumModules; i++) {
-      m_moduleStates[i].speed = 0_mps;
+      moduleStates[i] = {0_mps, m_moduleHeadings[i]};
     }
 
-    return m_moduleStates;
+    return moduleStates;
   }
 
   // We have a new center of rotation. We need to compute the matrix again.
@@ -58,28 +68,16 @@
     auto speed = units::math::hypot(x, y);
     Rotation2d rotation{x.value(), y.value()};
 
-    m_moduleStates[i] = {speed, rotation};
+    moduleStates[i] = {speed, rotation};
+    m_moduleHeadings[i] = rotation;
   }
 
-  return m_moduleStates;
-}
-
-template <size_t NumModules>
-template <typename... ModuleStates>
-ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
-    ModuleStates&&... wheelStates) const {
-  static_assert(sizeof...(wheelStates) == NumModules,
-                "Number of modules is not consistent with number of wheel "
-                "locations provided in constructor.");
-
-  wpi::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
-
-  return this->ToChassisSpeeds(moduleStates);
+  return moduleStates;
 }
 
 template <size_t NumModules>
 ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
-    wpi::array<SwerveModuleState, NumModules> moduleStates) const {
+    const wpi::array<SwerveModuleState, NumModules>& moduleStates) const {
   Matrixd<NumModules * 2, 1> moduleStateMatrix;
 
   for (size_t i = 0; i < NumModules; ++i) {
@@ -97,19 +95,6 @@
 }
 
 template <size_t NumModules>
-template <typename... ModuleDeltas>
-Twist2d SwerveDriveKinematics<NumModules>::ToTwist2d(
-    ModuleDeltas&&... wheelDeltas) const {
-  static_assert(sizeof...(wheelDeltas) == NumModules,
-                "Number of modules is not consistent with number of wheel "
-                "locations provided in constructor.");
-
-  wpi::array<SwerveModulePosition, NumModules> moduleDeltas{wheelDeltas...};
-
-  return this->ToTwist2d(moduleDeltas);
-}
-
-template <size_t NumModules>
 Twist2d SwerveDriveKinematics<NumModules>::ToTwist2d(
     wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const {
   Matrixd<NumModules * 2, 1> moduleDeltaMatrix;
@@ -134,12 +119,13 @@
     wpi::array<SwerveModuleState, NumModules>* moduleStates,
     units::meters_per_second_t attainableMaxSpeed) {
   auto& states = *moduleStates;
-  auto realMaxSpeed = std::max_element(states.begin(), states.end(),
-                                       [](const auto& a, const auto& b) {
-                                         return units::math::abs(a.speed) <
-                                                units::math::abs(b.speed);
-                                       })
-                          ->speed;
+  auto realMaxSpeed =
+      units::math::abs(std::max_element(states.begin(), states.end(),
+                                        [](const auto& a, const auto& b) {
+                                          return units::math::abs(a.speed) <
+                                                 units::math::abs(b.speed);
+                                        })
+                           ->speed);
 
   if (realMaxSpeed > attainableMaxSpeed) {
     for (auto& module : states) {
@@ -151,18 +137,19 @@
 template <size_t NumModules>
 void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
     wpi::array<SwerveModuleState, NumModules>* moduleStates,
-    ChassisSpeeds currentChassisSpeed,
+    ChassisSpeeds desiredChassisSpeed,
     units::meters_per_second_t attainableMaxModuleSpeed,
     units::meters_per_second_t attainableMaxRobotTranslationSpeed,
     units::radians_per_second_t attainableMaxRobotRotationSpeed) {
   auto& states = *moduleStates;
 
-  auto realMaxSpeed = std::max_element(states.begin(), states.end(),
-                                       [](const auto& a, const auto& b) {
-                                         return units::math::abs(a.speed) <
-                                                units::math::abs(b.speed);
-                                       })
-                          ->speed;
+  auto realMaxSpeed =
+      units::math::abs(std::max_element(states.begin(), states.end(),
+                                        [](const auto& a, const auto& b) {
+                                          return units::math::abs(a.speed) <
+                                                 units::math::abs(b.speed);
+                                        })
+                           ->speed);
 
   if (attainableMaxRobotTranslationSpeed == 0_mps ||
       attainableMaxRobotRotationSpeed == 0_rad_per_s || realMaxSpeed == 0_mps) {
@@ -170,10 +157,10 @@
   }
 
   auto translationalK =
-      units::math::hypot(currentChassisSpeed.vx, currentChassisSpeed.vy) /
+      units::math::hypot(desiredChassisSpeed.vx, desiredChassisSpeed.vy) /
       attainableMaxRobotTranslationSpeed;
 
-  auto rotationalK = units::math::abs(currentChassisSpeed.omega) /
+  auto rotationalK = units::math::abs(desiredChassisSpeed.omega) /
                      attainableMaxRobotRotationSpeed;
 
   auto k = units::math::max(translationalK, rotationalK);
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
index 015c2c0..2e0e553 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
@@ -11,8 +11,11 @@
 #include <wpi/SymbolExports.h>
 #include <wpi/timestamp.h>
 
+#include "Odometry.h"
 #include "SwerveDriveKinematics.h"
+#include "SwerveDriveWheelPositions.h"
 #include "SwerveModulePosition.h"
+#include "SwerveModuleState.h"
 #include "frc/geometry/Pose2d.h"
 #include "units/time.h"
 
@@ -28,7 +31,9 @@
  * when using computer-vision systems.
  */
 template <size_t NumModules>
-class SwerveDriveOdometry {
+class SwerveDriveOdometry
+    : public Odometry<SwerveDriveWheelSpeeds<NumModules>,
+                      SwerveDriveWheelPositions<NumModules>> {
  public:
   /**
    * Constructs a SwerveDriveOdometry object.
@@ -56,13 +61,13 @@
   void ResetPosition(
       const Rotation2d& gyroAngle,
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
-      const Pose2d& pose);
-
-  /**
-   * Returns the position of the robot on the field.
-   * @return The pose of the robot.
-   */
-  const Pose2d& GetPose() const { return m_pose; }
+      const Pose2d& pose) {
+    Odometry<
+        SwerveDriveWheelSpeeds<NumModules>,
+        SwerveDriveWheelPositions<NumModules>>::ResetPosition(gyroAngle,
+                                                              {modulePositions},
+                                                              pose);
+  }
 
   /**
    * Updates the robot's position on the field using forward kinematics and
@@ -79,17 +84,15 @@
    */
   const Pose2d& Update(
       const Rotation2d& gyroAngle,
-      const wpi::array<SwerveModulePosition, NumModules>& modulePositions);
+      const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
+    return Odometry<
+        SwerveDriveWheelSpeeds<NumModules>,
+        SwerveDriveWheelPositions<NumModules>>::Update(gyroAngle,
+                                                       {modulePositions});
+  }
 
  private:
-  SwerveDriveKinematics<NumModules> m_kinematics;
-  Pose2d m_pose;
-
-  Rotation2d m_previousAngle;
-  Rotation2d m_gyroOffset;
-
-  wpi::array<SwerveModulePosition, NumModules> m_previousModulePositions{
-      wpi::empty_array};
+  SwerveDriveKinematics<NumModules> m_kinematicsImpl;
 };
 
 extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
index 64b46c1..48ddfec 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
@@ -13,58 +13,11 @@
     SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
     const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
     const Pose2d& initialPose)
-    : m_kinematics(kinematics), m_pose(initialPose) {
-  m_previousAngle = m_pose.Rotation();
-  m_gyroOffset = m_pose.Rotation() - gyroAngle;
-
-  for (size_t i = 0; i < NumModules; i++) {
-    m_previousModulePositions[i] = {modulePositions[i].distance,
-                                    modulePositions[i].angle};
-  }
-
+    : Odometry<SwerveDriveWheelSpeeds<NumModules>,
+               SwerveDriveWheelPositions<NumModules>>(
+          m_kinematicsImpl, gyroAngle, {modulePositions}, initialPose),
+      m_kinematicsImpl(kinematics) {
   wpi::math::MathSharedStore::ReportUsage(
       wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
 }
-
-template <size_t NumModules>
-void SwerveDriveOdometry<NumModules>::ResetPosition(
-    const Rotation2d& gyroAngle,
-    const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
-    const Pose2d& pose) {
-  m_pose = pose;
-  m_previousAngle = pose.Rotation();
-  m_gyroOffset = m_pose.Rotation() - gyroAngle;
-
-  for (size_t i = 0; i < NumModules; i++) {
-    m_previousModulePositions[i].distance = modulePositions[i].distance;
-  }
-}
-
-template <size_t NumModules>
-const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
-    const Rotation2d& gyroAngle,
-    const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
-  auto moduleDeltas =
-      wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
-  for (size_t index = 0; index < NumModules; index++) {
-    auto lastPosition = m_previousModulePositions[index];
-    auto currentPosition = modulePositions[index];
-    moduleDeltas[index] = {currentPosition.distance - lastPosition.distance,
-                           currentPosition.angle};
-
-    m_previousModulePositions[index].distance = modulePositions[index].distance;
-  }
-
-  auto angle = gyroAngle + m_gyroOffset;
-
-  auto twist = m_kinematics.ToTwist2d(moduleDeltas);
-  twist.dtheta = (angle - m_previousAngle).Radians();
-
-  auto newPose = m_pose.Exp(twist);
-
-  m_previousAngle = angle;
-  m_pose = {newPose.Translation(), angle};
-
-  return m_pose;
-}
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveWheelPositions.h
new file mode 100644
index 0000000..c1686d2
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveWheelPositions.h
@@ -0,0 +1,51 @@
+// 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 <wpi/MathExtras.h>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/kinematics/SwerveModulePosition.h"
+
+namespace frc {
+/**
+ * Represents the wheel positions for a swerve drive drivetrain.
+ */
+template <size_t NumModules>
+struct WPILIB_DLLEXPORT SwerveDriveWheelPositions {
+  /**
+   * The distances driven by the wheels.
+   */
+  wpi::array<SwerveModulePosition, NumModules> positions;
+
+  /**
+   * Checks equality between this SwerveDriveWheelPositions and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const SwerveDriveWheelPositions& other) const = default;
+
+  /**
+   * Checks inequality between this SwerveDriveWheelPositions and another
+   * object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const SwerveDriveWheelPositions& other) const = default;
+
+  SwerveDriveWheelPositions<NumModules> Interpolate(
+      const SwerveDriveWheelPositions<NumModules>& endValue, double t) const {
+    auto result =
+        wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
+    for (size_t i = 0; i < NumModules; i++) {
+      result[i] = positions[i].Interpolate(endValue.positions[i], t);
+    }
+    return {result};
+  }
+};
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
index 18ed464..93f7465 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <wpi/MathExtras.h>
 #include <wpi/SymbolExports.h>
 
 #include "frc/geometry/Rotation2d.h"
@@ -25,5 +26,19 @@
    * Angle of the module.
    */
   Rotation2d angle;
+
+  /**
+   * Checks equality between this SwerveModulePosition and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const SwerveModulePosition& other) const;
+
+  SwerveModulePosition Interpolate(const SwerveModulePosition& endValue,
+                                   double t) const {
+    return {wpi::Lerp(distance, endValue.distance, t),
+            wpi::Lerp(angle, endValue.angle, t)};
+  }
 };
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
index cae2d53..2f95d9b 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
@@ -27,6 +27,14 @@
   Rotation2d angle;
 
   /**
+   * Checks equality between this SwerveModuleState and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const SwerveModuleState& other) const;
+
+  /**
    * Minimize the change in heading the desired swerve module state would
    * require by potentially reversing the direction the wheel spins. If this is
    * used with the PIDController class's continuous input functionality, the
@@ -36,13 +44,6 @@
    * @param currentAngle The current module angle.
    */
   static SwerveModuleState Optimize(const SwerveModuleState& desiredState,
-                                    const Rotation2d& currentAngle) {
-    auto delta = desiredState.angle - currentAngle;
-    if (units::math::abs(delta.Degrees()) > 90_deg) {
-      return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}};
-    } else {
-      return {desiredState.speed, desiredState.angle};
-    }
-  }
+                                    const Rotation2d& currentAngle);
 };
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/WheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/WheelPositions.h
new file mode 100644
index 0000000..8867f66
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/WheelPositions.h
@@ -0,0 +1,15 @@
+// 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 <concepts>
+
+namespace frc {
+template <typename T>
+concept WheelPositions =
+    std::copy_constructible<T> && requires(T a, T b, double t) {
+      { a.Interpolate(b, t) } -> std::convertible_to<T>;
+    };
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
index 0636707..1d6aaeb 100644
--- a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
+++ b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
@@ -49,7 +49,7 @@
    * Returns the hermite basis matrix for cubic hermite spline interpolation.
    * @return The hermite basis matrix for cubic hermite spline interpolation.
    */
-  static Matrixd<4, 4> MakeHermiteBasis() {
+  static Eigen::Matrix4d MakeHermiteBasis() {
     // Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
     // the coefficients of the spline P(t) = a₃t³ + a₂t² + a₁t + a₀.
     //
@@ -71,10 +71,10 @@
     // [a₁] = [ 0  1  0  0][P(i+1) ]
     // [a₀] = [ 1  0  0  0][P'(i+1)]
 
-    static const Matrixd<4, 4> basis{{+2.0, +1.0, -2.0, +1.0},
-                                     {-3.0, -2.0, +3.0, -1.0},
-                                     {+0.0, +1.0, +0.0, +0.0},
-                                     {+1.0, +0.0, +0.0, +0.0}};
+    static const Eigen::Matrix4d basis{{+2.0, +1.0, -2.0, +1.0},
+                                       {-3.0, -2.0, +3.0, -1.0},
+                                       {+0.0, +1.0, +0.0, +0.0},
+                                       {+1.0, +0.0, +0.0, +0.0}};
     return basis;
   }
 
diff --git a/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h b/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
index 0720cd1..e1d2d52 100644
--- a/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
+++ b/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
@@ -56,7 +56,7 @@
   };
 
   /**
-   * Parameterizes the spline. This method breaks up the spline into various
+   * Parametrizes the spline. This method breaks up the spline into various
    * arcs until their dx, dy, and dtheta are within specific tolerances.
    *
    * @param spline The spline to parameterize.
diff --git a/wpimath/src/main/native/include/frc/system/Discretization.h b/wpimath/src/main/native/include/frc/system/Discretization.h
index 957b875..72a3d43 100644
--- a/wpimath/src/main/native/include/frc/system/Discretization.h
+++ b/wpimath/src/main/native/include/frc/system/Discretization.h
@@ -4,9 +4,10 @@
 
 #pragma once
 
+#include <unsupported/Eigen/MatrixFunctions>
+
 #include "frc/EigenCore.h"
 #include "units/time.h"
-#include "unsupported/Eigen/MatrixFunctions"
 
 namespace frc {
 
@@ -44,9 +45,9 @@
   // M = [A  B]
   //     [0  0]
   Matrixd<States + Inputs, States + Inputs> M;
-  M.setZero();
   M.template block<States, States>(0, 0) = contA;
   M.template block<States, Inputs>(0, States) = contB;
+  M.template block<Inputs, States + Inputs>(States, 0).setZero();
 
   // ϕ = eᴹᵀ = [A_d  B_d]
   //           [ 0    I ]
@@ -101,95 +102,6 @@
 }
 
 /**
- * Discretizes the given continuous A and Q matrices.
- *
- * Rather than solving a 2N x 2N matrix exponential like in DiscretizeAQ()
- * (which is expensive), we take advantage of the structure of the block matrix
- * of A and Q.
- *
- * <ul>
- *   <li>eᴬᵀ, which is only N x N, is relatively cheap.
- *   <li>The upper-right quarter of the 2N x 2N matrix, which we can approximate
- *       using a taylor series to several terms and still be substantially
- *       cheaper than taking the big exponential.
- * </ul>
- *
- * @tparam States Number of states.
- * @param contA Continuous system matrix.
- * @param contQ Continuous process noise covariance matrix.
- * @param dt    Discretization timestep.
- * @param discA Storage for discrete system matrix.
- * @param discQ Storage for discrete process noise covariance matrix.
- */
-template <int States>
-void DiscretizeAQTaylor(const Matrixd<States, States>& contA,
-                        const Matrixd<States, States>& contQ,
-                        units::second_t dt, Matrixd<States, States>* discA,
-                        Matrixd<States, States>* discQ) {
-  //       T
-  // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-  //       0
-  //
-  // M = [−A  Q ]
-  //     [ 0  Aᵀ]
-  // ϕ = eᴹᵀ
-  // ϕ₁₂ = A_d⁻¹Q_d
-  //
-  // Taylor series of ϕ:
-  //
-  //   ϕ = eᴹᵀ = I + MT + 1/2 M²T² + 1/6 M³T³ + …
-  //   ϕ = eᴹᵀ = I + MT + 1/2 T²M² + 1/6 T³M³ + …
-  //
-  // Taylor series of ϕ expanded for ϕ₁₂:
-  //
-  //   ϕ₁₂ = 0 + QT + 1/2 T² (−AQ + QAᵀ) + 1/6 T³ (−A lastTerm + Q Aᵀ²) + …
-  //
-  // ```
-  // lastTerm = Q
-  // lastCoeff = T
-  // ATn = Aᵀ
-  // ϕ₁₂ = lastTerm lastCoeff = QT
-  //
-  // for i in range(2, 6):
-  //   // i = 2
-  //   lastTerm = −A lastTerm + Q ATn = −AQ + QAᵀ
-  //   lastCoeff *= T/i → lastCoeff *= T/2 = 1/2 T²
-  //   ATn *= Aᵀ = Aᵀ²
-  //
-  //   // i = 3
-  //   lastTerm = −A lastTerm + Q ATn = −A (−AQ + QAᵀ) + QAᵀ² = …
-  //   …
-  // ```
-
-  // Make continuous Q symmetric if it isn't already
-  Matrixd<States, States> Q = (contQ + contQ.transpose()) / 2.0;
-
-  Matrixd<States, States> lastTerm = Q;
-  double lastCoeff = dt.value();
-
-  // Aᵀⁿ
-  Matrixd<States, States> ATn = contA.transpose();
-
-  Matrixd<States, States> phi12 = lastTerm * lastCoeff;
-
-  // i = 6 i.e. 5th order should be enough precision
-  for (int i = 2; i < 6; ++i) {
-    lastTerm = -contA * lastTerm + Q * ATn;
-    lastCoeff *= dt.value() / static_cast<double>(i);
-
-    phi12 += lastTerm * lastCoeff;
-
-    ATn *= contA.transpose();
-  }
-
-  DiscretizeA<States>(contA, dt, discA);
-  Q = *discA * phi12;
-
-  // Make discrete Q symmetric if it isn't already
-  *discQ = (Q + Q.transpose()) / 2.0;
-}
-
-/**
  * Returns a discretized version of the provided continuous measurement noise
  * covariance matrix.
  *
diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
index 1300a82..c61531a 100644
--- a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
+++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <functional>
+
 #include <wpi/SymbolExports.h>
 
 #include "frc/EigenCore.h"
diff --git a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
index bb856ec..98b6cc3 100644
--- a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
+++ b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
@@ -4,12 +4,10 @@
 
 #pragma once
 
-#include <frc/StateSpaceUtil.h>
-
 #include <algorithm>
 #include <array>
+#include <cmath>
 
-#include "Eigen/Core"
 #include "units/time.h"
 
 namespace frc {
@@ -122,7 +120,11 @@
                               (b1[6] - b2[6]) * k7))
                             .norm();
 
-      h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
+      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;
diff --git a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
index 831f532..ad711ee 100644
--- a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
+++ b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
@@ -97,7 +97,7 @@
   }
 
   /**
-   * Returns the speed produced by the motor at a given torque and input
+   * Returns the angular speed produced by the motor at a given torque and input
    * voltage.
    *
    * @param torque        The torque produced by the motor.
@@ -119,89 +119,123 @@
   }
 
   /**
-   * Returns instance of CIM.
+   * Returns a gearbox of CIM motors.
    */
   static constexpr DCMotor CIM(int numMotors = 1) {
     return DCMotor(12_V, 2.42_Nm, 133_A, 2.7_A, 5310_rpm, numMotors);
   }
 
   /**
-   * Returns instance of MiniCIM.
+   * Returns a gearbox of MiniCIM motors.
    */
   static constexpr DCMotor MiniCIM(int numMotors = 1) {
     return DCMotor(12_V, 1.41_Nm, 89_A, 3_A, 5840_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Bag motor.
+   * Returns a gearbox of Bag motor motors.
    */
   static constexpr DCMotor Bag(int numMotors = 1) {
     return DCMotor(12_V, 0.43_Nm, 53_A, 1.8_A, 13180_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Vex 775 Pro.
+   * Returns a gearbox of Vex 775 Pro motors.
    */
   static constexpr DCMotor Vex775Pro(int numMotors = 1) {
     return DCMotor(12_V, 0.71_Nm, 134_A, 0.7_A, 18730_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Andymark RS 775-125.
+   * Returns a gearbox of Andymark RS 775-125 motors.
    */
   static constexpr DCMotor RS775_125(int numMotors = 1) {
     return DCMotor(12_V, 0.28_Nm, 18_A, 1.6_A, 5800_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Banebots RS 775.
+   * Returns a gearbox of Banebots RS 775 motors.
    */
   static constexpr DCMotor BanebotsRS775(int numMotors = 1) {
     return DCMotor(12_V, 0.72_Nm, 97_A, 2.7_A, 13050_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Andymark 9015.
+   * Returns a gearbox of Andymark 9015 motors.
    */
   static constexpr DCMotor Andymark9015(int numMotors = 1) {
     return DCMotor(12_V, 0.36_Nm, 71_A, 3.7_A, 14270_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Banebots RS 550.
+   * Returns a gearbox of Banebots RS 550 motors.
    */
   static constexpr DCMotor BanebotsRS550(int numMotors = 1) {
     return DCMotor(12_V, 0.38_Nm, 84_A, 0.4_A, 19000_rpm, numMotors);
   }
 
   /**
-   * Returns instance of NEO brushless motor.
+   * Returns a gearbox of NEO brushless motors.
    */
   static constexpr DCMotor NEO(int numMotors = 1) {
     return DCMotor(12_V, 2.6_Nm, 105_A, 1.8_A, 5676_rpm, numMotors);
   }
 
   /**
-   * Returns instance of NEO 550 brushless motor.
+   * Returns a gearbox of NEO 550 brushless motors.
    */
   static constexpr DCMotor NEO550(int numMotors = 1) {
     return DCMotor(12_V, 0.97_Nm, 100_A, 1.4_A, 11000_rpm, numMotors);
   }
 
   /**
-   * Returns instance of Falcon 500 brushless motor.
+   * Returns a gearbox of Falcon 500 brushless motors.
    */
   static constexpr DCMotor Falcon500(int numMotors = 1) {
     return DCMotor(12_V, 4.69_Nm, 257_A, 1.5_A, 6380_rpm, numMotors);
   }
 
   /**
+   * Return a gearbox of Falcon 500 motors with FOC (Field-Oriented Control)
+   * enabled.
+   */
+  static constexpr DCMotor Falcon500FOC(int numMotors = 1) {
+    // https://store.ctr-electronics.com/falcon-500-powered-by-talon-fx/
+    return DCMotor(12_V, 5.84_Nm, 304_A, 1.5_A, 6080_rpm, numMotors);
+  }
+
+  /**
    * Return a gearbox of Romi/TI_RSLK MAX motors.
    */
   static constexpr DCMotor RomiBuiltIn(int numMotors = 1) {
     // From https://www.pololu.com/product/1520/specs
     return DCMotor(4.5_V, 0.1765_Nm, 1.25_A, 0.13_A, 150_rpm, numMotors);
   }
+
+  /**
+   * Return a gearbox of Kraken X60 brushless motors.
+   */
+  static constexpr DCMotor KrakenX60(int numMotors = 1) {
+    // From https://store.ctr-electronics.com/announcing-kraken-x60/
+    return DCMotor(12_V, 7.09_Nm, 366_A, 2_A, 6000_rpm, numMotors);
+  }
+
+  /**
+   * Return a gearbox of Kraken X60 brushless motors with FOC (Field-Oriented
+   * Control) enabled.
+   */
+  static constexpr DCMotor KrakenX60FOC(int numMotors = 1) {
+    // From https://store.ctr-electronics.com/announcing-kraken-x60/
+    return DCMotor(12_V, 9.37_Nm, 483_A, 2_A, 5800_rpm, numMotors);
+  }
+
+  /**
+   * Return a gearbox of Neo Vortex brushless motors.
+   */
+  static constexpr DCMotor NeoVortex(int numMotors = 1) {
+    // From https://www.revrobotics.com/next-generation-spark-neo/
+    return DCMotor(12_V, 3.60_Nm, 211_A, 3.615_A, 6784_rpm, numMotors);
+  }
 };
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
index 3e69545..64f3496 100644
--- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
+++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <concepts>
 #include <stdexcept>
 
 #include <wpi/SymbolExports.h>
@@ -76,9 +77,9 @@
    * @param kA The acceleration gain, in volts/(unit/sec²).
    * @throws std::domain_error if kV <= 0 or kA <= 0.
    */
-  template <typename Distance, typename = std::enable_if_t<
-                                   std::is_same_v<units::meter, Distance> ||
-                                   std::is_same_v<units::radian, Distance>>>
+  template <typename Distance>
+    requires std::same_as<units::meter, Distance> ||
+             std::same_as<units::radian, Distance>
   static LinearSystem<1, 1, 1> IdentifyVelocitySystem(
       decltype(1_V / Velocity_t<Distance>(1)) kV,
       decltype(1_V / Acceleration_t<Distance>(1)) kA) {
@@ -117,9 +118,9 @@
    *
    * @throws std::domain_error if kV <= 0 or kA <= 0.
    */
-  template <typename Distance, typename = std::enable_if_t<
-                                   std::is_same_v<units::meter, Distance> ||
-                                   std::is_same_v<units::radian, Distance>>>
+  template <typename Distance>
+    requires std::same_as<units::meter, Distance> ||
+             std::same_as<units::radian, Distance>
   static LinearSystem<2, 1, 1> IdentifyPositionSystem(
       decltype(1_V / Velocity_t<Distance>(1)) kV,
       decltype(1_V / Acceleration_t<Distance>(1)) kA) {
@@ -217,6 +218,48 @@
   static LinearSystem<2, 1, 2> DCMotorSystem(DCMotor motor,
                                              units::kilogram_square_meter_t J,
                                              double G);
+
+  /**
+   * Create a state-space model of a DC motor system from its kV
+   * (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
+   * found using SysId. the states of the system are [position, velocity],
+   * inputs are [voltage], and outputs are [position].
+   *
+   * You MUST use an SI unit (i.e. meters or radians) for the Distance template
+   * argument. You may still use non-SI units (such as feet or inches) for the
+   * actual method arguments; they will automatically be converted to SI
+   * internally.
+   *
+   * The parameters provided by the user are from this feedforward model:
+   *
+   * u = K_v v + K_a a
+   *
+   * @param kV The velocity gain, in volts/(unit/sec).
+   * @param kA The acceleration gain, in volts/(unit/sec²).
+   *
+   * @throws std::domain_error if kV <= 0 or kA <= 0.
+   */
+  template <typename Distance>
+    requires std::same_as<units::meter, Distance> ||
+             std::same_as<units::radian, Distance>
+  static LinearSystem<2, 1, 2> DCMotorSystem(
+      decltype(1_V / Velocity_t<Distance>(1)) kV,
+      decltype(1_V / Acceleration_t<Distance>(1)) kA) {
+    if (kV <= decltype(kV){0}) {
+      throw std::domain_error("Kv must be greater than zero.");
+    }
+    if (kA <= decltype(kA){0}) {
+      throw std::domain_error("Ka must be greater than zero.");
+    }
+
+    Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
+    Matrixd<2, 1> B{0.0, 1.0 / kA.value()};
+    Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
+    Matrixd<2, 1> D{{0.0}, {0.0}};
+
+    return LinearSystem<2, 1, 2>(A, B, C, D);
+  }
+
   /**
    * Create a state-space model of differential drive drivetrain. In this model,
    * the states are [left velocity, right velocity], the inputs are [left
diff --git a/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h
new file mode 100644
index 0000000..f45987b
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h
@@ -0,0 +1,194 @@
+// 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 "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * A Exponential-shaped velocity profile.
+ *
+ * While this class can be used for a profiled movement from start to finish,
+ * the intended usage is to filter a reference's dynamics based on
+ * ExponentialProfile velocity constraints. To compute the reference obeying
+ * this constraint, do the following.
+ *
+ * Initialization:
+ * @code{.cpp}
+ * ExponentialProfile::Constraints constraints{kMaxV, kV, kA};
+ * State previousProfiledReference = {initialReference, 0_mps};
+ * @endcode
+ *
+ * Run on update:
+ * @code{.cpp}
+ * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate,
+ * previousProfiledReference, unprofiledReference);
+ * @endcode
+ *
+ * where `unprofiledReference` is free to change between calls. Note that when
+ * the unprofiled reference is within the constraints, `Calculate()` returns the
+ * unprofiled reference unchanged.
+ *
+ * Otherwise, a timer can be started to provide monotonic values for
+ * `Calculate()` and to determine when the profile has completed via
+ * `IsFinished()`.
+ */
+template <class Distance, class Input>
+class ExponentialProfile {
+ public:
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using Input_t = units::unit_t<Input>;
+  using A_t = units::unit_t<units::inverse<units::seconds>>;
+  using B_t =
+      units::unit_t<units::compound_unit<Acceleration, units::inverse<Input>>>;
+  using KV = units::compound_unit<Input, units::inverse<Velocity>>;
+  using kV_t = units::unit_t<KV>;
+  using KA = units::compound_unit<Input, units::inverse<Acceleration>>;
+  using kA_t = units::unit_t<KA>;
+
+  class Constraints {
+   public:
+    Constraints(Input_t maxInput, A_t A, B_t B)
+        : maxInput{maxInput}, A{A}, B{B} {}
+    Constraints(Input_t maxInput, kV_t kV, kA_t kA)
+        : maxInput{maxInput}, A{-kV / kA}, B{1 / kA} {}
+    Velocity_t MaxVelocity() const { return -maxInput * B / A; }
+
+    Input_t maxInput{0};
+    A_t A{0};
+    B_t B{0};
+  };
+
+  class State {
+   public:
+    Distance_t position{0};
+    Velocity_t velocity{0};
+    bool operator==(const State&) const = default;
+  };
+
+  class ProfileTiming {
+   public:
+    units::second_t inflectionTime;
+    units::second_t totalTime;
+
+    bool IsFinished(const units::second_t& time) const {
+      return time > totalTime;
+    }
+  };
+
+  /**
+   * Construct a ExponentialProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum input.
+   */
+  explicit ExponentialProfile(Constraints constraints);
+
+  ExponentialProfile(const ExponentialProfile&) = default;
+  ExponentialProfile& operator=(const ExponentialProfile&) = default;
+  ExponentialProfile(ExponentialProfile&&) = default;
+  ExponentialProfile& operator=(ExponentialProfile&&) = default;
+
+  /**
+   * Calculate the correct position and velocity for the profile at a time t
+   * where the current state is at time t = 0.
+   */
+  State Calculate(const units::second_t& t, const State& current,
+                  const State& goal) const;
+
+  /**
+   * Calculate the point after which the fastest way to reach the goal state is
+   * to apply input in the opposite direction.
+   */
+  State CalculateInflectionPoint(const State& current, const State& goal) const;
+
+  /**
+   * Calculate the time it will take for this profile to reach the goal state.
+   */
+  units::second_t TimeLeftUntil(const State& current, const State& goal) const;
+
+  /**
+   * Calculate the time it will take for this profile to reach the inflection
+   * point, and the time it will take for this profile to reach the goal state.
+   */
+  ProfileTiming CalculateProfileTiming(const State& current,
+                                       const State& goal) const;
+
+ private:
+  /**
+   * Calculate the point after which the fastest way to reach the goal state is
+   * to apply input in the opposite direction.
+   */
+  State CalculateInflectionPoint(const State& current, const State& goal,
+                                 const Input_t& input) const;
+
+  /**
+   * Calculate the time it will take for this profile to reach the inflection
+   * point, and the time it will take for this profile to reach the goal state.
+   */
+  ProfileTiming CalculateProfileTiming(const State& current,
+                                       const State& inflectionPoint,
+                                       const State& goal,
+                                       const Input_t& input) const;
+
+  /**
+   * Calculate the velocity reached after t seconds when applying an input from
+   * the initial state.
+   */
+  Velocity_t ComputeVelocityFromTime(const units::second_t& time,
+                                     const Input_t& input,
+                                     const State& initial) const;
+
+  /**
+   * Calculate the position reached after t seconds when applying an input from
+   * the initial state.
+   */
+  Distance_t ComputeDistanceFromTime(const units::second_t& time,
+                                     const Input_t& input,
+                                     const State& initial) const;
+
+  /**
+   * Calculate the distance reached at the same time as the given velocity when
+   * applying the given input from the initial state.
+   */
+  Distance_t ComputeDistanceFromVelocity(const Velocity_t& velocity,
+                                         const Input_t& input,
+                                         const State& initial) const;
+
+  /**
+   * Calculate the time required to reach a specified velocity given the initial
+   * velocity.
+   */
+  units::second_t ComputeTimeFromVelocity(const Velocity_t& velocity,
+                                          const Input_t& input,
+                                          const Velocity_t& initial) const;
+
+  /**
+   * Calculate the velocity at which input should be reversed in order to reach
+   * the goal state from the current state.
+   */
+  Velocity_t SolveForInflectionVelocity(const Input_t& input,
+                                        const State& current,
+                                        const State& goal) const;
+
+  /**
+   * Returns true if the profile should be inverted.
+   *
+   * <p>The profile is inverted if we should first apply negative input in order
+   * to reach the goal state.
+   */
+  bool ShouldFlipInput(const State& current, const State& goal) const;
+
+  Constraints m_constraints;
+};
+}  // namespace frc
+
+#include "ExponentialProfile.inc"
diff --git a/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.inc b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.inc
new file mode 100644
index 0000000..ded5245
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.inc
@@ -0,0 +1,253 @@
+// 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 <algorithm>
+
+#include <fmt/core.h>
+
+#include "frc/trajectory/ExponentialProfile.h"
+#include "units/math.h"
+
+namespace frc {
+template <class Distance, class Input>
+ExponentialProfile<Distance, Input>::ExponentialProfile(Constraints constraints)
+    : m_constraints(constraints) {}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::State
+ExponentialProfile<Distance, Input>::Calculate(const units::second_t& t,
+                                               const State& current,
+                                               const State& goal) const {
+  auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
+  auto u = direction * m_constraints.maxInput;
+
+  auto inflectionPoint = CalculateInflectionPoint(current, goal, u);
+  auto timing = CalculateProfileTiming(current, inflectionPoint, goal, u);
+
+  if (t < 0_s) {
+    return current;
+  } else if (t < timing.inflectionTime) {
+    return {ComputeDistanceFromTime(t, u, current),
+            ComputeVelocityFromTime(t, u, current)};
+  } else if (t < timing.totalTime) {
+    return {ComputeDistanceFromTime(t - timing.totalTime, -u, goal),
+            ComputeVelocityFromTime(t - timing.totalTime, -u, goal)};
+  } else {
+    return goal;
+  }
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::State
+ExponentialProfile<Distance, Input>::CalculateInflectionPoint(
+    const State& current, const State& goal) const {
+  auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
+  auto u = direction * m_constraints.maxInput;
+
+  return CalculateInflectionPoint(current, goal, u);
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::State
+ExponentialProfile<Distance, Input>::CalculateInflectionPoint(
+    const State& current, const State& goal, const Input_t& input) const {
+  auto u = input;
+
+  if (current == goal) {
+    return current;
+  }
+
+  auto inflectionVelocity = SolveForInflectionVelocity(u, current, goal);
+  auto inflectionPosition =
+      ComputeDistanceFromVelocity(inflectionVelocity, -u, goal);
+
+  return {inflectionPosition, inflectionVelocity};
+}
+
+template <class Distance, class Input>
+units::second_t ExponentialProfile<Distance, Input>::TimeLeftUntil(
+    const State& current, const State& goal) const {
+  auto timing = CalculateProfileTiming(current, goal);
+
+  return timing.totalTime;
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::ProfileTiming
+ExponentialProfile<Distance, Input>::CalculateProfileTiming(
+    const State& current, const State& goal) const {
+  auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
+  auto u = direction * m_constraints.maxInput;
+
+  auto inflectionPoint = CalculateInflectionPoint(current, goal, u);
+  return CalculateProfileTiming(current, inflectionPoint, goal, u);
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::ProfileTiming
+ExponentialProfile<Distance, Input>::CalculateProfileTiming(
+    const State& current, const State& inflectionPoint, const State& goal,
+    const Input_t& input) const {
+  auto u = input;
+  auto u_dir = units::math::abs(u) / u;
+
+  units::second_t inflectionT_forward;
+
+  // We need to handle 5 cases here:
+  //
+  // - Approaching -maxVelocity from below
+  // - Approaching -maxVelocity from above
+  // - Approaching maxVelocity from below
+  // - Approaching maxVelocity from above
+  // - At +-maxVelocity
+  //
+  // For cases 1 and 3, we want to subtract epsilon from the inflection point
+  // velocity For cases 2 and 4, we want to add epsilon to the inflection point
+  // velocity. For case 5, we have reached inflection point velocity.
+  auto epsilon = Velocity_t(1e-9);
+  if (units::math::abs(u_dir * m_constraints.MaxVelocity() -
+                       inflectionPoint.velocity) < epsilon) {
+    auto solvableV = inflectionPoint.velocity;
+    units::second_t t_to_solvable_v;
+    Distance_t x_at_solvable_v;
+    if (units::math::abs(current.velocity - inflectionPoint.velocity) <
+        epsilon) {
+      t_to_solvable_v = 0_s;
+      x_at_solvable_v = current.position;
+    } else {
+      if (units::math::abs(current.velocity) > m_constraints.MaxVelocity()) {
+        solvableV += u_dir * epsilon;
+      } else {
+        solvableV -= u_dir * epsilon;
+      }
+
+      t_to_solvable_v = ComputeTimeFromVelocity(solvableV, u, current.velocity);
+      x_at_solvable_v = ComputeDistanceFromVelocity(solvableV, u, current);
+    }
+
+    inflectionT_forward =
+        t_to_solvable_v + u_dir * (inflectionPoint.position - x_at_solvable_v) /
+                              m_constraints.MaxVelocity();
+  } else {
+    inflectionT_forward =
+        ComputeTimeFromVelocity(inflectionPoint.velocity, u, current.velocity);
+  }
+
+  auto inflectionT_backward =
+      ComputeTimeFromVelocity(inflectionPoint.velocity, -u, goal.velocity);
+
+  return {inflectionT_forward, inflectionT_forward - inflectionT_backward};
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::Distance_t
+ExponentialProfile<Distance, Input>::ComputeDistanceFromTime(
+    const units::second_t& time, const Input_t& input,
+    const State& initial) const {
+  auto A = m_constraints.A;
+  auto B = m_constraints.B;
+  auto u = input;
+
+  return initial.position +
+         (-B * u * time +
+          (initial.velocity + B * u / A) * (units::math::exp(A * time) - 1)) /
+             A;
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::Velocity_t
+ExponentialProfile<Distance, Input>::ComputeVelocityFromTime(
+    const units::second_t& time, const Input_t& input,
+    const State& initial) const {
+  auto A = m_constraints.A;
+  auto B = m_constraints.B;
+  auto u = input;
+
+  return (initial.velocity + B * u / A) * units::math::exp(A * time) -
+         B * u / A;
+}
+
+template <class Distance, class Input>
+units::second_t ExponentialProfile<Distance, Input>::ComputeTimeFromVelocity(
+    const Velocity_t& velocity, const Input_t& input,
+    const Velocity_t& initial) const {
+  auto A = m_constraints.A;
+  auto B = m_constraints.B;
+  auto u = input;
+
+  return units::math::log((A * velocity + B * u) / (A * initial + B * u)) / A;
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::Distance_t
+ExponentialProfile<Distance, Input>::ComputeDistanceFromVelocity(
+    const Velocity_t& velocity, const Input_t& input,
+    const State& initial) const {
+  auto A = m_constraints.A;
+  auto B = m_constraints.B;
+  auto u = input;
+
+  return initial.position + (velocity - initial.velocity) / A -
+         B * u / (A * A) *
+             units::math::log((A * velocity + B * u) /
+                              (A * initial.velocity + B * u));
+}
+
+template <class Distance, class Input>
+typename ExponentialProfile<Distance, Input>::Velocity_t
+ExponentialProfile<Distance, Input>::SolveForInflectionVelocity(
+    const Input_t& input, const State& current, const State& goal) const {
+  auto A = m_constraints.A;
+  auto B = m_constraints.B;
+  auto u = input;
+
+  auto u_dir = u / units::math::abs(u);
+
+  auto position_delta = goal.position - current.position;
+  auto velocity_delta = goal.velocity - current.velocity;
+
+  auto scalar = (A * current.velocity + B * u) * (A * goal.velocity - B * u);
+  auto power = -A / B / u * (A * position_delta - velocity_delta);
+
+  auto a = -A * A;
+  auto c = B * B * u * u + scalar * units::math::exp(power);
+
+  if (-1e-9 < c.value() && c.value() < 0) {
+    // numeric instability - the heuristic gets it right but c is around -1e-13
+    return Velocity_t(0);
+  }
+
+  return u_dir * units::math::sqrt(-c / a);
+}
+
+template <class Distance, class Input>
+bool ExponentialProfile<Distance, Input>::ShouldFlipInput(
+    const State& current, const State& goal) const {
+  auto u = m_constraints.maxInput;
+
+  auto v0 = current.velocity;
+  auto xf = goal.position;
+  auto vf = goal.velocity;
+
+  auto x_forward = ComputeDistanceFromVelocity(vf, u, current);
+  auto x_reverse = ComputeDistanceFromVelocity(vf, -u, current);
+
+  if (v0 >= m_constraints.MaxVelocity()) {
+    return xf < x_reverse;
+  }
+
+  if (v0 <= -m_constraints.MaxVelocity()) {
+    return xf < x_forward;
+  }
+
+  auto a = v0 >= Velocity_t(0);
+  auto b = vf >= Velocity_t(0);
+  auto c = xf >= x_forward;
+  auto d = xf >= x_reverse;
+
+  return (a && !d) || (b && !c) || (!c && !d);
+}
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
index f5ee79b..ca97593 100644
--- a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
+++ b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
@@ -7,6 +7,7 @@
 #include <vector>
 
 #include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
 
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Transform2d.h"
@@ -15,10 +16,6 @@
 #include "units/time.h"
 #include "units/velocity.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace frc {
 /**
  * Represents a time-parameterized trajectory. The trajectory contains of
@@ -66,6 +63,8 @@
 
   /**
    * Constructs a trajectory from a vector of states.
+   *
+   * @throws std::invalid_argument if the vector of states is empty.
    */
   explicit Trajectory(const std::vector<State>& states);
 
@@ -77,6 +76,7 @@
 
   /**
    * Return the states of the trajectory.
+   *
    * @return The states of the trajectory.
    */
   const std::vector<State>& States() const { return m_states; }
@@ -86,6 +86,7 @@
    *
    * @param t The point in time since the beginning of the trajectory to sample.
    * @return The state at that point in time.
+   * @throws std::runtime_error if the trajectory has no states.
    */
   State Sample(units::second_t t) const;
 
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h
index b1a0b52..4e6c1e6 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h
@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <concepts>
 #include <memory>
 #include <utility>
 #include <vector>
@@ -74,8 +75,7 @@
    * Adds a user-defined constraint to the trajectory.
    * @param constraint The user-defined constraint.
    */
-  template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
-                                     TrajectoryConstraint, Constraint>>>
+  template <std::derived_from<TrajectoryConstraint> Constraint>
   void AddConstraint(Constraint constraint) {
     m_constraints.emplace_back(std::make_unique<Constraint>(constraint));
   }
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
index 24a8253..73aab38 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <wpi/deprecated.h>
+
 #include "units/time.h"
 #include "wpimath/MathShared.h"
 
@@ -21,13 +23,14 @@
  * @code{.cpp}
  * TrapezoidProfile::Constraints constraints{kMaxV, kMaxA};
  * double previousProfiledReference = initialReference;
+ * TrapezoidProfile profile{constraints};
  * @endcode
  *
  * Run on update:
  * @code{.cpp}
- * TrapezoidProfile profile{constraints, unprofiledReference,
- *                          previousProfiledReference};
- * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate);
+ * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate,
+ *                                               unprofiledReference,
+ *                                               previousProfiledReference);
  * @endcode
  *
  * where `unprofiledReference` is free to change between calls. Note that when
@@ -75,9 +78,22 @@
    * Construct a TrapezoidProfile.
    *
    * @param constraints The constraints on the profile, like maximum velocity.
+   */
+  TrapezoidProfile(Constraints constraints);  // NOLINT
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
    * @param goal        The desired state when the profile is complete.
    * @param initial     The initial state (usually the current state).
+   * @deprecated Pass the desired and current state into calculate instead of
+   * constructing a new TrapezoidProfile with the desired and current state
    */
+  WPI_DEPRECATED(
+      "Pass the desired and current state into calculate instead of "
+      "constructing a new TrapezoidProfile with the desired and current "
+      "state")
   TrapezoidProfile(Constraints constraints, State goal,
                    State initial = State{Distance_t{0}, Velocity_t{0}});
 
@@ -91,10 +107,26 @@
    * where the beginning of the profile was at time t = 0.
    *
    * @param t The time since the beginning of the profile.
+   * @deprecated Pass the desired and current state into calculate instead of
+   * constructing a new TrapezoidProfile with the desired and current state
    */
+  [[deprecated(
+      "Pass the desired and current state into calculate instead of "
+      "constructing a new TrapezoidProfile with the desired and current "
+      "state")]]
   State Calculate(units::second_t t) const;
 
   /**
+   * Calculate the correct position and velocity for the profile at a time t
+   * where the beginning of the profile was at time t = 0.
+   *
+   * @param t The time since the beginning of the profile.
+   * @param goal        The desired state when the profile is complete.
+   * @param current     The initial state (usually the current state).
+   */
+  State Calculate(units::second_t t, State goal, State current);
+
+  /**
    * Returns the time left until a target distance in the profile is reached.
    *
    * @param target The target distance.
@@ -141,8 +173,9 @@
   int m_direction;
 
   Constraints m_constraints;
-  State m_initial;
-  State m_goal;
+  State m_current;
+  State m_goal;   // TODO: remove
+  bool m_newAPI;  // TODO: remove
 
   units::second_t m_endAccel;
   units::second_t m_endFullSpeed;
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
index 19eb1f3..24e0a46 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
@@ -11,21 +11,26 @@
 
 namespace frc {
 template <class Distance>
+TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints)
+    : m_constraints(constraints), m_newAPI(true) {}
+
+template <class Distance>
 TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints,
                                              State goal, State initial)
     : m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
       m_constraints(constraints),
-      m_initial(Direct(initial)),
-      m_goal(Direct(goal)) {
-  if (m_initial.velocity > m_constraints.maxVelocity) {
-    m_initial.velocity = m_constraints.maxVelocity;
+      m_current(Direct(initial)),
+      m_goal(Direct(goal)),
+      m_newAPI(false) {
+  if (m_current.velocity > m_constraints.maxVelocity) {
+    m_current.velocity = m_constraints.maxVelocity;
   }
 
   // Deal with a possibly truncated motion profile (with nonzero initial or
   // final velocity) by calculating the parameters as if the profile began and
   // ended at zero velocity
   units::second_t cutoffBegin =
-      m_initial.velocity / m_constraints.maxAcceleration;
+      m_current.velocity / m_constraints.maxAcceleration;
   Distance_t cutoffDistBegin =
       cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
 
@@ -37,7 +42,7 @@
   // of a truncated one
 
   Distance_t fullTrapezoidDist =
-      cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
+      cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd;
   units::second_t accelerationTime =
       m_constraints.maxVelocity / m_constraints.maxAcceleration;
 
@@ -60,15 +65,19 @@
 template <class Distance>
 typename TrapezoidProfile<Distance>::State
 TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
-  State result = m_initial;
+  if (m_newAPI) {
+    throw std::runtime_error(
+        "Cannot use new constructor with deprecated Calculate()");
+  }
+  State result = m_current;
 
   if (t < m_endAccel) {
     result.velocity += t * m_constraints.maxAcceleration;
     result.position +=
-        (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+        (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
   } else if (t < m_endFullSpeed) {
     result.velocity = m_constraints.maxVelocity;
-    result.position += (m_initial.velocity +
+    result.position += (m_current.velocity +
                         m_endAccel * m_constraints.maxAcceleration / 2.0) *
                            m_endAccel +
                        m_constraints.maxVelocity * (t - m_endAccel);
@@ -86,12 +95,83 @@
 
   return Direct(result);
 }
+template <class Distance>
+typename TrapezoidProfile<Distance>::State
+TrapezoidProfile<Distance>::Calculate(units::second_t t, State goal,
+                                      State current) {
+  m_direction = ShouldFlipAcceleration(current, goal) ? -1 : 1;
+  m_current = Direct(current);
+  goal = Direct(goal);
+  if (m_current.velocity > m_constraints.maxVelocity) {
+    m_current.velocity = m_constraints.maxVelocity;
+  }
+
+  // Deal with a possibly truncated motion profile (with nonzero initial or
+  // final velocity) by calculating the parameters as if the profile began and
+  // ended at zero velocity
+  units::second_t cutoffBegin =
+      m_current.velocity / m_constraints.maxAcceleration;
+  Distance_t cutoffDistBegin =
+      cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+  units::second_t cutoffEnd = goal.velocity / m_constraints.maxAcceleration;
+  Distance_t cutoffDistEnd =
+      cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
+
+  // Now we can calculate the parameters as if it was a full trapezoid instead
+  // of a truncated one
+
+  Distance_t fullTrapezoidDist =
+      cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd;
+  units::second_t accelerationTime =
+      m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+  Distance_t fullSpeedDist =
+      fullTrapezoidDist -
+      accelerationTime * accelerationTime * m_constraints.maxAcceleration;
+
+  // Handle the case where the profile never reaches full speed
+  if (fullSpeedDist < Distance_t{0}) {
+    accelerationTime =
+        units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+    fullSpeedDist = Distance_t{0};
+  }
+
+  m_endAccel = accelerationTime - cutoffBegin;
+  m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+  m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+  State result = m_current;
+
+  if (t < m_endAccel) {
+    result.velocity += t * m_constraints.maxAcceleration;
+    result.position +=
+        (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+  } else if (t < m_endFullSpeed) {
+    result.velocity = m_constraints.maxVelocity;
+    result.position += (m_current.velocity +
+                        m_endAccel * m_constraints.maxAcceleration / 2.0) *
+                           m_endAccel +
+                       m_constraints.maxVelocity * (t - m_endAccel);
+  } else if (t <= m_endDeccel) {
+    result.velocity =
+        goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
+    units::second_t timeLeft = m_endDeccel - t;
+    result.position =
+        goal.position -
+        (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) *
+            timeLeft;
+  } else {
+    result = goal;
+  }
+
+  return Direct(result);
+}
 
 template <class Distance>
 units::second_t TrapezoidProfile<Distance>::TimeLeftUntil(
     Distance_t target) const {
-  Distance_t position = m_initial.position * m_direction;
-  Velocity_t velocity = m_initial.velocity * m_direction;
+  Distance_t position = m_current.position * m_direction;
+  Velocity_t velocity = m_current.velocity * m_direction;
 
   units::second_t endAccel = m_endAccel * m_direction;
   units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
index 0edd8cc..0d96893 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
@@ -20,9 +20,8 @@
 class WPILIB_DLLEXPORT DifferentialDriveKinematicsConstraint
     : public TrajectoryConstraint {
  public:
-  DifferentialDriveKinematicsConstraint(
-      const DifferentialDriveKinematics& kinematics,
-      units::meters_per_second_t maxSpeed);
+  DifferentialDriveKinematicsConstraint(DifferentialDriveKinematics kinematics,
+                                        units::meters_per_second_t maxSpeed);
 
   units::meters_per_second_t MaxVelocity(
       const Pose2d& pose, units::curvature_t curvature,
diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
index 40a0d8e..8f7ba2c 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
@@ -34,7 +34,7 @@
    */
   DifferentialDriveVoltageConstraint(
       const SimpleMotorFeedforward<units::meter>& feedforward,
-      const DifferentialDriveKinematics& kinematics, units::volt_t maxVoltage);
+      DifferentialDriveKinematics kinematics, units::volt_t maxVoltage);
 
   units::meters_per_second_t MaxVelocity(
       const Pose2d& pose, units::curvature_t curvature,
diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
index f9f0d2e..74f3c55 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <concepts>
 #include <limits>
 
 #include "frc/geometry/Rotation2d.h"
@@ -15,8 +16,7 @@
 /**
  * Enforces a particular constraint only within an elliptical region.
  */
-template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
-                                   TrajectoryConstraint, Constraint>>>
+template <std::derived_from<TrajectoryConstraint> Constraint>
 class EllipticalRegionConstraint : public TrajectoryConstraint {
  public:
   /**
diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
index 18522fe..f3b364b 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <concepts>
 #include <limits>
 
 #include "frc/geometry/Rotation2d.h"
@@ -14,8 +15,7 @@
 /**
  * Enforces a particular constraint only within a rectangular region.
  */
-template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
-                                   TrajectoryConstraint, Constraint>>>
+template <std::derived_from<TrajectoryConstraint> Constraint>
 class RectangularRegionConstraint : public TrajectoryConstraint {
  public:
   /**
diff --git a/wpimath/src/main/native/include/units/angular_acceleration.h b/wpimath/src/main/native/include/units/angular_acceleration.h
index 632982b..8174dac 100644
--- a/wpimath/src/main/native/include/units/angular_acceleration.h
+++ b/wpimath/src/main/native/include/units/angular_acceleration.h
@@ -4,7 +4,7 @@
 
 #pragma once
 
-#include "units/angular_velocity.h"
+#include "units/angle.h"
 #include "units/base.h"
 #include "units/time.h"
 
diff --git a/wpimath/src/main/native/include/units/angular_jerk.h b/wpimath/src/main/native/include/units/angular_jerk.h
new file mode 100644
index 0000000..b58bc16
--- /dev/null
+++ b/wpimath/src/main/native/include/units/angular_jerk.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 "units/angle.h"
+#include "units/base.h"
+#include "units/time.h"
+
+namespace units {
+/**
+ * @namespace units::angular_jerk
+ * @brief namespace for unit types and containers representing angular
+ *        jerk values
+ * @details The SI unit for angular jerk is
+ *          `radians_per_second_cubed`, and the corresponding `base_unit`
+ *          category is`angular_jerk_unit`.
+ * @anchor angularJerkContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+UNIT_ADD(angular_jerk, radians_per_second_cubed, radians_per_second_cubed,
+         rad_per_s_cu, unit<std::ratio<1>, units::category::angular_jerk_unit>)
+UNIT_ADD(angular_jerk, degrees_per_second_cubed, degrees_per_second_cubed,
+         deg_per_s_cu,
+         compound_unit<angle::degrees, inverse<cubed<time::seconds>>>)
+UNIT_ADD(angular_jerk, turns_per_second_cubed, turns_per_second_cubed,
+         tr_per_s_cu,
+         compound_unit<angle::turns, inverse<cubed<time::seconds>>>)
+
+UNIT_ADD_CATEGORY_TRAIT(angular_jerk)
+
+using namespace angular_jerk;
+}  // namespace units
diff --git a/wpimath/src/main/native/include/units/base.h b/wpimath/src/main/native/include/units/base.h
index 9d021b1..34cd59e 100644
--- a/wpimath/src/main/native/include/units/base.h
+++ b/wpimath/src/main/native/include/units/base.h
@@ -76,7 +76,7 @@
 	#include <locale>
 	#include <string>
 #endif
-#if !defined(UNIT_LIB_DISABLE_FMT)
+#if __has_include(<fmt/format.h>) && !defined(UNIT_LIB_DISABLE_FMT)
 	#include <locale>
 	#include <string>
 	#include <fmt/format.h>
@@ -176,7 +176,7 @@
  * @param		abbrev - abbreviated unit name, e.g. 'm'
  * @note		When UNIT_LIB_ENABLE_IOSTREAM isn't defined, the macro does not generate any code
  */
-#if !defined(UNIT_LIB_DISABLE_FMT)
+#if __has_include(<fmt/format.h>) && !defined(UNIT_LIB_DISABLE_FMT)
 	#define UNIT_ADD_IO(namespaceName, nameSingular, abbrev)\
 	}\
 	template <>\
@@ -463,13 +463,19 @@
 	//----------------------------------
 
 	/**
+	 * @defgroup 	Units Unit API
+	*/
+
+	/**
 	 * @defgroup	UnitContainers Unit Containers
+	 * @ingroup		Units
 	 * @brief		Defines a series of classes which contain dimensioned values. Unit containers
 	 *				store a value, and support various arithmetic operations.
 	 */
 
 	/**
 	 * @defgroup	UnitTypes Unit Types
+	 * @ingroup		Units
 	 * @brief		Defines a series of classes which represent units. These types are tags used by
 	 *				the conversion function, to create compound units, or to create `unit_t` types.
 	 *				By themselves, they are not containers and have no stored value.
@@ -477,6 +483,7 @@
 
 	/**
 	 * @defgroup	UnitManipulators Unit Manipulators
+	 * @ingroup		Units
 	 * @brief		Defines a series of classes used to manipulate unit types, such as `inverse<>`, `squared<>`, and metric prefixes.
 	 *				Unit manipulators can be chained together, e.g. `inverse<squared<pico<time::seconds>>>` to
 	 *				represent picoseconds^-2.
@@ -484,6 +491,7 @@
 
 	 /**
 	  * @defgroup	CompileTimeUnitManipulators Compile-time Unit Manipulators
+	  * @ingroup	Units
 	  * @brief		Defines a series of classes used to manipulate `unit_value_t` types at compile-time, such as `unit_value_add<>`, `unit_value_sqrt<>`, etc.
 	  *				Compile-time manipulators can be chained together, e.g. `unit_value_sqrt<unit_value_add<unit_value_power<a, 2>, unit_value_power<b, 2>>>` to
 	  *				represent `c = sqrt(a^2 + b^2).
@@ -491,17 +499,20 @@
 
 	 /**
 	 * @defgroup	UnitMath Unit Math
+	 * @ingroup		Units
 	 * @brief		Defines a collection of unit-enabled, strongly-typed versions of `<cmath>` functions.
 	 * @details		Includes most c++11 extensions.
 	 */
 
 	/**
 	 * @defgroup	Conversion Explicit Conversion
+	 * @ingroup		Units
 	 * @brief		Functions used to convert values of one logical type to another.
 	 */
 
 	/**
 	 * @defgroup	TypeTraits Type Traits
+	 * @ingroup		Units
 	 * @brief		Defines a series of classes to obtain unit type information at compile-time.
 	 */
 
@@ -810,6 +821,7 @@
 		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<-1>,	std::ratio<1>>																						angular_velocity_unit;			///< Represents an SI derived unit of angular velocity
 		typedef base_unit<detail::meter_ratio<1>,	std::ratio<0>,	std::ratio<-2>>																										acceleration_unit;				///< Represents an SI derived unit of acceleration
 		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<-2>,	std::ratio<1>>																						angular_acceleration_unit;			///< Represents an SI derived unit of angular acceleration
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<-3>,	std::ratio<1>>																						angular_jerk_unit;			    ///< Represents an SI derived unit of angular jerk
 		typedef base_unit<detail::meter_ratio<1>,	std::ratio<1>,	std::ratio<-2>>																										force_unit;						///< Represents an SI derived unit of force
 		typedef base_unit<detail::meter_ratio<-1>,	std::ratio<1>,	std::ratio<-2>>																										pressure_unit;					///< Represents an SI derived unit of pressure
 		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<1>,	std::ratio<0>,	std::ratio<1>>																		charge_unit;					///< Represents an SI derived unit of charge
@@ -1394,7 +1406,7 @@
 	 *					error. This value should be chosen to be as high as possible before
 	 *					integer overflow errors occur in the compiler.
 	 * @note		USE WITH CAUTION. The is an approximate value. In general, squared<sqrt<meter>> != meter,
-	 *				i.e. the operation is not reversible, and it will result in propogated approximations.
+	 *				i.e. the operation is not reversible, and it will result in propagated approximations.
 	 *				Use only when absolutely necessary.
 	 */
 	template<class U, std::intmax_t Eps = 10000000000>
@@ -1758,7 +1770,7 @@
 #ifdef FOR_DOXYGEN_PURPOSOES_ONLY
 		/**
 		* @ingroup		TypeTraits
-		* @brief		Trait for accessing the publically defined types of `units::unit_t`
+		* @brief		Trait for accessing the publicly defined types of `units::unit_t`
 		* @details		The units library determines certain properties of the unit_t types passed to them
 		*				and what they represent by using the members of the corresponding unit_t_traits instantiation.
 		*/
@@ -1788,7 +1800,7 @@
 
 		/**
 		 * @ingroup		TypeTraits
-		 * @brief		Trait for accessing the publically defined types of `units::unit_t`
+		 * @brief		Trait for accessing the publicly defined types of `units::unit_t`
 		 * @details
 		 */
 		template<typename T>
@@ -2875,7 +2887,7 @@
 	}
 #endif
 }
-#if !defined(UNIT_LIB_DISABLE_FMT)
+#if __has_include(<fmt/format.h>) && !defined(UNIT_LIB_DISABLE_FMT)
 template <>
 struct fmt::formatter<units::dimensionless::dB_t> : fmt::formatter<double>
 {
@@ -2972,7 +2984,7 @@
 #ifdef FOR_DOXYGEN_PURPOSES_ONLY
 		/**
 		* @ingroup		TypeTraits
-		* @brief		Trait for accessing the publically defined types of `units::unit_value_t_traits`
+		* @brief		Trait for accessing the publicly defined types of `units::unit_value_t_traits`
 		* @details		The units library determines certain properties of the `unit_value_t` types passed to
 		*				them and what they represent by using the members of the corresponding `unit_value_t_traits`
 		*				instantiation.
@@ -2999,7 +3011,7 @@
 
 		/**
 		 * @ingroup		TypeTraits
-		 * @brief		Trait for accessing the publically defined types of `units::unit_value_t_traits`
+		 * @brief		Trait for accessing the publicly defined types of `units::unit_value_t_traits`
 		 * @details
 		 */
 		template<typename T>
@@ -3440,6 +3452,6 @@
 using namespace units::literals;
 #endif  // UNIT_HAS_LITERAL_SUPPORT
 
-#if !defined(UNIT_LIB_DISABLE_FMT)
-#include "frc/fmt/Units.h"
+#if __has_include(<fmt/format.h>) && !defined(UNIT_LIB_DISABLE_FMT)
+#include "units/formatter.h"
 #endif
diff --git a/wpimath/src/main/native/include/frc/fmt/Units.h b/wpimath/src/main/native/include/units/formatter.h
similarity index 88%
rename from wpimath/src/main/native/include/frc/fmt/Units.h
rename to wpimath/src/main/native/include/units/formatter.h
index 1ec61ca..1c17b0a 100644
--- a/wpimath/src/main/native/include/frc/fmt/Units.h
+++ b/wpimath/src/main/native/include/units/formatter.h
@@ -4,43 +4,41 @@
 
 #pragma once
 
+#include <type_traits>
+
 #include <fmt/format.h>
 
 #include "units/base.h"
 
+// FIXME: Replace enable_if with requires clause and remove <type_traits>
+// include once using GCC >= 12. GCC 11 incorrectly emits a struct redefinition
+// error because it doesn't use the requires clause to disambiguate.
+
 /**
  * Formatter for unit types.
- *
- * @tparam Units Unit tag for which type of units the `unit_t` represents (e.g.
- *               meters).
- * @tparam T Underlying type of the storage. Defaults to double.
- * @tparam NonLinearScale Optional scale class for the units. Defaults to linear
- *                        (i.e. does not scale the unit value). Examples of
- *                        non-linear scales could be logarithmic, decibel, or
- *                        richter scales. Non-linear scales must adhere to the
- *                        non-linear-scale concept.
  */
-template <class Units, typename T, template <typename> class NonLinearScale>
-struct fmt::formatter<units::unit_t<Units, T, NonLinearScale>>
-    : fmt::formatter<double> {
+template <typename Unit, typename CharT>
+struct fmt::formatter<Unit, CharT,
+                      std::enable_if_t<units::traits::is_unit_t_v<Unit>>> {
+  constexpr auto parse(fmt::format_parse_context& ctx) {
+    return m_underlying.parse(ctx);
+  }
+
   /**
    * Writes out a formatted unit.
    *
-   * @tparam FormatContext Format string context type.
    * @param obj Unit instance.
    * @param ctx Format string context.
    */
-  template <typename FormatContext>
-  auto format(const units::unit_t<Units, T, NonLinearScale>& obj,
-              FormatContext& ctx) {
+  auto format(const Unit& obj, fmt::format_context& ctx) const {
+    using Units = typename Unit::unit_type;
     using BaseUnits =
         units::unit<std::ratio<1>,
                     typename units::traits::unit_traits<Units>::base_unit_type>;
 
     auto out = ctx.out();
 
-    out = fmt::formatter<double>::format(
-        units::convert<Units, BaseUnits>(obj()), ctx);
+    out = m_underlying.format(units::convert<Units, BaseUnits>(obj()), ctx);
 
     if constexpr (units::traits::unit_traits<
                       Units>::base_unit_type::meter_ratio::num != 0) {
@@ -215,4 +213,7 @@
 
     return out;
   }
+
+ private:
+  fmt::formatter<typename Unit::underlying_type, CharT> m_underlying;
 };
diff --git a/wpimath/src/main/native/include/wpimath/MathShared.h b/wpimath/src/main/native/include/wpimath/MathShared.h
index 3b9b3cd..8e0698d 100644
--- a/wpimath/src/main/native/include/wpimath/MathShared.h
+++ b/wpimath/src/main/native/include/wpimath/MathShared.h
@@ -9,6 +9,8 @@
 #include <fmt/format.h>
 #include <wpi/SymbolExports.h>
 
+#include "units/time.h"
+
 namespace wpi::math {
 
 enum class MathUsageId {
@@ -31,6 +33,7 @@
   virtual void ReportWarningV(fmt::string_view format,
                               fmt::format_args args) = 0;
   virtual void ReportUsage(MathUsageId id, int count) = 0;
+  virtual units::second_t GetTimestamp() = 0;
 
   template <typename S, typename... Args>
   inline void ReportError(const S& format, Args&&... args) {
@@ -70,6 +73,10 @@
   static void ReportUsage(MathUsageId id, int count) {
     GetMathShared().ReportUsage(id, count);
   }
+
+  static units::second_t GetTimestamp() {
+    return GetMathShared().GetTimestamp();
+  }
 };
 
 }  // namespace wpi::math
diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h
deleted file mode 100644
index 47097ed..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h
+++ /dev/null
@@ -1,162 +0,0 @@
-#pragma once
-
-#include <type_traits>
-
-/// @file
-/// Provides Drake's assertion implementation.  This is intended to be used
-/// both within Drake and by other software.  Drake's asserts can be armed
-/// and disarmed independently from the system-wide asserts.
-
-#ifdef DRAKE_DOXYGEN_CXX
-/// @p DRAKE_ASSERT(condition) is similar to the built-in @p assert(condition)
-/// from the C++ system header @p <cassert>.  Unless Drake's assertions are
-/// disarmed by the pre-processor definitions listed below, @p DRAKE_ASSERT
-/// will evaluate @p condition and iff the value is false will trigger an
-/// assertion failure with a message showing at least the condition text,
-/// function name, file, and line.
-///
-/// By default, assertion failures will :abort() the program.  However, when
-/// using the pydrake python bindings, assertion failures will instead throw a
-/// C++ exception that causes a python SystemExit exception.
-///
-/// Assertions are enabled or disabled using the following pre-processor macros:
-///
-/// - If @p DRAKE_ENABLE_ASSERTS is defined, then @p DRAKE_ASSERT is armed.
-/// - If @p DRAKE_DISABLE_ASSERTS is defined, then @p DRAKE_ASSERT is disarmed.
-/// - If both macros are defined, then it is a compile-time error.
-/// - If neither are defined, then NDEBUG governs assertions as usual.
-///
-/// This header will define exactly one of either @p DRAKE_ASSERT_IS_ARMED or
-/// @p DRAKE_ASSERT_IS_DISARMED to indicate whether @p DRAKE_ASSERT is armed.
-///
-/// This header will define both `constexpr bool drake::kDrakeAssertIsArmed`
-/// and `constexpr bool drake::kDrakeAssertIsDisarmed` globals.
-///
-/// One difference versus the standard @p assert(condition) is that the
-/// @p condition within @p DRAKE_ASSERT is always syntax-checked, even if
-/// Drake's assertions are disarmed.
-///
-/// Treat @p DRAKE_ASSERT like a statement -- it must always be used
-/// in block scope, and must always be followed by a semicolon.
-#define DRAKE_ASSERT(condition)
-/// Like @p DRAKE_ASSERT, except that the expression must be void-valued; this
-/// allows for guarding expensive assertion-checking subroutines using the same
-/// macros as stand-alone assertions.
-#define DRAKE_ASSERT_VOID(expression)
-/// Evaluates @p condition and iff the value is false will trigger an assertion
-/// failure with a message showing at least the condition text, function name,
-/// file, and line.
-#define DRAKE_DEMAND(condition)
-/// Silences a "no return value" compiler warning by calling a function that
-/// always raises an exception or aborts (i.e., a function marked noreturn).
-/// Only use this macro at a point where (1) a point in the code is truly
-/// unreachable, (2) the fact that it's unreachable is knowable from only
-/// reading the function itself (and not, e.g., some larger design invariant),
-/// and (3) there is a compiler warning if this macro were removed.  The most
-/// common valid use is with a switch-case-return block where all cases are
-/// accounted for but the enclosing function is supposed to return a value.  Do
-/// *not* use this macro as a "logic error" assertion; it should *only* be used
-/// to silence false positive warnings.  When in doubt, throw an exception
-/// manually instead of using this macro.
-#define DRAKE_UNREACHABLE()
-#else  //  DRAKE_DOXYGEN_CXX
-
-// Users should NOT set these; only this header should set them.
-#ifdef DRAKE_ASSERT_IS_ARMED
-# error Unexpected DRAKE_ASSERT_IS_ARMED defined.
-#endif
-#ifdef DRAKE_ASSERT_IS_DISARMED
-# error Unexpected DRAKE_ASSERT_IS_DISARMED defined.
-#endif
-
-// Decide whether Drake assertions are enabled.
-#if defined(DRAKE_ENABLE_ASSERTS) && defined(DRAKE_DISABLE_ASSERTS)
-# error Conflicting assertion toggles.
-#elif defined(DRAKE_ENABLE_ASSERTS)
-# define DRAKE_ASSERT_IS_ARMED
-#elif defined(DRAKE_DISABLE_ASSERTS) || defined(NDEBUG)
-# define DRAKE_ASSERT_IS_DISARMED
-#else
-# define DRAKE_ASSERT_IS_ARMED
-#endif
-
-namespace drake {
-namespace internal {
-// Abort the program with an error message.
-[[noreturn]] void Abort(const char* condition, const char* func,
-                        const char* file, int line);
-// Report an assertion failure; will either Abort(...) or throw.
-[[noreturn]] void AssertionFailed(const char* condition, const char* func,
-                                  const char* file, int line);
-}  // namespace internal
-namespace assert {
-// Allows for specialization of how to bool-convert Conditions used in
-// assertions, in case they are not intrinsically convertible.  See
-// common/symbolic/expression/formula.h for an example use.  This is a public
-// interface to extend; it is intended to be specialized by unusual Scalar
-// types that require special handling.
-template <typename Condition>
-struct ConditionTraits {
-  static constexpr bool is_valid = std::is_convertible_v<Condition, bool>;
-  static bool Evaluate(const Condition& value) {
-    return value;
-  }
-};
-}  // namespace assert
-}  // namespace drake
-
-#define DRAKE_UNREACHABLE()                                             \
-  ::drake::internal::Abort(                                             \
-      "Unreachable code was reached?!", __func__, __FILE__, __LINE__)
-
-#define DRAKE_DEMAND(condition)                                              \
-  do {                                                                       \
-    typedef ::drake::assert::ConditionTraits<                                \
-        typename std::remove_cv_t<decltype(condition)>> Trait;               \
-    static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
-    static_assert(                                                           \
-        !std::is_pointer_v<decltype(condition)>,                             \
-        "When using DRAKE_DEMAND on a raw pointer, always write out "        \
-        "DRAKE_DEMAND(foo != nullptr), do not write DRAKE_DEMAND(foo) "      \
-        "and rely on implicit pointer-to-bool conversion.");                 \
-    if (!Trait::Evaluate(condition)) {                                       \
-      ::drake::internal::AssertionFailed(                                    \
-           #condition, __func__, __FILE__, __LINE__);                        \
-    }                                                                        \
-  } while (0)
-
-#ifdef DRAKE_ASSERT_IS_ARMED
-// Assertions are enabled.
-namespace drake {
-constexpr bool kDrakeAssertIsArmed = true;
-constexpr bool kDrakeAssertIsDisarmed = false;
-}  // namespace drake
-# define DRAKE_ASSERT(condition) DRAKE_DEMAND(condition)
-# define DRAKE_ASSERT_VOID(expression) do {                     \
-    static_assert(                                              \
-        std::is_convertible_v<decltype(expression), void>,      \
-        "Expression should be void.");                          \
-    expression;                                                 \
-  } while (0)
-#else
-// Assertions are disabled, so just typecheck the expression.
-namespace drake {
-constexpr bool kDrakeAssertIsArmed = false;
-constexpr bool kDrakeAssertIsDisarmed = true;
-}  // namespace drake
-# define DRAKE_ASSERT(condition) do {                                        \
-    typedef ::drake::assert::ConditionTraits<                                \
-        typename std::remove_cv_t<decltype(condition)>> Trait;               \
-    static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
-    static_assert(                                                           \
-        !std::is_pointer_v<decltype(condition)>,                             \
-        "When using DRAKE_ASSERT on a raw pointer, always write out "        \
-        "DRAKE_ASSERT(foo != nullptr), do not write DRAKE_ASSERT(foo) "      \
-        "and rely on implicit pointer-to-bool conversion.");                 \
-  } while (0)
-# define DRAKE_ASSERT_VOID(expression) static_assert(           \
-    std::is_convertible_v<decltype(expression), void>,          \
-    "Expression should be void.")
-#endif
-
-#endif  // DRAKE_DOXYGEN_CXX
diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h
deleted file mode 100644
index b428474..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#pragma once
-
-#include <stdexcept>
-#include <string>
-
-namespace drake {
-namespace internal {
-
-// This is what DRAKE_ASSERT and DRAKE_DEMAND throw when our assertions are
-// configured to throw.
-class assertion_error : public std::runtime_error {
- public:
-  explicit assertion_error(const std::string& what_arg)
-      : std::runtime_error(what_arg) {}
-};
-
-}  // namespace internal
-}  // namespace drake
diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h
deleted file mode 100644
index a96a6fb..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h
+++ /dev/null
@@ -1,90 +0,0 @@
-#pragma once
-
-// ============================================================================
-// N.B. The spelling of the macro names between doc/Doxyfile_CXX.in and this
-// file must be kept in sync!
-// ============================================================================
-
-/** @file
-Provides careful macros to selectively enable or disable the special member
-functions for copy-construction, copy-assignment, move-construction, and
-move-assignment.
-
-http://en.cppreference.com/w/cpp/language/member_functions#Special_member_functions
-
-When enabled via these macros, the `= default` implementation is provided.
-Code that needs custom copy or move functions should not use these macros.
-*/
-
-/** DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for
-copy-construction, copy-assignment, move-construction, and move-assignment.
-Drake's Doxygen is customized to render the deletions in detail, with
-appropriate comments.  Invoke this macro in the public section of the class
-declaration, e.g.:
-<pre>
-class Foo {
- public:
-  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Foo)
-
-  // ...
-};
-</pre>
-*/
-#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)      \
-  Classname(const Classname&) = delete;                 \
-  void operator=(const Classname&) = delete;            \
-  Classname(Classname&&) = delete;                      \
-  void operator=(Classname&&) = delete;
-
-/** DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member
-functions for copy-construction, copy-assignment, move-construction, and
-move-assignment.  This macro should be used only when copy-construction and
-copy-assignment defaults are well-formed.  Note that the defaulted move
-functions could conceivably still be ill-formed, in which case they will
-effectively not be declared or used -- but because the copy constructor exists
-the type will still be MoveConstructible.  Drake's Doxygen is customized to
-render the functions in detail, with appropriate comments.  Typically, you
-should invoke this macro in the public section of the class declaration, e.g.:
-<pre>
-class Foo {
- public:
-  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Foo)
-
-  // ...
-};
-</pre>
-
-However, if Foo has a virtual destructor (i.e., is subclassable), then
-typically you should use either DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN in the
-public section or else DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN in the
-protected section, to prevent
-<a href="https://en.wikipedia.org/wiki/Object_slicing">object slicing</a>.
-
-The macro contains a built-in self-check that copy-assignment is well-formed.
-This self-check proves that the Classname is CopyConstructible, CopyAssignable,
-MoveConstructible, and MoveAssignable (in all but the most arcane cases where a
-member of the Classname is somehow CopyAssignable but not CopyConstructible).
-Therefore, classes that use this macro typically will not need to have any unit
-tests that check for the presence nor correctness of these functions.
-
-However, the self-check does not provide any checks of the runtime efficiency
-of the functions.  If it is important for performance that the move functions
-actually move (instead of making a copy), then you should consider capturing
-that in a unit test.
-*/
-#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname)       \
-  Classname(const Classname&) = default;                        \
-  Classname& operator=(const Classname&) = default;             \
-  Classname(Classname&&) = default;                             \
-  Classname& operator=(Classname&&) = default;                  \
-  /* Fails at compile-time if copy-assign doesn't compile. */   \
-  /* Note that we do not test the copy-ctor here, because  */   \
-  /* it will not exist when Classname is abstract.         */   \
-  static void DrakeDefaultCopyAndMoveAndAssign_DoAssign(        \
-      Classname* a, const Classname& b) { *a = b; }             \
-  static_assert(                                                \
-      &DrakeDefaultCopyAndMoveAndAssign_DoAssign ==             \
-      &DrakeDefaultCopyAndMoveAndAssign_DoAssign,               \
-      "This assertion is never false; its only purpose is to "  \
-      "generate 'use of deleted function: operator=' errors "   \
-      "when Classname is a template.");
diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h
deleted file mode 100644
index fdd07a2..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h
+++ /dev/null
@@ -1,47 +0,0 @@
-#pragma once
-
-#include <type_traits>
-
-#include "drake/common/drake_assert.h"
-
-/// @file
-/// Provides a convenient wrapper to throw an exception when a condition is
-/// unmet.  This is similar to an assertion, but uses exceptions instead of
-/// ::abort(), and cannot be disabled.
-
-namespace drake {
-namespace internal {
-// Throw an error message.
-[[noreturn]] void Throw(const char* condition, const char* func,
-                        const char* file, int line);
-}  // namespace internal
-}  // namespace drake
-
-/// Evaluates @p condition and iff the value is false will throw an exception
-/// with a message showing at least the condition text, function name, file,
-/// and line.
-///
-/// The condition must not be a pointer, where we'd implicitly rely on its
-/// nullness. Instead, always write out "!= nullptr" to be precise.
-///
-/// Correct: `DRAKE_THROW_UNLESS(foo != nullptr);`
-/// Incorrect: `DRAKE_THROW_UNLESS(foo);`
-///
-/// Because this macro is intended to provide a useful exception message to
-/// users, we should err on the side of extra detail about the failure. The
-/// meaning of "foo" isolated within error message text does not make it
-/// clear that a null pointer is the proximate cause of the problem.
-#define DRAKE_THROW_UNLESS(condition)                                         \
-  do {                                                                        \
-    typedef ::drake::assert::ConditionTraits<                                 \
-        typename std::remove_cv_t<decltype(condition)>> Trait;                \
-    static_assert(Trait::is_valid, "Condition should be bool-convertible.");  \
-    static_assert(                                                            \
-        !std::is_pointer_v<decltype(condition)>,                              \
-        "When using DRAKE_THROW_UNLESS on a raw pointer, always write out "   \
-        "DRAKE_THROW_UNLESS(foo != nullptr), do not write DRAKE_THROW_UNLESS" \
-        "(foo) and rely on implicit pointer-to-bool conversion.");            \
-    if (!Trait::Evaluate(condition)) {                                        \
-      ::drake::internal::Throw(#condition, __func__, __FILE__, __LINE__);     \
-    }                                                                         \
-  } while (0)
diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h
deleted file mode 100644
index b3f369c..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h
+++ /dev/null
@@ -1,62 +0,0 @@
-#pragma once
-
-#include <vector>
-
-#include <Eigen/Core>
-
-namespace drake {
-
-/// Returns true if and only if the two matrices are equal to within a certain
-/// absolute elementwise @p tolerance.  Special values (infinities, NaN, etc.)
-/// do not compare as equal elements.
-template <typename DerivedA, typename DerivedB>
-bool is_approx_equal_abstol(const Eigen::MatrixBase<DerivedA>& m1,
-                            const Eigen::MatrixBase<DerivedB>& m2,
-                            double tolerance) {
-  return (
-      (m1.rows() == m2.rows()) &&
-      (m1.cols() == m2.cols()) &&
-      ((m1 - m2).template lpNorm<Eigen::Infinity>() <= tolerance));
-}
-
-/// Returns true if and only if a simple greedy search reveals a permutation
-/// of the columns of m2 to make the matrix equal to m1 to within a certain
-/// absolute elementwise @p tolerance. E.g., there exists a P such that
-/// <pre>
-///    forall i,j,  |m1 - m2*P|_{i,j} <= tolerance
-///    where P is a permutation matrix:
-///       P(i,j)={0,1}, sum_i P(i,j)=1, sum_j P(i,j)=1.
-/// </pre>
-/// Note: Returns false for matrices of different sizes.
-/// Note: The current implementation is O(n^2) in the number of columns.
-/// Note: In marginal cases (with similar but not identical columns) this
-/// algorithm can fail to find a permutation P even if it exists because it
-/// accepts the first column match (m1(i),m2(j)) and removes m2(j) from the
-/// pool. It is possible that other columns of m2 would also match m1(i) but
-/// that m2(j) is the only match possible for a later column of m1.
-template <typename DerivedA, typename DerivedB>
-bool IsApproxEqualAbsTolWithPermutedColumns(
-    const Eigen::MatrixBase<DerivedA>& m1,
-    const Eigen::MatrixBase<DerivedB>& m2, double tolerance) {
-  if ((m1.cols() != m2.cols()) || (m1.rows() != m2.rows())) return false;
-
-  std::vector<bool> available(m2.cols());
-  for (int i = 0; i < m2.cols(); i++) available[i] = true;
-
-  for (int i = 0; i < m1.cols(); i++) {
-    bool found_match = false;
-    for (int j = 0; j < m2.cols(); j++) {
-      if (available[j] &&
-          is_approx_equal_abstol(m1.col(i), m2.col(j), tolerance)) {
-        found_match = true;
-        available[j] = false;
-        break;
-      }
-    }
-    if (!found_match) return false;
-  }
-  return true;
-}
-
-
-}  // namespace drake
diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h
deleted file mode 100644
index 024b355..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h
+++ /dev/null
@@ -1,103 +0,0 @@
-#pragma once
-
-#include <new>
-#include <type_traits>
-#include <utility>
-
-#include "drake/common/drake_copyable.h"
-
-namespace drake {
-
-/// Wraps an underlying type T such that its storage is a direct member field
-/// of this object (i.e., without any indirection into the heap), but *unlike*
-/// most member fields T's destructor is never invoked.
-///
-/// This is especially useful for function-local static variables that are not
-/// trivially destructable.  We shouldn't call their destructor at program exit
-/// because of the "indeterminate order of ... destruction" as mentioned in
-/// cppguide's
-/// <a href="https://drake.mit.edu/styleguide/cppguide.html#Static_and_Global_Variables">Static
-/// and Global Variables</a> section, but other solutions to this problem place
-///  the objects on the heap through an indirection.
-///
-/// Compared with other approaches, this mechanism more clearly describes the
-/// intent to readers, avoids "possible leak" warnings from memory-checking
-/// tools, and is probably slightly faster.
-///
-/// Example uses:
-///
-/// The singleton pattern:
-/// @code
-/// class Singleton {
-///  public:
-///   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Singleton)
-///   static Singleton& getInstance() {
-///     static never_destroyed<Singleton> instance;
-///     return instance.access();
-///   }
-///  private:
-///   friend never_destroyed<Singleton>;
-///   Singleton() = default;
-/// };
-/// @endcode
-///
-/// A lookup table, created on demand the first time its needed, and then
-/// reused thereafter:
-/// @code
-/// enum class Foo { kBar, kBaz };
-/// Foo ParseFoo(const std::string& foo_string) {
-///   using Dict = std::unordered_map<std::string, Foo>;
-///   static const drake::never_destroyed<Dict> string_to_enum{
-///     std::initializer_list<Dict::value_type>{
-///       {"bar", Foo::kBar},
-///       {"baz", Foo::kBaz},
-///     }
-///   };
-///   return string_to_enum.access().at(foo_string);
-/// }
-/// @endcode
-///
-/// In cases where computing the static data is more complicated than an
-/// initializer_list, you can use a temporary lambda to populate the value:
-/// @code
-/// const std::vector<double>& GetConstantMagicNumbers() {
-///   static const drake::never_destroyed<std::vector<double>> result{[]() {
-///     std::vector<double> prototype;
-///     std::mt19937 random_generator;
-///     for (int i = 0; i < 10; ++i) {
-///       double new_value = random_generator();
-///       prototype.push_back(new_value);
-///     }
-///     return prototype;
-///   }()};
-///   return result.access();
-/// }
-/// @endcode
-///
-/// Note in particular the `()` after the lambda. That causes it to be invoked.
-//
-// The above examples are repeated in the unit test; keep them in sync.
-template <typename T>
-class never_destroyed {
- public:
-  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(never_destroyed)
-
-  /// Passes the constructor arguments along to T using perfect forwarding.
-  template <typename... Args>
-  explicit never_destroyed(Args&&... args) {
-    // Uses "placement new" to construct a `T` in `storage_`.
-    new (&storage_) T(std::forward<Args>(args)...);
-  }
-
-  /// Does nothing.  Guaranteed!
-  ~never_destroyed() = default;
-
-  /// Returns the underlying T reference.
-  T& access() { return *reinterpret_cast<T*>(&storage_); }
-  const T& access() const { return *reinterpret_cast<const T*>(&storage_); }
-
- private:
-  typename std::aligned_storage<sizeof(T), alignof(T)>::type storage_;
-};
-
-}  // namespace drake
diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h b/wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h
deleted file mode 100644
index 55b8442..0000000
--- a/wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h
+++ /dev/null
@@ -1,85 +0,0 @@
-#pragma once
-
-#include <cmath>
-#include <cstdlib>
-
-#include <Eigen/Core>
-#include <wpi/SymbolExports.h>
-
-namespace drake {
-namespace math {
-
-/**
-Computes the unique stabilizing solution X to the discrete-time algebraic
-Riccati equation:
-
-AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
-
-@throws std::exception if Q is not positive semi-definite.
-@throws std::exception if R is not positive definite.
-
-Based on the Schur Vector approach outlined in this paper:
-"On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
-by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell
-*/
-WPILIB_DLLEXPORT
-Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
-    const Eigen::Ref<const Eigen::MatrixXd>& A,
-    const Eigen::Ref<const Eigen::MatrixXd>& B,
-    const Eigen::Ref<const Eigen::MatrixXd>& Q,
-    const Eigen::Ref<const Eigen::MatrixXd>& R);
-
-/**
-Computes the unique stabilizing solution X to the discrete-time algebraic
-Riccati equation:
-
-AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
-
-This is equivalent to solving the original DARE:
-
-A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
-
-where A₂ and Q₂ are a change of variables:
-
-A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
-
-This overload of the DARE is useful for finding the control law uₖ that
-minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
-
-@verbatim
-    ∞ [xₖ]ᵀ[Q  N][xₖ]
-J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
-   k=0
-@endverbatim
-
-This is a more general form of the following. The linear-quadratic regulator
-is the feedback control law uₖ that minimizes the following cost function
-subject to xₖ₊₁ = Axₖ + Buₖ:
-
-@verbatim
-    ∞
-J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
-   k=0
-@endverbatim
-
-This can be refactored as:
-
-@verbatim
-    ∞ [xₖ]ᵀ[Q 0][xₖ]
-J = Σ [uₖ] [0 R][uₖ] ΔT
-   k=0
-@endverbatim
-
-@throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite.
-@throws std::runtime_error if R is not positive definite.
-*/
-WPILIB_DLLEXPORT
-Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
-    const Eigen::Ref<const Eigen::MatrixXd>& A,
-    const Eigen::Ref<const Eigen::MatrixXd>& B,
-    const Eigen::Ref<const Eigen::MatrixXd>& Q,
-    const Eigen::Ref<const Eigen::MatrixXd>& R,
-    const Eigen::Ref<const Eigen::MatrixXd>& N);
-}  // namespace math
-}  // namespace drake
-
diff --git a/wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp b/wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp
deleted file mode 100644
index 88e7e66..0000000
--- a/wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp
+++ /dev/null
@@ -1,87 +0,0 @@
-// This file contains the implementation of both drake_assert and drake_throw.
-/* clang-format off to disable clang-format-includes */
-#include "drake/common/drake_assert.h"
-#include "drake/common/drake_throw.h"
-/* clang-format on */
-
-#include <atomic>
-#include <cstdlib>
-#include <iostream>
-#include <sstream>
-#include <stdexcept>
-#include <string>
-
-#include "drake/common/drake_assertion_error.h"
-#include "drake/common/never_destroyed.h"
-
-namespace drake {
-namespace internal {
-namespace {
-
-// Singleton to manage assertion configuration.
-struct AssertionConfig {
-  static AssertionConfig& singleton() {
-    static never_destroyed<AssertionConfig> global;
-    return global.access();
-  }
-
-  std::atomic<bool> assertion_failures_are_exceptions;
-};
-
-// Stream into @p out the given failure details; only @p condition may be null.
-void PrintFailureDetailTo(std::ostream& out, const char* condition,
-                          const char* func, const char* file, int line) {
-  out << "Failure at " << file << ":" << line << " in " << func << "()";
-  if (condition) {
-    out << ": condition '" << condition << "' failed.";
-  } else {
-    out << ".";
-  }
-}
-}  // namespace
-
-// Declared in drake_assert.h.
-void Abort(const char* condition, const char* func, const char* file,
-           int line) {
-  std::cerr << "abort: ";
-  PrintFailureDetailTo(std::cerr, condition, func, file, line);
-  std::cerr << std::endl;
-  std::abort();
-}
-
-// Declared in drake_throw.h.
-void Throw(const char* condition, const char* func, const char* file,
-           int line) {
-  std::ostringstream what;
-  PrintFailureDetailTo(what, condition, func, file, line);
-  throw assertion_error(what.str().c_str());
-}
-
-// Declared in drake_assert.h.
-void AssertionFailed(const char* condition, const char* func, const char* file,
-                     int line) {
-  if (AssertionConfig::singleton().assertion_failures_are_exceptions) {
-    Throw(condition, func, file, line);
-  } else {
-    Abort(condition, func, file, line);
-  }
-}
-
-}  // namespace internal
-}  // namespace drake
-
-// Configures the DRAKE_ASSERT and DRAKE_DEMAND assertion failure handling
-// behavior.
-//
-// By default, assertion failures will result in an ::abort().  If this method
-// has ever been called, failures will result in a thrown exception instead.
-//
-// Assertion configuration has process-wide scope.  Changes here will affect
-// all assertions within the current process.
-//
-// This method is intended ONLY for use by pydrake bindings, and thus is not
-// declared in any header file, to discourage anyone from using it.
-extern "C" void drake_set_assertion_failure_to_throw_exception() {
-  drake::internal::AssertionConfig::singleton().
-      assertion_failures_are_exceptions = true;
-}
diff --git a/wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp b/wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp
deleted file mode 100644
index 20ea2b7..0000000
--- a/wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp
+++ /dev/null
@@ -1,475 +0,0 @@
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-
-#include <Eigen/Eigenvalues>
-#include <Eigen/QR>
-
-#include "drake/common/drake_assert.h"
-#include "drake/common/drake_throw.h"
-#include "drake/common/is_approx_equal_abstol.h"
-
-namespace drake {
-namespace math {
-namespace {
-/* helper functions */
-template <typename T>
-int sgn(T val) {
-  return (T(0) < val) - (val < T(0));
-}
-void check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
-                        const Eigen::Ref<const Eigen::MatrixXd>& B) {
-  // This function checks if (A,B) is a stabilizable pair.
-  // (A,B) is stabilizable if and only if the uncontrollable eigenvalues of
-  // A, if any, have absolute values less than one, where an eigenvalue is
-  // uncontrollable if Rank[lambda * I - A, B] < n.
-  int n = B.rows(), m = B.cols();
-  Eigen::EigenSolver<Eigen::MatrixXd> es(A);
-  for (int i = 0; i < n; i++) {
-    if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
-            es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
-        1)
-      continue;
-    Eigen::MatrixXcd E(n, n + m);
-    E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(n, n) - A, B;
-    Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr(E);
-    DRAKE_THROW_UNLESS(qr.rank() == n);
-  }
-}
-void check_detectable(const Eigen::Ref<const Eigen::MatrixXd>& A,
-                      const Eigen::Ref<const Eigen::MatrixXd>& Q) {
-  // This function check if (A,C) is a detectable pair, where Q = C' * C.
-  // (A,C) is detectable if and only if the unobservable eigenvalues of A,
-  // if any, have absolute values less than one, where an eigenvalue is
-  // unobservable if Rank[lambda * I - A; C] < n.
-  // Also, (A,C) is detectable if and only if (A',C') is stabilizable.
-  int n = A.rows();
-  Eigen::LDLT<Eigen::MatrixXd> ldlt(Q);
-  Eigen::MatrixXd L = ldlt.matrixL();
-  Eigen::MatrixXd D = ldlt.vectorD();
-  Eigen::MatrixXd D_sqrt = Eigen::MatrixXd::Zero(n, n);
-  for (int i = 0; i < n; i++) {
-    D_sqrt(i, i) = sqrt(D(i));
-  }
-  Eigen::MatrixXd C = L * D_sqrt;
-  check_stabilizable(A.transpose(), C.transpose());
-}
-
-// "Givens rotation" computes an orthogonal 2x2 matrix R such that
-// it eliminates the 2nd coordinate of the vector [a,b]', i.e.,
-// R * [ a ] = [ a_hat ]
-//     [ b ]   [   0   ]
-// The implementation is based on
-// https://en.wikipedia.org/wiki/Givens_rotation#Stable_calculation
-void Givens_rotation(double a, double b, Eigen::Ref<Eigen::Matrix2d> R,
-                     double eps = 1e-10) {
-  double c, s;
-  if (fabs(b) < eps) {
-    c = (a < -eps ? -1 : 1);
-    s = 0;
-  } else if (fabs(a) < eps) {
-    c = 0;
-    s = -sgn(b);
-  } else if (fabs(a) > fabs(b)) {
-    double t = b / a;
-    double u = sgn(a) * fabs(sqrt(1 + t * t));
-    c = 1 / u;
-    s = -c * t;
-  } else {
-    double t = a / b;
-    double u = sgn(b) * fabs(sqrt(1 + t * t));
-    s = -1 / u;
-    c = -s * t;
-  }
-  R(0, 0) = c, R(0, 1) = -s, R(1, 0) = s, R(1, 1) = c;
-}
-
-// The arguments S, T, and Z will be changed.
-void swap_block_11(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
-                   Eigen::Ref<Eigen::MatrixXd> Z, int p) {
-  // Dooren, Case I, p124-125.
-  int n2 = S.rows();
-  Eigen::Matrix2d A = S.block<2, 2>(p, p);
-  Eigen::Matrix2d B = T.block<2, 2>(p, p);
-  Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::Matrix2d H = A(1, 1) * B - B(1, 1) * A;
-  Givens_rotation(H(0, 1), H(0, 0), Z1.block<2, 2>(p, p));
-  S = (S * Z1).eval();
-  T = (T * Z1).eval();
-  Z = (Z * Z1).eval();
-  Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(n2, n2);
-  Givens_rotation(T(p, p), T(p + 1, p), Q.block<2, 2>(p, p));
-  S = (Q * S).eval();
-  T = (Q * T).eval();
-  S(p + 1, p) = 0;
-  T(p + 1, p) = 0;
-}
-
-// The arguments S, T, and Z will be changed.
-void swap_block_21(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
-                   Eigen::Ref<Eigen::MatrixXd> Z, int p) {
-  // Dooren, Case II, p126-127.
-  int n2 = S.rows();
-  Eigen::Matrix3d A = S.block<3, 3>(p, p);
-  Eigen::Matrix3d B = T.block<3, 3>(p, p);
-  // Compute H and eliminate H(1,0) by row operation.
-  Eigen::Matrix3d H = A(2, 2) * B - B(2, 2) * A;
-  Eigen::Matrix3d R = Eigen::MatrixXd::Identity(3, 3);
-  Givens_rotation(H(0, 0), H(1, 0), R.block<2, 2>(0, 0));
-  H = (R * H).eval();
-  Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
-  // Compute Z1, Z2, Q1, Q2.
-  Givens_rotation(H(1, 2), H(1, 1), Z1.block<2, 2>(p + 1, p + 1));
-  H = (H * Z1.block<3, 3>(p, p)).eval();
-  Givens_rotation(H(0, 1), H(0, 0), Z2.block<2, 2>(p, p));
-  S = (S * Z1).eval();
-  T = (T * Z1).eval();
-  Z = (Z * Z1 * Z2).eval();
-  Givens_rotation(T(p + 1, p + 1), T(p + 2, p + 1),
-                  Q1.block<2, 2>(p + 1, p + 1));
-  S = (Q1 * S * Z2).eval();
-  T = (Q1 * T * Z2).eval();
-  Givens_rotation(T(p, p), T(p + 1, p), Q2.block<2, 2>(p, p));
-  S = (Q2 * S).eval();
-  T = (Q2 * T).eval();
-  S(p + 1, p) = 0;
-  S(p + 2, p) = 0;
-  T(p + 1, p) = 0;
-  T(p + 2, p) = 0;
-  T(p + 2, p + 1) = 0;
-}
-
-// The arguments S, T, and Z will be changed.
-void swap_block_12(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
-                   Eigen::Ref<Eigen::MatrixXd> Z, int p) {
-  int n2 = S.rows();
-  // Swap the role of S and T.
-  Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Q0 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(n2, n2);
-  Givens_rotation(S(p + 1, p + 1), S(p + 2, p + 1),
-                  Q0.block<2, 2>(p + 1, p + 1));
-  S = (Q0 * S).eval();
-  T = (Q0 * T).eval();
-  Eigen::Matrix3d A = S.block<3, 3>(p, p);
-  Eigen::Matrix3d B = T.block<3, 3>(p, p);
-  // Compute H and eliminate H(2,1) by column operation.
-  Eigen::Matrix3d H = B(0, 0) * A - A(0, 0) * B;
-  Eigen::Matrix3d R = Eigen::MatrixXd::Identity(3, 3);
-  Givens_rotation(H(2, 2), H(2, 1), R.block<2, 2>(1, 1));
-  H = (H * R).eval();
-  // Compute Q1, Q2, Z1, Z2.
-  Givens_rotation(H(0, 1), H(1, 1), Q1.block<2, 2>(p, p));
-  H = (Q1.block<3, 3>(p, p) * H).eval();
-  Givens_rotation(H(1, 2), H(2, 2), Q2.block<2, 2>(p + 1, p + 1));
-  S = (Q1 * S).eval();
-  T = (Q1 * T).eval();
-  Givens_rotation(S(p + 1, p + 1), S(p + 1, p), Z1.block<2, 2>(p, p));
-  S = (Q2 * S * Z1).eval();
-  T = (Q2 * T * Z1).eval();
-  Givens_rotation(S(p + 2, p + 2), S(p + 2, p + 1),
-                  Z2.block<2, 2>(p + 1, p + 1));
-  S = (S * Z2).eval();
-  T = (T * Z2).eval();
-  Z = (Z * Z1 * Z2).eval();
-  // Swap back the role of S and T.
-  Givens_rotation(T(p, p), T(p + 1, p), Q3.block<2, 2>(p, p));
-  S = (Q3 * S).eval();
-  T = (Q3 * T).eval();
-  S(p + 2, p) = 0;
-  S(p + 2, p + 1) = 0;
-  T(p + 1, p) = 0;
-  T(p + 2, p) = 0;
-  T(p + 2, p + 1) = 0;
-}
-
-// The arguments S, T, and Z will be changed.
-void swap_block_22(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
-                   Eigen::Ref<Eigen::MatrixXd> Z, int p) {
-  // Direct Swapping Algorithm based on
-  // "Numerical Methods for General and Structured Eigenvalue Problems" by
-  // Daniel Kressner, p108-111.
-  // ( http://sma.epfl.ch/~anchpcommon/publications/kressner_eigenvalues.pdf ).
-  // Also relevant but not applicable here:
-  // "On Swapping Diagonal Blocks in Real Schur Form" by Zhaojun Bai and James
-  // W. Demmelt;
-  int n2 = S.rows();
-  Eigen::MatrixXd A = S.block<4, 4>(p, p);
-  Eigen::MatrixXd B = T.block<4, 4>(p, p);
-  // Solve
-  // A11 * X - Y A22 = A12
-  // B11 * X - Y B22 = B12
-  // Reduce to solve Cx=D, where x=[x1;...;x4;y1;...;y4].
-  Eigen::Matrix<double, 8, 8> C = Eigen::Matrix<double, 8, 8>::Zero();
-  Eigen::Matrix<double, 8, 1> D;
-  // clang-format off
-  C << A(0, 0), 0, A(0, 1), 0, -A(2, 2), -A(3, 2), 0, 0,
-       0, A(0, 0), 0, A(0, 1), -A(2, 3), -A(3, 3), 0, 0,
-       A(1, 0), 0, A(1, 1), 0, 0, 0, -A(2, 2), -A(3, 2),
-       0, A(1, 0), 0, A(1, 1), 0, 0, -A(2, 3), -A(3, 3),
-       B(0, 0), 0, B(0, 1), 0, -B(2, 2), -B(3, 2), 0, 0,
-       0, B(0, 0), 0, B(0, 1), -B(2, 3), -B(3, 3), 0, 0,
-       B(1, 0), 0, B(1, 1), 0, 0, 0, -B(2, 2), -B(3, 2),
-       0, B(1, 0), 0, B(1, 1), 0, 0, -B(2, 3), -B(3, 3);
-  // clang-format on
-  D << A(0, 2), A(0, 3), A(1, 2), A(1, 3), B(0, 2), B(0, 3), B(1, 2), B(1, 3);
-  Eigen::MatrixXd x = C.colPivHouseholderQr().solve(D);
-  // Q * [ -Y ] = [ R_Y ] ,  Z' * [ -X ] = [ R_X ] .
-  //     [ I  ]   [  0  ]         [ I  ] = [  0  ]
-  Eigen::Matrix<double, 4, 2> X, Y;
-  X << -x(0, 0), -x(1, 0), -x(2, 0), -x(3, 0), Eigen::Matrix2d::Identity();
-  Y << -x(4, 0), -x(5, 0), -x(6, 0), -x(7, 0), Eigen::Matrix2d::Identity();
-  Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
-  Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 4, 2> > qr1(X);
-  Z1.block<4, 4>(p, p) = qr1.householderQ();
-  Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 4, 2> > qr2(Y);
-  Q1.block<4, 4>(p, p) = qr2.householderQ().adjoint();
-  // Apply transform Q1 * (S,T) * Z1.
-  S = (Q1 * S * Z1).eval();
-  T = (Q1 * T * Z1).eval();
-  Z = (Z * Z1).eval();
-  // Eliminate the T(p+3,p+2) entry.
-  Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
-  Givens_rotation(T(p + 2, p + 2), T(p + 3, p + 2),
-                  Q2.block<2, 2>(p + 2, p + 2));
-  S = (Q2 * S).eval();
-  T = (Q2 * T).eval();
-  // Eliminate the T(p+1,p) entry.
-  Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(n2, n2);
-  Givens_rotation(T(p, p), T(p + 1, p), Q3.block<2, 2>(p, p));
-  S = (Q3 * S).eval();
-  T = (Q3 * T).eval();
-  S(p + 2, p) = 0;
-  S(p + 2, p + 1) = 0;
-  S(p + 3, p) = 0;
-  S(p + 3, p + 1) = 0;
-  T(p + 1, p) = 0;
-  T(p + 2, p) = 0;
-  T(p + 2, p + 1) = 0;
-  T(p + 3, p) = 0;
-  T(p + 3, p + 1) = 0;
-  T(p + 3, p + 2) = 0;
-}
-
-// Functionality of "swap_block" function:
-// swap the 1x1 or 2x2 blocks pointed by p and q.
-// There are four cases: swapping 1x1 and 1x1 matrices, swapping 2x2 and 1x1
-// matrices, swapping 1x1 and 2x2 matrices, and swapping 2x2 and 2x2 matrices.
-// Algorithms are described in the papers
-// "A generalized eigenvalue approach for solving Riccati equations" by P. Van
-// Dooren, 1981 ( http://epubs.siam.org/doi/pdf/10.1137/0902010 ), and
-// "Numerical Methods for General and Structured Eigenvalue Problems" by
-// Daniel Kressner, 2005.
-void swap_block(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
-                Eigen::Ref<Eigen::MatrixXd> Z, int p, int q, int q_block_size,
-                double eps = 1e-10) {
-  int p_tmp = q, p_block_size;
-  while (p_tmp-- > p) {
-    p_block_size = 1;
-    if (p_tmp >= 1 && fabs(S(p_tmp, p_tmp - 1)) > eps) {
-      p_block_size = 2;
-      p_tmp--;
-    }
-    switch (p_block_size * 10 + q_block_size) {
-      case 11:
-        swap_block_11(S, T, Z, p_tmp);
-        break;
-      case 21:
-        swap_block_21(S, T, Z, p_tmp);
-        break;
-      case 12:
-        swap_block_12(S, T, Z, p_tmp);
-        break;
-      case 22:
-        swap_block_22(S, T, Z, p_tmp);
-        break;
-    }
-  }
-}
-
-// Functionality of "reorder_eigen" function:
-// Reorder the eigenvalues of (S,T) such that the top-left n by n matrix has
-// stable eigenvalues by multiplying Q's and Z's on the left and the right,
-// respectively.
-// Stable eigenvalues are inside the unit disk.
-//
-// Algorithm:
-// Go along the diagonals of (S,T) from the top left to the bottom right.
-// Once find a stable eigenvalue, push it to top left.
-// In implementation, use two pointers, p and q.
-// p points to the current block (1x1 or 2x2) and q points to the block with the
-// stable eigenvalue(s).
-// Push the block pointed by q to the position pointed by p.
-// Finish when n stable eigenvalues are placed at the top-left n by n matrix.
-// The algorithm for swapping blocks is described in the papers
-// "A generalized eigenvalue approach for solving Riccati equations" by P. Van
-// Dooren, 1981, and "Numerical Methods for General and Structured Eigenvalue
-// Problems" by Daniel Kressner, 2005.
-void reorder_eigen(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
-                   Eigen::Ref<Eigen::MatrixXd> Z, double eps = 1e-10) {
-  // abs(a) < eps => a = 0
-  int n2 = S.rows();
-  int n = n2 / 2, p = 0, q = 0;
-
-  // Find the first unstable p block.
-  while (p < n) {
-    if (fabs(S(p + 1, p)) < eps) {  // p block size = 1
-      if (fabs(T(p, p)) > eps && fabs(S(p, p)) <= fabs(T(p, p))) {  // stable
-        p++;
-        continue;
-      }
-    } else {  // p block size = 2
-      const double det_T =
-          T(p, p) * T(p + 1, p + 1) - T(p + 1, p) * T(p, p + 1);
-      if (fabs(det_T) > eps) {
-        const double det_S =
-            S(p, p) * S(p + 1, p + 1) - S(p + 1, p) * S(p, p + 1);
-        if (fabs(det_S) <= fabs(det_T)) {  // stable
-          p += 2;
-          continue;
-        }
-      }
-    }
-    break;
-  }
-  q = p;
-
-  // Make the first n generalized eigenvalues stable.
-  while (p < n && q < n2) {
-    // Update q.
-    int q_block_size = 0;
-    while (q < n2) {
-      if (q == n2 - 1 || fabs(S(q + 1, q)) < eps) {  // block size = 1
-        if (fabs(T(q, q)) > eps && fabs(S(q, q)) <= fabs(T(q, q))) {
-          q_block_size = 1;
-          break;
-        }
-        q++;
-      } else {  // block size = 2
-        const double det_T =
-            T(q, q) * T(q + 1, q + 1) - T(q + 1, q) * T(q, q + 1);
-        if (fabs(det_T) > eps) {
-          const double det_S =
-              S(q, q) * S(q + 1, q + 1) - S(q + 1, q) * S(q, q + 1);
-          if (fabs(det_S) <= fabs(det_T)) {
-            q_block_size = 2;
-            break;
-          }
-        }
-        q += 2;
-      }
-    }
-    if (q >= n2)
-      throw std::runtime_error("fail to find enough stable eigenvalues");
-    // Swap blocks pointed by p and q.
-    if (p != q) {
-      swap_block(S, T, Z, p, q, q_block_size);
-      p += q_block_size;
-      q += q_block_size;
-    }
-  }
-  if (p < n && q >= n2)
-    throw std::runtime_error("fail to find enough stable eigenvalues");
-}
-}  // namespace
-
-/**
- * DiscreteAlgebraicRiccatiEquation function
- * computes the unique stabilizing solution X to the discrete-time algebraic
- * Riccati equation:
- *
- * AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
- *
- * @throws std::exception if Q is not positive semi-definite.
- * @throws std::exception if R is not positive definite.
- *
- * Based on the Schur Vector approach outlined in this paper:
- * "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
- * by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell, in TAC, 1980,
- * http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=1102434
- *
- * Note: When, for example, n = 100, m = 80, and entries of A, B, Q_half,
- * R_half are sampled from standard normal distributions, where
- * Q = Q_halfᵀ Q_half and similar for R, the absolute error of the solution
- * is 10⁻⁶, while the absolute error of the solution computed by Matlab is
- * 10⁻⁸.
- *
- * TODO(weiqiao.han): I may overwrite the RealQZ function to improve the
- * accuracy, together with more thorough tests.
- */
-
-Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
-    const Eigen::Ref<const Eigen::MatrixXd>& A,
-    const Eigen::Ref<const Eigen::MatrixXd>& B,
-    const Eigen::Ref<const Eigen::MatrixXd>& Q,
-    const Eigen::Ref<const Eigen::MatrixXd>& R) {
-  int n = B.rows(), m = B.cols();
-
-  DRAKE_DEMAND(m <= n);
-  DRAKE_DEMAND(A.rows() == n && A.cols() == n);
-  DRAKE_DEMAND(Q.rows() == n && Q.cols() == n);
-  DRAKE_DEMAND(R.rows() == m && R.cols() == m);
-  DRAKE_DEMAND(is_approx_equal_abstol(Q, Q.transpose(), 1e-10));
-  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(Q);
-  for (int i = 0; i < n; i++) {
-    DRAKE_THROW_UNLESS(es.eigenvalues()[i] >= 0);
-  }
-  DRAKE_DEMAND(is_approx_equal_abstol(R, R.transpose(), 1e-10));
-  Eigen::LLT<Eigen::MatrixXd> R_cholesky(R);
-  DRAKE_THROW_UNLESS(R_cholesky.info() == Eigen::Success);
-  check_stabilizable(A, B);
-  check_detectable(A, Q);
-
-  Eigen::MatrixXd M(2 * n, 2 * n), L(2 * n, 2 * n);
-  M << A, Eigen::MatrixXd::Zero(n, n), -Q, Eigen::MatrixXd::Identity(n, n);
-  L << Eigen::MatrixXd::Identity(n, n), B * R.inverse() * B.transpose(),
-      Eigen::MatrixXd::Zero(n, n), A.transpose();
-
-  // QZ decomposition of M and L
-  // QMZ = S, QLZ = T
-  // where Q and Z are real orthogonal matrixes
-  // T is upper-triangular matrix, and S is upper quasi-triangular matrix
-  Eigen::RealQZ<Eigen::MatrixXd> qz(2 * n);
-  qz.compute(M, L);  // M = Q S Z,  L = Q T Z (Q and Z computed by Eigen package
-                     // are adjoints of Q and Z above)
-  Eigen::MatrixXd S = qz.matrixS(), T = qz.matrixT(),
-                  Z = qz.matrixZ().adjoint();
-
-  // Reorder the generalized eigenvalues of (S,T).
-  Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(2 * n, 2 * n);
-  reorder_eigen(S, T, Z2);
-  Z = (Z * Z2).eval();
-
-  // The first n columns of Z is ( U1 ) .
-  //                             ( U2 )
-  //            -1
-  // X = U2 * U1   is a solution of the discrete time Riccati equation.
-  Eigen::MatrixXd U1 = Z.block(0, 0, n, n), U2 = Z.block(n, 0, n, n);
-  Eigen::MatrixXd X = U2 * U1.inverse();
-  X = (X + X.adjoint().eval()) / 2.0;
-  return X;
-}
-
-Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
-    const Eigen::Ref<const Eigen::MatrixXd>& A,
-    const Eigen::Ref<const Eigen::MatrixXd>& B,
-    const Eigen::Ref<const Eigen::MatrixXd>& Q,
-    const Eigen::Ref<const Eigen::MatrixXd>& R,
-    const Eigen::Ref<const Eigen::MatrixXd>& N) {
-    DRAKE_DEMAND(N.rows() == B.rows() && N.cols() == B.cols());
-
-    // This is a change of variables to make the DARE that includes Q, R, and N
-    // cost matrices fit the form of the DARE that includes only Q and R cost
-    // matrices.
-    Eigen::MatrixXd A2 = A - B * R.llt().solve(N.transpose());
-    Eigen::MatrixXd Q2 = Q - N * R.llt().solve(N.transpose());
-    return DiscreteAlgebraicRiccatiEquation(A2, B, Q2, R);
-}
-
-}  // namespace math
-}  // namespace drake
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdDeque b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdDeque
deleted file mode 100644
index bc68397..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdDeque
+++ /dev/null
@@ -1,27 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDDEQUE_MODULE_H
-#define EIGEN_STDDEQUE_MODULE_H
-
-#include "Core"
-#include <deque>
-
-#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */
-
-#define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...)
-
-#else
-
-#include "src/StlSupport/StdDeque.h"
-
-#endif
-
-#endif // EIGEN_STDDEQUE_MODULE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdList b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdList
deleted file mode 100644
index 4c6262c..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdList
+++ /dev/null
@@ -1,26 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDLIST_MODULE_H
-#define EIGEN_STDLIST_MODULE_H
-
-#include "Core"
-#include <list>
-
-#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */
-
-#define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...)
-
-#else
-
-#include "src/StlSupport/StdList.h"
-
-#endif
-
-#endif // EIGEN_STDLIST_MODULE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdVector b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdVector
deleted file mode 100644
index 0c4697a..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdVector
+++ /dev/null
@@ -1,27 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDVECTOR_MODULE_H
-#define EIGEN_STDVECTOR_MODULE_H
-
-#include "Core"
-#include <vector>
-
-#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */
-
-#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...)
-
-#else
-
-#include "src/StlSupport/StdVector.h"
-
-#endif
-
-#endif // EIGEN_STDVECTOR_MODULE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
index d973255..9a630e4 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
@@ -78,7 +78,7 @@
     // This warning is a false positive
     #pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
   #endif
-  #if __GNUC__==12
+  #if __GNUC__>=12
     // This warning is a false positive
     #pragma GCC diagnostic ignored "-Warray-bounds"
   #endif
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h
index 986c3d4..81986b9 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h
@@ -58,6 +58,16 @@
 // Compiler identification, EIGEN_COMP_*
 //------------------------------------------------------------------------------------------
 
+/// \internal Disable NEON features in Intellisense
+#if __INTELLISENSE__
+#ifdef __ARM_NEON
+#undef __ARM_NEON
+#endif
+#ifdef __ARM_NEON__
+#undef __ARM_NEON__
+#endif
+#endif
+
 /// \internal EIGEN_COMP_GNUC set to 1 for all compilers compatible with GCC
 #ifdef __GNUC__
   #define EIGEN_COMP_GNUC (__GNUC__*10+__GNUC_MINOR__)
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h
index f9c56ba..7cb2c26 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h
@@ -270,11 +270,11 @@
       }
 
 
-      Index count = 0;
+//       Index count = 0;
       // FIXME compute a reference value to filter zeros
       for (typename AmbiVector<Scalar,StorageIndex>::Iterator it(tempVector/*,1e-12*/); it; ++it)
       {
-        ++ count;
+//         ++ count;
 //         std::cerr << "fill " << it.index() << ", " << col << "\n";
 //         std::cout << it.value() << "  ";
         // FIXME use insertBack
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
index 6f75d50..7aecbca 100644
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
@@ -75,8 +75,6 @@
   // Identify the relaxed supernodes by postorder traversal of the etree
   Index snode_start; // beginning of a snode 
   StorageIndex k;
-  Index nsuper_et_post = 0; // Number of relaxed snodes in postordered etree 
-  Index nsuper_et = 0; // Number of relaxed snodes in the original etree 
   StorageIndex l; 
   for (j = 0; j < n; )
   {
@@ -88,7 +86,6 @@
       parent = et(j);
     }
     // Found a supernode in postordered etree, j is the last column 
-    ++nsuper_et_post;
     k = StorageIndex(n);
     for (Index i = snode_start; i <= j; ++i)
       k = (std::min)(k, inv_post(i));
@@ -97,7 +94,6 @@
     {
       // This is also a supernode in the original etree
       relax_end(k) = l; // Record last column 
-      ++nsuper_et; 
     }
     else 
     {
@@ -107,7 +103,6 @@
         if (descendants(i) == 0) 
         {
           relax_end(l) = l;
-          ++nsuper_et;
         }
       }
     }
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdDeque.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdDeque.h
deleted file mode 100644
index 6d47e75..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdDeque.h
+++ /dev/null
@@ -1,116 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDDEQUE_H
-#define EIGEN_STDDEQUE_H
-
-#include "details.h"
-
-/**
- * This section contains a convenience MACRO which allows an easy specialization of
- * std::deque such that for data types with alignment issues the correct allocator
- * is used automatically.
- */
-#define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...) \
-namespace std \
-{ \
-  template<> \
-  class deque<__VA_ARGS__, std::allocator<__VA_ARGS__> >           \
-    : public deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \
-  { \
-    typedef deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > deque_base; \
-  public: \
-    typedef __VA_ARGS__ value_type; \
-    typedef deque_base::allocator_type allocator_type; \
-    typedef deque_base::size_type size_type;  \
-    typedef deque_base::iterator iterator;  \
-    explicit deque(const allocator_type& a = allocator_type()) : deque_base(a) {}  \
-    template<typename InputIterator> \
-    deque(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : deque_base(first, last, a) {} \
-    deque(const deque& c) : deque_base(c) {}  \
-    explicit deque(size_type num, const value_type& val = value_type()) : deque_base(num, val) {} \
-    deque(iterator start_, iterator end_) : deque_base(start_, end_) {}  \
-    deque& operator=(const deque& x) {  \
-      deque_base::operator=(x);  \
-      return *this;  \
-    } \
-  }; \
-}
-
-// check whether we really need the std::deque specialization
-#if !EIGEN_HAS_CXX11_CONTAINERS && !(defined(_GLIBCXX_DEQUE) && (!EIGEN_GNUC_AT_LEAST(4,1))) /* Note that before gcc-4.1 we already have: std::deque::resize(size_type,const T&). */
-
-namespace std {
-
-#define EIGEN_STD_DEQUE_SPECIALIZATION_BODY \
-  public:  \
-    typedef T value_type; \
-    typedef typename deque_base::allocator_type allocator_type; \
-    typedef typename deque_base::size_type size_type;  \
-    typedef typename deque_base::iterator iterator;  \
-    typedef typename deque_base::const_iterator const_iterator;  \
-    explicit deque(const allocator_type& a = allocator_type()) : deque_base(a) {}  \
-    template<typename InputIterator> \
-    deque(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) \
-    : deque_base(first, last, a) {} \
-    deque(const deque& c) : deque_base(c) {}  \
-    explicit deque(size_type num, const value_type& val = value_type()) : deque_base(num, val) {} \
-    deque(iterator start_, iterator end_) : deque_base(start_, end_) {}  \
-    deque& operator=(const deque& x) {  \
-      deque_base::operator=(x);  \
-      return *this;  \
-    }
-
-  template<typename T>
-  class deque<T,EIGEN_ALIGNED_ALLOCATOR<T> >
-    : public deque<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
-                   Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> >
-{
-  typedef deque<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
-                Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> > deque_base;
-  EIGEN_STD_DEQUE_SPECIALIZATION_BODY
-
-  void resize(size_type new_size)
-  { resize(new_size, T()); }
-
-#if defined(_DEQUE_)
-  // workaround MSVC std::deque implementation
-  void resize(size_type new_size, const value_type& x)
-  {
-    if (deque_base::size() < new_size)
-      deque_base::_Insert_n(deque_base::end(), new_size - deque_base::size(), x);
-    else if (new_size < deque_base::size())
-      deque_base::erase(deque_base::begin() + new_size, deque_base::end());
-  }
-  void push_back(const value_type& x)
-  { deque_base::push_back(x); } 
-  void push_front(const value_type& x)
-  { deque_base::push_front(x); }
-  using deque_base::insert;  
-  iterator insert(const_iterator position, const value_type& x)
-  { return deque_base::insert(position,x); }
-  void insert(const_iterator position, size_type new_size, const value_type& x)
-  { deque_base::insert(position, new_size, x); }
-#else
-  // default implementation which should always work.
-  void resize(size_type new_size, const value_type& x)
-  {
-    if (new_size < deque_base::size())
-      deque_base::erase(deque_base::begin() + new_size, deque_base::end());
-    else if (new_size > deque_base::size())
-      deque_base::insert(deque_base::end(), new_size - deque_base::size(), x);
-  }
-#endif
-  };
-}
-
-#endif // check whether specialization is actually required
-
-#endif // EIGEN_STDDEQUE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdList.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdList.h
deleted file mode 100644
index 8ba3fad..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdList.h
+++ /dev/null
@@ -1,106 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDLIST_H
-#define EIGEN_STDLIST_H
-
-#include "details.h"
-
-/**
- * This section contains a convenience MACRO which allows an easy specialization of
- * std::list such that for data types with alignment issues the correct allocator
- * is used automatically.
- */
-#define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...) \
-namespace std \
-{ \
-  template<> \
-  class list<__VA_ARGS__, std::allocator<__VA_ARGS__> >           \
-    : public list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \
-  { \
-    typedef list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > list_base; \
-  public: \
-    typedef __VA_ARGS__ value_type; \
-    typedef list_base::allocator_type allocator_type; \
-    typedef list_base::size_type size_type;  \
-    typedef list_base::iterator iterator;  \
-    explicit list(const allocator_type& a = allocator_type()) : list_base(a) {}  \
-    template<typename InputIterator> \
-    list(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : list_base(first, last, a) {} \
-    list(const list& c) : list_base(c) {}  \
-    explicit list(size_type num, const value_type& val = value_type()) : list_base(num, val) {} \
-    list(iterator start_, iterator end_) : list_base(start_, end_) {}  \
-    list& operator=(const list& x) {  \
-      list_base::operator=(x);  \
-      return *this;  \
-    } \
-  }; \
-}
-
-// check whether we really need the std::list specialization
-#if !EIGEN_HAS_CXX11_CONTAINERS && !(defined(_GLIBCXX_LIST) && (!EIGEN_GNUC_AT_LEAST(4,1))) /* Note that before gcc-4.1 we already have: std::list::resize(size_type,const T&). */
-
-namespace std
-{
-
-#define EIGEN_STD_LIST_SPECIALIZATION_BODY \
-  public:  \
-    typedef T value_type; \
-    typedef typename list_base::allocator_type allocator_type; \
-    typedef typename list_base::size_type size_type;  \
-    typedef typename list_base::iterator iterator;  \
-    typedef typename list_base::const_iterator const_iterator;  \
-    explicit list(const allocator_type& a = allocator_type()) : list_base(a) {}  \
-    template<typename InputIterator> \
-    list(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) \
-    : list_base(first, last, a) {} \
-    list(const list& c) : list_base(c) {}  \
-    explicit list(size_type num, const value_type& val = value_type()) : list_base(num, val) {} \
-    list(iterator start_, iterator end_) : list_base(start_, end_) {}  \
-    list& operator=(const list& x) {  \
-    list_base::operator=(x);  \
-    return *this; \
-  }
-
-  template<typename T>
-  class list<T,EIGEN_ALIGNED_ALLOCATOR<T> >
-    : public list<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
-                  Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> >
-  {
-    typedef list<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
-                 Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> > list_base;
-    EIGEN_STD_LIST_SPECIALIZATION_BODY
-
-    void resize(size_type new_size)
-    { resize(new_size, T()); }
-
-    void resize(size_type new_size, const value_type& x)
-    {
-      if (list_base::size() < new_size)
-        list_base::insert(list_base::end(), new_size - list_base::size(), x);
-      else
-        while (new_size < list_base::size()) list_base::pop_back();
-    }
-
-#if defined(_LIST_)
-    // workaround MSVC std::list implementation
-    void push_back(const value_type& x)
-    { list_base::push_back(x); } 
-    using list_base::insert;  
-    iterator insert(const_iterator position, const value_type& x)
-    { return list_base::insert(position,x); }
-    void insert(const_iterator position, size_type new_size, const value_type& x)
-    { list_base::insert(position, new_size, x); }
-#endif
-  };
-}
-
-#endif // check whether specialization is actually required
-
-#endif // EIGEN_STDLIST_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdVector.h
deleted file mode 100644
index 9fcf19b..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdVector.h
+++ /dev/null
@@ -1,131 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STDVECTOR_H
-#define EIGEN_STDVECTOR_H
-
-#include "details.h"
-
-/**
- * This section contains a convenience MACRO which allows an easy specialization of
- * std::vector such that for data types with alignment issues the correct allocator
- * is used automatically.
- */
-#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...) \
-namespace std \
-{ \
-  template<> \
-  class vector<__VA_ARGS__, std::allocator<__VA_ARGS__> >  \
-    : public vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \
-  { \
-    typedef vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > vector_base; \
-  public: \
-    typedef __VA_ARGS__ value_type; \
-    typedef vector_base::allocator_type allocator_type; \
-    typedef vector_base::size_type size_type;  \
-    typedef vector_base::iterator iterator;  \
-    explicit vector(const allocator_type& a = allocator_type()) : vector_base(a) {}  \
-    template<typename InputIterator> \
-    vector(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : vector_base(first, last, a) {} \
-    vector(const vector& c) : vector_base(c) {}  \
-    explicit vector(size_type num, const value_type& val = value_type()) : vector_base(num, val) {} \
-    vector(iterator start_, iterator end_) : vector_base(start_, end_) {}  \
-    vector& operator=(const vector& x) {  \
-      vector_base::operator=(x);  \
-      return *this;  \
-    } \
-  }; \
-}
-
-// Don't specialize if containers are implemented according to C++11
-#if !EIGEN_HAS_CXX11_CONTAINERS
-
-namespace std {
-
-#define EIGEN_STD_VECTOR_SPECIALIZATION_BODY \
-  public:  \
-    typedef T value_type; \
-    typedef typename vector_base::allocator_type allocator_type; \
-    typedef typename vector_base::size_type size_type;  \
-    typedef typename vector_base::iterator iterator;  \
-    typedef typename vector_base::const_iterator const_iterator;  \
-    explicit vector(const allocator_type& a = allocator_type()) : vector_base(a) {}  \
-    template<typename InputIterator> \
-    vector(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) \
-    : vector_base(first, last, a) {} \
-    vector(const vector& c) : vector_base(c) {}  \
-    explicit vector(size_type num, const value_type& val = value_type()) : vector_base(num, val) {} \
-    vector(iterator start_, iterator end_) : vector_base(start_, end_) {}  \
-    vector& operator=(const vector& x) {  \
-      vector_base::operator=(x);  \
-      return *this;  \
-    }
-
-  template<typename T>
-  class vector<T,EIGEN_ALIGNED_ALLOCATOR<T> >
-    : public vector<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
-                    Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> >
-{
-  typedef vector<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T),
-                 Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T)> > vector_base;
-  EIGEN_STD_VECTOR_SPECIALIZATION_BODY
-
-  void resize(size_type new_size)
-  { resize(new_size, T()); }
-
-#if defined(_VECTOR_)
-  // workaround MSVC std::vector implementation
-  void resize(size_type new_size, const value_type& x)
-  {
-    if (vector_base::size() < new_size)
-      vector_base::_Insert_n(vector_base::end(), new_size - vector_base::size(), x);
-    else if (new_size < vector_base::size())
-      vector_base::erase(vector_base::begin() + new_size, vector_base::end());
-  }
-  void push_back(const value_type& x)
-  { vector_base::push_back(x); } 
-  using vector_base::insert;  
-  iterator insert(const_iterator position, const value_type& x)
-  { return vector_base::insert(position,x); }
-  void insert(const_iterator position, size_type new_size, const value_type& x)
-  { vector_base::insert(position, new_size, x); }
-#elif defined(_GLIBCXX_VECTOR) && (!(EIGEN_GNUC_AT_LEAST(4,1)))
-  /* Note that before gcc-4.1 we already have: std::vector::resize(size_type,const T&).
-   * However, this specialization is still needed to make the above EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION trick to work. */
-  void resize(size_type new_size, const value_type& x)
-  {
-    vector_base::resize(new_size,x);
-  }
-#elif defined(_GLIBCXX_VECTOR) && EIGEN_GNUC_AT_LEAST(4,2)
-  // workaround GCC std::vector implementation
-  void resize(size_type new_size, const value_type& x)
-  {
-    if (new_size < vector_base::size())
-      vector_base::_M_erase_at_end(this->_M_impl._M_start + new_size);
-    else
-      vector_base::insert(vector_base::end(), new_size - vector_base::size(), x);
-  }
-#else
-  // either GCC 4.1 or non-GCC
-  // default implementation which should always work.
-  void resize(size_type new_size, const value_type& x)
-  {
-    if (new_size < vector_base::size())
-      vector_base::erase(vector_base::begin() + new_size, vector_base::end());
-    else if (new_size > vector_base::size())
-      vector_base::insert(vector_base::end(), new_size - vector_base::size(), x);
-  }
-#endif
-  };
-}
-#endif // !EIGEN_HAS_CXX11_CONTAINERS
-
-
-#endif // EIGEN_STDVECTOR_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/details.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/details.h
deleted file mode 100644
index 2cfd13e..0000000
--- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/details.h
+++ /dev/null
@@ -1,84 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_STL_DETAILS_H
-#define EIGEN_STL_DETAILS_H
-
-#ifndef EIGEN_ALIGNED_ALLOCATOR
-  #define EIGEN_ALIGNED_ALLOCATOR Eigen::aligned_allocator
-#endif
-
-namespace Eigen {
-
-  // This one is needed to prevent reimplementing the whole std::vector.
-  template <class T>
-  class aligned_allocator_indirection : public EIGEN_ALIGNED_ALLOCATOR<T>
-  {
-  public:
-    typedef std::size_t     size_type;
-    typedef std::ptrdiff_t  difference_type;
-    typedef T*              pointer;
-    typedef const T*        const_pointer;
-    typedef T&              reference;
-    typedef const T&        const_reference;
-    typedef T               value_type;
-
-    template<class U>
-    struct rebind
-    {
-      typedef aligned_allocator_indirection<U> other;
-    };
-
-    aligned_allocator_indirection() {}
-    aligned_allocator_indirection(const aligned_allocator_indirection& ) : EIGEN_ALIGNED_ALLOCATOR<T>() {}
-    aligned_allocator_indirection(const EIGEN_ALIGNED_ALLOCATOR<T>& ) {}
-    template<class U>
-    aligned_allocator_indirection(const aligned_allocator_indirection<U>& ) {}
-    template<class U>
-    aligned_allocator_indirection(const EIGEN_ALIGNED_ALLOCATOR<U>& ) {}
-    ~aligned_allocator_indirection() {}
-  };
-
-#if EIGEN_COMP_MSVC
-
-  // sometimes, MSVC detects, at compile time, that the argument x
-  // in std::vector::resize(size_t s,T x) won't be aligned and generate an error
-  // even if this function is never called. Whence this little wrapper.
-#define EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T) \
-  typename Eigen::internal::conditional< \
-    Eigen::internal::is_arithmetic<T>::value, \
-    T, \
-    Eigen::internal::workaround_msvc_stl_support<T> \
-  >::type
-
-  namespace internal {
-  template<typename T> struct workaround_msvc_stl_support : public T
-  {
-    inline workaround_msvc_stl_support() : T() {}
-    inline workaround_msvc_stl_support(const T& other) : T(other) {}
-    inline operator T& () { return *static_cast<T*>(this); }
-    inline operator const T& () const { return *static_cast<const T*>(this); }
-    template<typename OtherT>
-    inline T& operator=(const OtherT& other)
-    { T::operator=(other); return *this; }
-    inline workaround_msvc_stl_support& operator=(const workaround_msvc_stl_support& other)
-    { T::operator=(other); return *this; }
-  };
-  }
-
-#else
-
-#define EIGEN_WORKAROUND_MSVC_STL_SUPPORT(T) T
-
-#endif
-
-}
-
-#endif // EIGEN_STL_DETAILS_H
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp
index cb28ff0..650d05d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp
index 0a3ada6..6d7b66d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp
index 9d3bc07..a47003d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp
index 79579d7..8767200 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp
index 210d9fc..6a79e87 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp
index dfad57e..a5f3ff6 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp
index 9aea85b..3f46974 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp
index 97c8d6a..5ca55b7 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp
index 2e1cb76..dfb4dc3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp
index e888e47..e43d4fc 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp
index fb050a2..0fc17f3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp
index e8570ab..ff1097b 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp
index a3bab74..c1741f7 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp
index 0e98012..82f4c60 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp
index b3cebb8..fc89c0d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp
index afec09e..d0bc83a 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp
index a93f8db..412d686 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp
index 0c66829..595ffc2 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
@@ -28,6 +28,30 @@
 namespace internal
 {
 
+// see https://en.wikipedia.org/wiki/Euler%27s_continued_fraction_formula
+
+#if __cplusplus >= 201402L // C++14 version
+
+template<typename T>
+constexpr
+T
+exp_cf_recur(const T x, const int depth_end)
+noexcept
+{
+    int depth = GCEM_EXP_MAX_ITER_SMALL - 1;
+    T res = T(1);
+
+    while (depth > depth_end - 1) {
+        res = T(1) + x/T(depth - 1) - x/depth/res;
+
+        --depth;
+    }
+
+    return res;
+}
+
+#else // C++11 version
+
 template<typename T>
 constexpr
 T
@@ -36,20 +60,20 @@
 {
     return( depth < GCEM_EXP_MAX_ITER_SMALL ? \
             // if
-                depth == 1 ? \
-                    T(1) - x/exp_cf_recur(x,depth+1) : 
-                    T(1) + x/T(depth - 1) - x/depth/exp_cf_recur(x,depth+1) : 
+                T(1) + x/T(depth - 1) - x/depth/exp_cf_recur(x,depth+1) : 
              // else
                 T(1) );
 }
 
+#endif
+
 template<typename T>
 constexpr
 T
 exp_cf(const T x)
 noexcept
 {
-    return( T(1)/exp_cf_recur(x,1) );
+    return( T(1) / (T(1) - x / exp_cf_recur(x,2)) );
 }
 
 template<typename T>
@@ -72,7 +96,7 @@
             //
             is_neginf(x) ? \
                 T(0) :
-            //
+            // indistinguishable from zero
             GCLIM<T>::min() > abs(x) ? \
                 T(1) : 
             //
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp
index 11b2eb9..70c9ecf 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp
index 539c3f3..ffb9c82 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp
index 710adce..200e4e9 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
@@ -34,10 +34,20 @@
 find_exponent(const T x, const llint_t exponent)
 noexcept
 {
-    return( x < T(1)  ? \
-                find_exponent(x*T(10),exponent - llint_t(1)) :
+    return( // < 1
+            x < T(1e-03)  ? \
+                find_exponent(x * T(1e+04), exponent - llint_t(4)) :
+            x < T(1e-01)  ? \
+                find_exponent(x * T(1e+02), exponent - llint_t(2)) :
+            x < T(1)  ? \
+                find_exponent(x * T(10), exponent - llint_t(1)) :
+            // > 10
             x > T(10) ? \
-                find_exponent(x/T(10),exponent + llint_t(1)) :
+                find_exponent(x / T(10), exponent + llint_t(1)) :
+            x > T(1e+02) ? \
+                find_exponent(x / T(1e+02), exponent + llint_t(2)) :
+            x > T(1e+04) ? \
+                find_exponent(x / T(1e+04), exponent + llint_t(4)) :
             // else
                 exponent );
 }
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp
index d9769e6..5ed3d26 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp
index bd5e0b9..d193632 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp
index c60ff6a..8b260ff 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp
index c804ce6..02459ef 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp
index 4a10bbe..1e277fb 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp
index cd2747c..4113738 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
@@ -53,7 +53,7 @@
 #endif
 
 #ifndef GCEM_VERSION_MINOR
-    #define GCEM_VERSION_MINOR 16
+    #define GCEM_VERSION_MINOR 17
 #endif
 
 #ifndef GCEM_VERSION_PATCH
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp
index 5a805ed..01ad4e9 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp
index 5645dbe..dbb9f60 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp
index f7fdfa0..9f575a3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp
index 38734a5..9ee4146 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp
index 1e57fc1..e5976d0 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp
index 0200f11..d0e33fb 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp
index fa925bb..de0641d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp
index 25f2e3c..b632fa3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp
index 627c509..568614f 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp
index a7a1af3..a3fcbc6 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp
index e6da720..a74a8d3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp
index 2213849..60c87b4 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp
index b0d8fb4..a7ca776 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp
index 5d78eb3..507c6d4 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp
index 76bf833..58915dc 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp
index 0d83e97..c2e24b0 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
@@ -31,6 +31,28 @@
 // continued fraction seems to be a better approximation for small x
 // see http://functions.wolfram.com/ElementaryFunctions/Log/10/0005/
 
+#if __cplusplus >= 201402L // C++14 version
+
+template<typename T>
+constexpr
+T
+log_cf_main(const T xx, const int depth_end)
+noexcept
+{
+    int depth = GCEM_LOG_MAX_ITER_SMALL - 1;
+    T res = T(2*(depth+1) - 1);
+
+    while (depth > depth_end - 1) {
+        res = T(2*depth - 1) - T(depth*depth) * xx / res;
+
+        --depth;
+    }
+
+    return res;
+}
+
+#else // C++11 version
+
 template<typename T>
 constexpr
 T
@@ -39,11 +61,13 @@
 {
     return( depth < GCEM_LOG_MAX_ITER_SMALL ? \
             // if 
-                T(2*depth - 1) - T(depth*depth)*xx/log_cf_main(xx,depth+1) :
+                T(2*depth - 1) - T(depth*depth) * xx / log_cf_main(xx,depth+1) :
             // else 
                 T(2*depth - 1) );
 }
 
+#endif
+
 template<typename T>
 constexpr
 T
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp
index 4a3c37d..cda8894 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp
index 3883b22..ccd08b8 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp
index 56b7f8e..a97fed4 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp
index 7aa9a2b..2bcaadd 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp
index df6152b..af23ea2 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
@@ -35,9 +35,9 @@
 noexcept
 {
     return( x < T(1) ? \
-                mantissa(x*T(10)) : 
+                mantissa(x * T(10)) : 
             x > T(10) ? \
-                mantissa(x/T(10)) :
+                mantissa(x / T(10)) :
             // else
                 x );
 }
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp
index 4aed84f..ddc3e4e 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp
index d593dbc..5ce70b3 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp
index db33f87..79d24a4 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp
index 166a8c1..3891ede 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp
index 3a902ca..4e67155 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp
index e609b89..295f43d 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp
index 44281f9..d4e448c 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp
index 43d7a5e..9ac4a09 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp
index 605a35a..e2eec9e 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp
index e207a5a..282e244 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp
index 128cd32..56c8dca 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp
index 5355301..fe3ecdd 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp
index 0fd559d..1b2753c 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
@@ -37,16 +37,37 @@
     return( abs(xn - x/xn) / (T(1) + xn) < GCLIM<T>::min() ? \
             // if
                 xn :
-            count < GCEM_SQRT_MAX_ITER ? \
             // else
-                sqrt_recur(x, T(0.5)*(xn + x/xn), count+1) :
-                xn );
+                count < GCEM_SQRT_MAX_ITER ? \
+                // if
+                    sqrt_recur(x, T(0.5)*(xn + x/xn), count+1) :
+                // else
+                    xn );
 }
 
 template<typename T>
 constexpr
 T
-sqrt_check(const T x, const T m_val)
+sqrt_simplify(const T x, const T m_val)
+noexcept
+{
+    return( x > T(1e+08) ? \
+                sqrt_simplify(x / T(1e+08), T(1e+04) * m_val) :
+            x > T(1e+06) ? \
+                sqrt_simplify(x / T(1e+06), T(1e+03) * m_val) :
+            x > T(1e+04) ? \
+                sqrt_simplify(x / T(1e+04), T(1e+02) * m_val) :
+            x > T(100) ? \
+                sqrt_simplify(x / T(100), T(10) * m_val) :
+            x > T(4) ? \
+                sqrt_simplify(x / T(4), T(2) * m_val) :
+                m_val * sqrt_recur(x, x / T(2), 0) );
+}
+
+template<typename T>
+constexpr
+T
+sqrt_check(const T x)
 noexcept
 {
     return( is_nan(x) ? \
@@ -63,9 +84,7 @@
             GCLIM<T>::min() > abs(T(1) - x) ? \
                 x :
             // else
-            x > T(4) ? \
-                sqrt_check(x/T(4), T(2)*m_val) :
-                m_val * sqrt_recur(x, x/T(2), 0) );
+            sqrt_simplify(x, T(1)) );
 }
 
 }
@@ -84,7 +103,7 @@
 sqrt(const T x)
 noexcept
 {
-    return internal::sqrt_check( static_cast<return_t<T>>(x), return_t<T>(1) );
+    return internal::sqrt_check( static_cast<return_t<T>>(x) );
 }
 
 #endif
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp
index e53f5c8..386cce0 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp
index 109d751..30b4318 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp
index 5a9ae97..deffd3a 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp
index 4e19ef9..af3f448 100644
--- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp
@@ -1,6 +1,6 @@
 /*################################################################################
   ##
-  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##   Copyright (C) 2016-2023 Keith O'Hara
   ##
   ##   This file is part of the GCE-Math C++ library.
   ##
diff --git a/wpimath/src/main/proto/controller.proto b/wpimath/src/main/proto/controller.proto
new file mode 100644
index 0000000..0d0d3fb
--- /dev/null
+++ b/wpimath/src/main/proto/controller.proto
@@ -0,0 +1,37 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufArmFeedforward {
+  double ks = 1;
+  double kg = 2;
+  double kv = 3;
+  double ka = 4;
+}
+
+message ProtobufDifferentialDriveFeedforward {
+  double kv_linear = 1;
+  double ka_linear = 2;
+  double kv_angular = 3;
+  double ka_angular = 4;
+}
+
+message ProtobufElevatorFeedforward {
+  double ks = 1;
+  double kg = 2;
+  double kv = 3;
+  double ka = 4;
+}
+
+message ProtobufSimpleMotorFeedforward {
+  double ks = 1;
+  double kv = 2;
+  double ka = 3;
+}
+
+message ProtobufDifferentialDriveWheelVoltages {
+  double left = 1;
+  double right = 2;
+}
diff --git a/wpimath/src/main/proto/geometry2d.proto b/wpimath/src/main/proto/geometry2d.proto
new file mode 100644
index 0000000..d52da45
--- /dev/null
+++ b/wpimath/src/main/proto/geometry2d.proto
@@ -0,0 +1,30 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufTranslation2d {
+  double x = 1;
+  double y = 2;
+}
+
+message ProtobufRotation2d {
+  double value = 1;
+}
+
+message ProtobufPose2d {
+  ProtobufTranslation2d translation = 1;
+  ProtobufRotation2d rotation = 2;
+}
+
+message ProtobufTransform2d {
+  ProtobufTranslation2d translation = 1;
+  ProtobufRotation2d rotation = 2;
+}
+
+message ProtobufTwist2d {
+  double dx = 1;
+  double dy = 2;
+  double dtheta = 3;
+}
diff --git a/wpimath/src/main/proto/geometry3d.proto b/wpimath/src/main/proto/geometry3d.proto
new file mode 100644
index 0000000..23b4d64
--- /dev/null
+++ b/wpimath/src/main/proto/geometry3d.proto
@@ -0,0 +1,41 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufTranslation3d {
+  double x = 1;
+  double y = 2;
+  double z = 3;
+}
+
+message ProtobufQuaternion {
+  double w = 1;
+  double x = 2;
+  double y = 3;
+  double z = 4;
+}
+
+message ProtobufRotation3d {
+  ProtobufQuaternion q = 1;
+}
+
+message ProtobufPose3d {
+  ProtobufTranslation3d translation = 1;
+  ProtobufRotation3d rotation = 2;
+}
+
+message ProtobufTransform3d {
+  ProtobufTranslation3d translation = 1;
+  ProtobufRotation3d rotation = 2;
+}
+
+message ProtobufTwist3d {
+  double dx = 1;
+  double dy = 2;
+  double dz = 3;
+  double rx = 4;
+  double ry = 5;
+  double rz = 6;
+}
diff --git a/wpimath/src/main/proto/kinematics.proto b/wpimath/src/main/proto/kinematics.proto
new file mode 100644
index 0000000..8fdbf2d
--- /dev/null
+++ b/wpimath/src/main/proto/kinematics.proto
@@ -0,0 +1,64 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+import "geometry2d.proto";
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufChassisSpeeds {
+  double vx = 1;
+  double vy = 2;
+  double omega = 3;
+}
+
+message ProtobufDifferentialDriveKinematics {
+  double track_width = 1;
+}
+
+message ProtobufDifferentialDriveWheelSpeeds {
+  double left = 1;
+  double right = 2;
+}
+
+message ProtobufMecanumDriveKinematics {
+  ProtobufTranslation2d front_left = 1;
+  ProtobufTranslation2d front_right = 2;
+  ProtobufTranslation2d rear_left = 3;
+  ProtobufTranslation2d rear_right = 4;
+}
+
+message ProtobufMecanumDriveMotorVoltages {
+  double front_left = 1;
+  double front_right = 2;
+  double rear_left = 3;
+  double rear_right = 4;
+}
+
+message ProtobufMecanumDriveWheelPositions {
+  double front_left = 1;
+  double front_right = 2;
+  double rear_left = 3;
+  double rear_right = 4;
+}
+
+message ProtobufMecanumDriveWheelSpeeds {
+  double front_left = 1;
+  double front_right = 2;
+  double rear_left = 3;
+  double rear_right = 4;
+}
+
+message ProtobufSwerveDriveKinematics {
+  repeated ProtobufTranslation2d modules = 1;
+}
+
+message ProtobufSwerveModulePosition {
+  double distance = 1;
+  ProtobufRotation2d angle = 2;
+}
+
+message ProtobufSwerveModuleState {
+  double speed = 1;
+  ProtobufRotation2d angle = 2;
+}
diff --git a/wpimath/src/main/proto/plant.proto b/wpimath/src/main/proto/plant.proto
new file mode 100644
index 0000000..d0d9eab
--- /dev/null
+++ b/wpimath/src/main/proto/plant.proto
@@ -0,0 +1,16 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufDCMotor {
+  double nominal_voltage = 1;
+  double stall_torque = 2;
+  double stall_current = 3;
+  double free_current = 4;
+  double free_speed = 5;
+  double r = 6;
+  double kv = 7;
+  double kt = 8;
+}
diff --git a/wpimath/src/main/proto/spline.proto b/wpimath/src/main/proto/spline.proto
new file mode 100644
index 0000000..ef0a3c9
--- /dev/null
+++ b/wpimath/src/main/proto/spline.proto
@@ -0,0 +1,19 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufCubicHermiteSpline {
+  repeated double x_initial = 1;
+  repeated double x_final = 2;
+  repeated double y_initial = 3;
+  repeated double y_final = 4;
+}
+
+message ProtobufQuinticHermiteSpline {
+  repeated double x_initial = 1;
+  repeated double x_final = 2;
+  repeated double y_initial = 3;
+  repeated double y_final = 4;
+}
diff --git a/wpimath/src/main/proto/system.proto b/wpimath/src/main/proto/system.proto
new file mode 100644
index 0000000..4818c19
--- /dev/null
+++ b/wpimath/src/main/proto/system.proto
@@ -0,0 +1,17 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+import "wpimath.proto";
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufLinearSystem {
+  uint32 num_states = 1;
+  uint32 num_inputs = 2;
+  uint32 num_outputs = 3;
+  ProtobufMatrix a = 4;
+  ProtobufMatrix b = 5;
+  ProtobufMatrix c = 6;
+  ProtobufMatrix d = 7;
+}
diff --git a/wpimath/src/main/proto/trajectory.proto b/wpimath/src/main/proto/trajectory.proto
new file mode 100644
index 0000000..a37a501
--- /dev/null
+++ b/wpimath/src/main/proto/trajectory.proto
@@ -0,0 +1,20 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+import "geometry2d.proto";
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufTrajectoryState {
+  double time = 1;
+  double velocity = 2;
+  double acceleration = 3;
+  ProtobufPose2d pose = 4;
+  double curvature = 5;
+}
+
+message ProtobufTrajectory {
+  double total_time = 1;
+  repeated ProtobufTrajectoryState states = 2;
+}
diff --git a/wpimath/src/main/proto/wpimath.proto b/wpimath/src/main/proto/wpimath.proto
new file mode 100644
index 0000000..06b993a
--- /dev/null
+++ b/wpimath/src/main/proto/wpimath.proto
@@ -0,0 +1,15 @@
+syntax = "proto3";
+
+package wpi.proto;
+
+option java_package = "edu.wpi.first.math.proto";
+
+message ProtobufMatrix {
+  uint32 num_rows = 1;
+  uint32 num_cols = 2;
+  repeated double data = 3;
+}
+
+message ProtobufVector {
+  repeated double rows = 1;
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java
index 2927847..3c281e5 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java
@@ -11,9 +11,14 @@
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.UtilityClassTest;
 import org.junit.jupiter.api.Test;
 
-class ComputerVisionUtilTest {
+class ComputerVisionUtilTest extends UtilityClassTest<ComputerVisionUtil> {
+  ComputerVisionUtilTest() {
+    super(ComputerVisionUtil.class);
+  }
+
   @Test
   void testObjectToRobotPose() {
     var robot = new Pose3d(1.0, 2.0, 0.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(30.0)));
diff --git a/wpimath/src/test/java/edu/wpi/first/math/DARETest.java b/wpimath/src/test/java/edu/wpi/first/math/DARETest.java
new file mode 100644
index 0000000..0d697bd
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/DARETest.java
@@ -0,0 +1,335 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.wpilibj.UtilityClassTest;
+import org.ejml.simple.SimpleMatrix;
+import org.junit.jupiter.api.Test;
+
+class DARETest extends UtilityClassTest<DARE> {
+  DARETest() {
+    super(DARE.class);
+  }
+
+  public static <R extends Num, C extends Num> void assertMatrixEqual(
+      Matrix<R, C> A, Matrix<R, C> B) {
+    for (int i = 0; i < A.getNumRows(); i++) {
+      for (int j = 0; j < A.getNumCols(); j++) {
+        assertEquals(A.get(i, j), B.get(i, j), 1e-4);
+      }
+    }
+  }
+
+  <States extends Num, Inputs extends Num> void assertDARESolution(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R,
+      Matrix<States, States> X) {
+    // Check that X is the solution to the DARE
+    // Y = AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+    var Y =
+        (A.transpose().times(X).times(A))
+            .minus(X)
+            .minus(
+                (A.transpose().times(X).times(B))
+                    .times((B.transpose().times(X).times(B).plus(R)).inv())
+                    .times(B.transpose().times(X).times(A)))
+            .plus(Q);
+    assertMatrixEqual(
+        new Matrix<States, States>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
+  }
+
+  <States extends Num, Inputs extends Num> void assertDARESolution(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R,
+      Matrix<States, Inputs> N,
+      Matrix<States, States> X) {
+    // Check that X is the solution to the DARE
+    // Y = AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+    var Y =
+        (A.transpose().times(X).times(A))
+            .minus(X)
+            .minus(
+                (A.transpose().times(X).times(B).plus(N))
+                    .times((B.transpose().times(X).times(B).plus(R)).inv())
+                    .times(B.transpose().times(X).times(A).plus(N.transpose())))
+            .plus(Q);
+    assertMatrixEqual(
+        new Matrix<States, States>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
+  }
+
+  @Test
+  void testNonInvertibleA_ABQR() {
+    // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
+    // Riccati Equation"
+
+    var A =
+        new Matrix<>(
+            Nat.N4(), Nat.N4(), new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
+    var B = new Matrix<>(Nat.N4(), Nat.N1(), new double[] {0, 0, 0, 1});
+    var Q =
+        new Matrix<>(
+            Nat.N4(), Nat.N4(), new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
+    var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.25});
+
+    var X = DARE.dare(A, B, Q, R);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, X);
+  }
+
+  @Test
+  void testNonInvertibleA_ABQRN() {
+    // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
+    // Riccati Equation"
+
+    var A =
+        new Matrix<>(
+            Nat.N4(), Nat.N4(), new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
+    var B = new Matrix<>(Nat.N4(), Nat.N1(), new double[] {0, 0, 0, 1});
+    var Q =
+        new Matrix<>(
+            Nat.N4(), Nat.N4(), new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
+    var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.25});
+
+    var Aref =
+        new Matrix<>(
+            Nat.N4(), Nat.N4(), new double[] {0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
+    Q = A.minus(Aref).transpose().times(Q).times(A.minus(Aref));
+    R = B.transpose().times(Q).times(B).plus(R);
+    var N = A.minus(Aref).transpose().times(Q).times(B);
+
+    var X = DARE.dare(A, B, Q, R, N);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, N, X);
+  }
+
+  @Test
+  void testInvertibleA_ABQR() {
+    var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 1, 0, 1});
+    var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
+    var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 0});
+    var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.3});
+
+    var X = DARE.dare(A, B, Q, R);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, X);
+  }
+
+  @Test
+  void testInvertibleA_ABQRN() {
+    var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 1, 0, 1});
+    var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
+    var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 0});
+    var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.3});
+
+    var Aref = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.5, 1, 0, 1});
+    Q = A.minus(Aref).transpose().times(Q).times(A.minus(Aref));
+    R = B.transpose().times(Q).times(B).plus(R);
+    var N = A.minus(Aref).transpose().times(Q).times(B);
+
+    var X = DARE.dare(A, B, Q, R, N);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, N, X);
+  }
+
+  @Test
+  void testFirstGeneralizedEigenvalueOfSTIsStable_ABQR() {
+    // The first generalized eigenvalue of (S, T) is stable
+
+    var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0, 1, 0, 0});
+    var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
+    var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 1});
+    var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {1});
+
+    var X = DARE.dare(A, B, Q, R);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, X);
+  }
+
+  @Test
+  void testFirstGeneralizedEigenvalueOfSTIsStable_ABQRN() {
+    // The first generalized eigenvalue of (S, T) is stable
+
+    var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0, 1, 0, 0});
+    var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
+    var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 1});
+    var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {1});
+
+    var Aref = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0, 0.5, 0, 0});
+    Q = A.minus(Aref).transpose().times(Q).times(A.minus(Aref));
+    R = B.transpose().times(Q).times(B).plus(R);
+    var N = A.minus(Aref).transpose().times(Q).times(B);
+
+    var X = DARE.dare(A, B, Q, R, N);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, N, X);
+  }
+
+  @Test
+  void testIdentitySystem_ABQR() {
+    var A = Matrix.eye(Nat.N2());
+    var B = Matrix.eye(Nat.N2());
+    var Q = Matrix.eye(Nat.N2());
+    var R = Matrix.eye(Nat.N2());
+
+    var X = DARE.dare(A, B, Q, R);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, X);
+  }
+
+  @Test
+  void testIdentitySystem_ABQRN() {
+    var A = Matrix.eye(Nat.N2());
+    var B = Matrix.eye(Nat.N2());
+    var Q = Matrix.eye(Nat.N2());
+    var R = Matrix.eye(Nat.N2());
+    var N = Matrix.eye(Nat.N2());
+
+    var X = DARE.dare(A, B, Q, R, N);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, N, X);
+  }
+
+  @Test
+  void testMoreInputsThanStates_ABQR() {
+    var A = Matrix.eye(Nat.N2());
+    var B = new Matrix<>(Nat.N2(), Nat.N3(), new double[] {1, 0, 0, 0, 0.5, 0.3});
+    var Q = Matrix.eye(Nat.N2());
+    var R = Matrix.eye(Nat.N3());
+
+    var X = DARE.dare(A, B, Q, R);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, X);
+  }
+
+  @Test
+  void testMoreInputsThanStates_ABQRN() {
+    var A = Matrix.eye(Nat.N2());
+    var B = new Matrix<>(Nat.N2(), Nat.N3(), new double[] {1, 0, 0, 0, 0.5, 0.3});
+    var Q = Matrix.eye(Nat.N2());
+    var R = Matrix.eye(Nat.N3());
+    var N = new Matrix<>(Nat.N2(), Nat.N3(), new double[] {1, 0, 0, 0, 1, 0});
+
+    var X = DARE.dare(A, B, Q, R, N);
+    assertMatrixEqual(X, X.transpose());
+    assertDARESolution(A, B, Q, R, N, X);
+  }
+
+  @Test
+  void testQNotSymmetricPositiveSemidefinite_ABQR() {
+    var A = Matrix.eye(Nat.N2());
+    var B = Matrix.eye(Nat.N2());
+    var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
+    var R = Matrix.eye(Nat.N2());
+
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
+  }
+
+  @Test
+  void testQNotSymmetricPositiveSemidefinite_ABQRN() {
+    var A = Matrix.eye(Nat.N2());
+    var B = Matrix.eye(Nat.N2());
+    var Q = Matrix.eye(Nat.N2());
+    var R = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
+    var N = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {2.0, 0.0, 0.0, 2.0});
+
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
+  }
+
+  @Test
+  void testRNotSymmetricPositiveDefinite_ABQR() {
+    var A = Matrix.eye(Nat.N2());
+    var B = Matrix.eye(Nat.N2());
+    var Q = Matrix.eye(Nat.N2());
+
+    var R1 = new Matrix<>(Nat.N2(), Nat.N2());
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R1));
+
+    var R2 = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R2));
+  }
+
+  @Test
+  void testRNotSymmetricPositiveDefinite_ABQRN() {
+    var A = Matrix.eye(Nat.N2());
+    var B = Matrix.eye(Nat.N2());
+    var Q = Matrix.eye(Nat.N2());
+    var N = Matrix.eye(Nat.N2());
+
+    var R1 = new Matrix<>(Nat.N2(), Nat.N2());
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R1, N));
+
+    var R2 = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R2, N));
+  }
+
+  @Test
+  void testABNotStabilizable_ABQR() {
+    var A = Matrix.eye(Nat.N2());
+    var B = new Matrix<>(Nat.N2(), Nat.N2());
+    var Q = Matrix.eye(Nat.N2());
+    var R = Matrix.eye(Nat.N2());
+
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
+  }
+
+  @Test
+  void testABNotStabilizable_ABQRN() {
+    var A = Matrix.eye(Nat.N2());
+    var B = new Matrix<>(Nat.N2(), Nat.N2());
+    var Q = Matrix.eye(Nat.N2());
+    var R = Matrix.eye(Nat.N2());
+    var N = Matrix.eye(Nat.N2());
+
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
+  }
+
+  @Test
+  void testACNotDetectable_ABQR() {
+    var A = Matrix.eye(Nat.N2());
+    var B = Matrix.eye(Nat.N2());
+    var Q = new Matrix<>(Nat.N2(), Nat.N2());
+    var R = Matrix.eye(Nat.N2());
+
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
+  }
+
+  @Test
+  void testACNotDetectable_ABQRN() {
+    var A = Matrix.eye(Nat.N2());
+    var B = Matrix.eye(Nat.N2());
+    var Q = new Matrix<>(Nat.N2(), Nat.N2());
+    var R = Matrix.eye(Nat.N2());
+    var N = new Matrix<>(Nat.N2(), Nat.N2());
+
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
+  }
+
+  @Test
+  void testQDecomposition() {
+    // Ensures the decomposition of Q into CᵀC is correct
+
+    var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1.0, 0.0, 0.0, 0.0});
+    var B = Matrix.eye(Nat.N2());
+    var R = Matrix.eye(Nat.N2());
+
+    // (A, C₁) should be detectable pair
+    var C_1 = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.0, 0.0, 1.0, 0.0});
+    var Q_1 = C_1.transpose().times(C_1);
+    assertDoesNotThrow(() -> DARE.dare(A, B, Q_1, R));
+
+    // (A, C₂) shouldn't be detectable pair
+    var C_2 = C_1.transpose();
+    var Q_2 = C_2.transpose().times(C_2);
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q_2, R));
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java b/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
deleted file mode 100644
index 3050867..0000000
--- a/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
+++ /dev/null
@@ -1,73 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package edu.wpi.first.math;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-import org.ejml.simple.SimpleMatrix;
-import org.junit.jupiter.api.Test;
-
-class DrakeTest {
-  public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) {
-    for (int i = 0; i < A.numRows(); i++) {
-      for (int j = 0; j < A.numCols(); j++) {
-        assertEquals(A.get(i, j), B.get(i, j), 1e-4);
-      }
-    }
-  }
-
-  private boolean solveDAREandVerify(
-      SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) {
-    var X = Drake.discreteAlgebraicRiccatiEquation(A, B, Q, R);
-
-    // expect that x is the same as it's transpose
-    assertEquals(X.numRows(), X.numCols());
-    assertMatrixEqual(X, X.transpose());
-
-    // Verify that this is a solution to the DARE.
-    SimpleMatrix Y =
-        A.transpose()
-            .mult(X)
-            .mult(A)
-            .minus(X)
-            .minus(
-                A.transpose()
-                    .mult(X)
-                    .mult(B)
-                    .mult(((B.transpose().mult(X).mult(B)).plus(R)).invert())
-                    .mult(B.transpose())
-                    .mult(X)
-                    .mult(A))
-            .plus(Q);
-    assertMatrixEqual(Y, new SimpleMatrix(Y.numRows(), Y.numCols()));
-
-    return true;
-  }
-
-  @Test
-  void testDiscreteAlgebraicRicattiEquation() {
-    int n1 = 4;
-    int m1 = 1;
-
-    // we know from Scipy that this should be [[0.05048525 0.10097051 0.20194102 0.40388203]]
-    SimpleMatrix A1 =
-        new SimpleMatrix(
-                n1, n1, true, new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0})
-            .transpose();
-    SimpleMatrix B1 = new SimpleMatrix(n1, m1, true, new double[] {0, 0, 0, 1});
-    SimpleMatrix Q1 =
-        new SimpleMatrix(
-            n1, n1, true, new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
-    SimpleMatrix R1 = new SimpleMatrix(m1, m1, true, new double[] {0.25});
-    assertTrue(solveDAREandVerify(A1, B1, Q1, R1));
-
-    SimpleMatrix A2 = new SimpleMatrix(2, 2, true, new double[] {1, 1, 0, 1});
-    SimpleMatrix B2 = new SimpleMatrix(2, 1, true, new double[] {0, 1});
-    SimpleMatrix Q2 = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 0});
-    SimpleMatrix R2 = new SimpleMatrix(1, 1, true, new double[] {0.3});
-    assertTrue(solveDAREandVerify(A2, B2, Q2, R2));
-  }
-}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
index a3de9cc..44f7c50 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
@@ -5,10 +5,32 @@
 package edu.wpi.first.math;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.wpilibj.UtilityClassTest;
 import org.junit.jupiter.api.Test;
 
-class MathUtilTest {
+class MathUtilTest extends UtilityClassTest<MathUtil> {
+  MathUtilTest() {
+    super(MathUtil.class);
+  }
+
+  @Test
+  void testClamp() {
+    // int
+    assertEquals(5, MathUtil.clamp(10, 1, 5));
+
+    // double
+    assertEquals(5.5, MathUtil.clamp(10.5, 1.5, 5.5));
+
+    // negative int
+    assertEquals(-5, MathUtil.clamp(-10, -5, -1));
+
+    // negative double
+    assertEquals(-5.5, MathUtil.clamp(-10.5, -5.5, -1.5));
+  }
+
   @Test
   void testApplyDeadbandUnityScale() {
     // < 0
@@ -88,4 +110,61 @@
     assertEquals(MathUtil.angleModulus(Math.PI / 2), Math.PI / 2);
     assertEquals(MathUtil.angleModulus(-Math.PI / 2), -Math.PI / 2);
   }
+
+  @Test
+  void testInterpolate() {
+    assertEquals(50, MathUtil.interpolate(0, 100, 0.5));
+    assertEquals(-50, MathUtil.interpolate(0, -100, 0.5));
+    assertEquals(0, MathUtil.interpolate(-50, 50, 0.5));
+    assertEquals(-25, MathUtil.interpolate(-50, 50, 0.25));
+    assertEquals(25, MathUtil.interpolate(-50, 50, 0.75));
+
+    assertEquals(0, MathUtil.interpolate(0, -100, -0.5));
+  }
+
+  @Test
+  void testIsNear() {
+    // The answer is always 42
+    // Positive integer checks
+    assertTrue(MathUtil.isNear(42, 42, 1));
+    assertTrue(MathUtil.isNear(42, 41, 2));
+    assertTrue(MathUtil.isNear(42, 43, 2));
+    assertFalse(MathUtil.isNear(42, 44, 1));
+
+    // Negative integer checks
+    assertTrue(MathUtil.isNear(-42, -42, 1));
+    assertTrue(MathUtil.isNear(-42, -41, 2));
+    assertTrue(MathUtil.isNear(-42, -43, 2));
+    assertFalse(MathUtil.isNear(-42, -44, 1));
+
+    // Mixed sign integer checks
+    assertFalse(MathUtil.isNear(-42, 42, 1));
+    assertFalse(MathUtil.isNear(-42, 41, 2));
+    assertFalse(MathUtil.isNear(-42, 43, 2));
+    assertFalse(MathUtil.isNear(42, -42, 1));
+    assertFalse(MathUtil.isNear(42, -41, 2));
+    assertFalse(MathUtil.isNear(42, -43, 2));
+
+    // Floating point checks
+    assertTrue(MathUtil.isNear(42, 41.5, 1));
+    assertTrue(MathUtil.isNear(42, 42.5, 1));
+    assertTrue(MathUtil.isNear(42, 41.5, 0.75));
+    assertTrue(MathUtil.isNear(42, 42.5, 0.75));
+
+    // Wraparound checks
+    assertTrue(MathUtil.isNear(0, 356, 5, 0, 360));
+    assertTrue(MathUtil.isNear(0, -356, 5, 0, 360));
+    assertTrue(MathUtil.isNear(0, 4, 5, 0, 360));
+    assertTrue(MathUtil.isNear(0, -4, 5, 0, 360));
+    assertTrue(MathUtil.isNear(400, 41, 5, 0, 360));
+    assertTrue(MathUtil.isNear(400, -319, 5, 0, 360));
+    assertTrue(MathUtil.isNear(400, 401, 5, 0, 360));
+    assertFalse(MathUtil.isNear(0, 356, 2.5, 0, 360));
+    assertFalse(MathUtil.isNear(0, -356, 2.5, 0, 360));
+    assertFalse(MathUtil.isNear(0, 4, 2.5, 0, 360));
+    assertFalse(MathUtil.isNear(0, -4, 2.5, 0, 360));
+    assertFalse(MathUtil.isNear(400, 35, 5, 0, 360));
+    assertFalse(MathUtil.isNear(400, -315, 5, 0, 360));
+    assertFalse(MathUtil.isNear(400, 395, 5, 0, 360));
+  }
 }
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 054c29a..ee33f01 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
@@ -12,13 +12,18 @@
 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.wpilibj.UtilityClassTest;
 import java.util.ArrayList;
 import java.util.List;
 import org.ejml.dense.row.MatrixFeatures_DDRM;
 import org.ejml.simple.SimpleMatrix;
 import org.junit.jupiter.api.Test;
 
-class StateSpaceUtilTest {
+class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
+  StateSpaceUtilTest() {
+    super(StateSpaceUtil.class);
+  }
+
   @Test
   void testCostArray() {
     var mat = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(1.0, 2.0, 3.0));
diff --git a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
index 5488b68..008953f 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
@@ -10,6 +10,44 @@
 
 class VectorTest {
   @Test
+  void testVectorPlus() {
+    var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
+    var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);
+    var result1 = vec1.plus(vec2);
+
+    assertEquals(5.0, result1.get(0, 0));
+    assertEquals(7.0, result1.get(1, 0));
+    assertEquals(9.0, result1.get(2, 0));
+
+    var vec3 = VecBuilder.fill(-1.0, 2.0, -3.0);
+    var vec4 = VecBuilder.fill(4.0, -5.0, 6.0);
+    var result2 = vec3.plus(vec4);
+
+    assertEquals(3.0, result2.get(0, 0));
+    assertEquals(-3.0, result2.get(1, 0));
+    assertEquals(3.0, result2.get(2, 0));
+  }
+
+  @Test
+  void testVectorMinus() {
+    var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
+    var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);
+    var result1 = vec1.minus(vec2);
+
+    assertEquals(-3.0, result1.get(0, 0));
+    assertEquals(-3.0, result1.get(1, 0));
+    assertEquals(-3.0, result1.get(2, 0));
+
+    var vec3 = VecBuilder.fill(-1.0, 2.0, -3.0);
+    var vec4 = VecBuilder.fill(4.0, -5.0, 6.0);
+    var result2 = vec3.minus(vec4);
+
+    assertEquals(-5.0, result2.get(0, 0));
+    assertEquals(7.0, result2.get(1, 0));
+    assertEquals(-9.0, result2.get(2, 0));
+  }
+
+  @Test
   void testVectorDot() {
     var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
     var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java
new file mode 100644
index 0000000..7b5cf77
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java
@@ -0,0 +1,39 @@
+// 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.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class ArmFeedforwardTest {
+  private static final double ks = 0.5;
+  private static final double kg = 1;
+  private static final double kv = 1.5;
+  private static final double ka = 2;
+  private final ArmFeedforward m_armFF = new ArmFeedforward(ks, kg, kv, ka);
+
+  @Test
+  void testCalculate() {
+    assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0), 0.002);
+    assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1), 0.002);
+    assertEquals(6.5, m_armFF.calculate(Math.PI / 3, 1, 2), 0.002);
+    assertEquals(2.5, m_armFF.calculate(Math.PI / 3, -1, 2), 0.002);
+  }
+
+  @Test
+  void testAcheviableVelocity() {
+    assertEquals(6, m_armFF.maxAchievableVelocity(12, Math.PI / 3, 1), 0.002);
+    assertEquals(-9, m_armFF.minAchievableVelocity(11.5, Math.PI / 3, 1), 0.002);
+  }
+
+  @Test
+  void testAcheviableAcceleration() {
+    assertEquals(4.75, m_armFF.maxAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
+    assertEquals(6.75, m_armFF.maxAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
+    assertEquals(-7.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
+    assertEquals(-5.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
index b23ee1e..faf5563 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
@@ -231,7 +231,6 @@
               .times(xAccelLimiter)
               .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0)));
       final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
-      System.out.println(a);
       assertTrue(Math.abs(a) > maxA);
       assertTrue(Math.abs(a) > -minA);
     }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java
new file mode 100644
index 0000000..6c978ac
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java
@@ -0,0 +1,56 @@
+// 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.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import org.junit.jupiter.api.Test;
+
+class ElevatorFeedforwardTest {
+  private static final double ks = 0.5;
+  private static final double kg = 1;
+  private static final double kv = 1.5;
+  private static final double ka = 2;
+
+  private final ElevatorFeedforward m_elevatorFF = new ElevatorFeedforward(ks, kg, kv, ka);
+
+  @Test
+  void testCalculate() {
+    assertEquals(1, m_elevatorFF.calculate(0), 0.002);
+    assertEquals(4.5, m_elevatorFF.calculate(2), 0.002);
+    assertEquals(6.5, m_elevatorFF.calculate(2, 1), 0.002);
+    assertEquals(-0.5, m_elevatorFF.calculate(-2, 1), 0.002);
+
+    var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-kv / ka);
+    var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / ka);
+    final double dt = 0.02;
+    var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);
+
+    var r = VecBuilder.fill(2.0);
+    var nextR = VecBuilder.fill(3.0);
+    assertEquals(
+        plantInversion.calculate(r, nextR).get(0, 0) + ks + kg,
+        m_elevatorFF.calculate(2.0, 3.0, dt),
+        0.002);
+  }
+
+  @Test
+  void testAcheviableVelocity() {
+    assertEquals(5, m_elevatorFF.maxAchievableVelocity(11, 1), 0.002);
+    assertEquals(-9, m_elevatorFF.minAchievableVelocity(11, 1), 0.002);
+  }
+
+  @Test
+  void testAcheviableAcceleration() {
+    assertEquals(3.75, m_elevatorFF.maxAchievableAcceleration(12, 2), 0.002);
+    assertEquals(7.25, m_elevatorFF.maxAchievableAcceleration(12, -2), 0.002);
+    assertEquals(-8.25, m_elevatorFF.minAchievableAcceleration(12, 2), 0.002);
+    assertEquals(-4.75, m_elevatorFF.minAchievableAcceleration(12, -2), 0.002);
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
index 31213f6..634ed1c 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
@@ -148,10 +148,10 @@
     assertEquals(0.51182128351092726, K.get(0, 1), 1e-10);
 
     // QRN overload
-    var Aref = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / (Ka * 2.0));
+    var Aref = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / (Ka * 5.0));
     var Kimf = getImplicitModelFollowingK(A, B, Q, R, Aref, 0.005);
     assertEquals(0.0, Kimf.get(0, 0), 1e-10);
-    assertEquals(-5.367540084534802e-05, Kimf.get(0, 1), 1e-10);
+    assertEquals(-6.9190500116751458e-05, Kimf.get(0, 1), 1e-10);
   }
 
   @Test
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
index 7b8484d..7ea8caa 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
@@ -58,12 +58,12 @@
     TrapezoidProfile profile;
     TrapezoidProfile.State state;
     for (int i = 0; i < 1000; i++) {
-      profile =
-          new TrapezoidProfile(
-              constraints,
+      profile = new TrapezoidProfile(constraints);
+      state =
+          profile.calculate(
+              kDt,
               new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)),
               new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0)));
-      state = profile.calculate(kDt);
       m_loop.setNextR(VecBuilder.fill(state.position, state.velocity));
 
       updateTwoState(m_plant, m_loop, (random.nextGaussian()) * kPositionStddev);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java
index 1fe4cb1..b25240e 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java
@@ -55,4 +55,24 @@
 
     assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5);
   }
+
+  @Test
+  void iZoneNoOutputTest() {
+    m_controller.setI(1);
+    m_controller.setIZone(1);
+
+    double out = m_controller.calculate(2, 0);
+
+    assertEquals(0, out, 1e-5);
+  }
+
+  @Test
+  void iZoneOutputTest() {
+    m_controller.setI(1);
+    m_controller.setIZone(1);
+
+    double out = m_controller.calculate(1, 0);
+
+    assertEquals(-1 * m_controller.getPeriod(), out, 1e-5);
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java
index b525f49..4fdb867 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java
@@ -19,7 +19,7 @@
     try (var controller = new PIDController(0.05, 0.0, 0.0)) {
       controller.enableContinuousInput(-kRange / 2, kRange / 2);
 
-      assertTrue(controller.atSetpoint());
+      assertFalse(controller.atSetpoint());
     }
   }
 
@@ -28,10 +28,7 @@
     try (var controller = new PIDController(0.05, 0.0, 0.0)) {
       controller.enableContinuousInput(-kRange / 2, kRange / 2);
 
-      assertTrue(
-          controller.atSetpoint(),
-          "Error was not in tolerance when it should have been. Error was "
-              + controller.getPositionError());
+      assertFalse(controller.atSetpoint());
 
       controller.setTolerance(kTolerance);
       controller.setSetpoint(kSetpoint);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
index c3906d4..e0e202f 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
@@ -143,8 +143,6 @@
 
     double t = 0.0;
 
-    System.out.print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
-
     final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
 
     double maxError = Double.NEGATIVE_INFINITY;
@@ -191,16 +189,6 @@
               leftDistanceMeters,
               rightDistanceMeters);
 
-      System.out.printf(
-          "%f, %f, %f, %f, %f, %f, %f\n",
-          t,
-          xHat.getX(),
-          xHat.getY(),
-          xHat.getRotation().getRadians(),
-          groundTruthState.poseMeters.getX(),
-          groundTruthState.poseMeters.getY(),
-          groundTruthState.poseMeters.getRotation().getRadians());
-
       double error =
           groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
       if (error > maxError) {
@@ -227,4 +215,92 @@
       assertEquals(0.0, maxError, 0.2, "Incorrect max error");
     }
   }
+
+  @Test
+  void testSimultaneousVisionMeasurements() {
+    // This tests for multiple vision measurements appled at the same time. The expected behavior
+    // is that all measurements affect the estimated pose. The alternative result is that only one
+    // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
+    // estimated pose would converge to that measurement.
+    var kinematics = new DifferentialDriveKinematics(1);
+
+    var estimator =
+        new DifferentialDrivePoseEstimator(
+            kinematics,
+            new Rotation2d(),
+            0,
+            0,
+            new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
+            VecBuilder.fill(0.02, 0.02, 0.01),
+            VecBuilder.fill(0.1, 0.1, 0.1));
+
+    estimator.updateWithTime(0, new Rotation2d(), 0, 0);
+
+    var visionMeasurements =
+        new Pose2d[] {
+          new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+          new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
+          new Pose2d(2, 4, Rotation2d.fromRadians(180)),
+        };
+
+    for (int i = 0; i < 1000; i++) {
+      for (var measurement : visionMeasurements) {
+        estimator.addVisionMeasurement(measurement, 0);
+      }
+    }
+
+    for (var measurement : visionMeasurements) {
+      var errorLog =
+          "Estimator converged to one vision measurement: "
+              + estimator.getEstimatedPosition().toString()
+              + " -> "
+              + measurement.toString();
+
+      var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
+      var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
+      var dtheta =
+          Math.abs(
+              measurement.getRotation().getDegrees()
+                  - estimator.getEstimatedPosition().getRotation().getDegrees());
+
+      assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
+    }
+  }
+
+  @Test
+  void testDiscardsStaleVisionMeasurements() {
+    var kinematics = new DifferentialDriveKinematics(1);
+    var estimator =
+        new DifferentialDrivePoseEstimator(
+            kinematics,
+            new Rotation2d(),
+            0,
+            0,
+            new Pose2d(),
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9));
+
+    double time = 0;
+
+    // Add enough measurements to fill up the buffer
+    for (; time < 4; time += 0.02) {
+      estimator.updateWithTime(time, new Rotation2d(), 0, 0);
+    }
+
+    var odometryPose = estimator.getEstimatedPosition();
+
+    // Apply a vision measurement made 3 seconds ago
+    // This test passes if this does not cause a ConcurrentModificationException.
+    estimator.addVisionMeasurement(
+        new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
+        1,
+        VecBuilder.fill(0.1, 0.1, 0.1));
+
+    assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
+    assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
+    assertEquals(
+        odometryPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().getRadians(),
+        "Incorrect Final Theta");
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
index 02d2d52..d844c5d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
@@ -151,8 +151,6 @@
 
     double t = 0.0;
 
-    System.out.print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
-
     final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
 
     double maxError = Double.NEGATIVE_INFINITY;
@@ -200,16 +198,6 @@
                   .minus(trajectory.getInitialPose().getRotation()),
               wheelPositions);
 
-      System.out.printf(
-          "%f, %f, %f, %f, %f, %f, %f\n",
-          t,
-          xHat.getX(),
-          xHat.getY(),
-          xHat.getRotation().getRadians(),
-          groundTruthState.poseMeters.getX(),
-          groundTruthState.poseMeters.getY(),
-          groundTruthState.poseMeters.getRotation().getRadians());
-
       double error =
           groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
       if (error > maxError) {
@@ -236,4 +224,100 @@
       assertEquals(0.0, maxError, 0.2, "Incorrect max error");
     }
   }
+
+  @Test
+  void testSimultaneousVisionMeasurements() {
+    // This tests for multiple vision measurements appled at the same time. The expected behavior
+    // is that all measurements affect the estimated pose. The alternative result is that only one
+    // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
+    // estimated pose would converge to that measurement.
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1), new Translation2d(1, -1),
+            new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+    var wheelPositions = new MecanumDriveWheelPositions();
+
+    var estimator =
+        new MecanumDrivePoseEstimator(
+            kinematics,
+            new Rotation2d(),
+            wheelPositions,
+            new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.45, 0.45, 0.1));
+
+    estimator.updateWithTime(0, new Rotation2d(), wheelPositions);
+
+    var visionMeasurements =
+        new Pose2d[] {
+          new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+          new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
+          new Pose2d(2, 4, Rotation2d.fromRadians(180)),
+        };
+
+    for (int i = 0; i < 1000; i++) {
+      for (var measurement : visionMeasurements) {
+        estimator.addVisionMeasurement(measurement, 0);
+      }
+    }
+
+    for (var measurement : visionMeasurements) {
+      var errorLog =
+          "Estimator converged to one vision measurement: "
+              + estimator.getEstimatedPosition().toString()
+              + " -> "
+              + measurement.toString();
+
+      var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
+      var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
+      var dtheta =
+          Math.abs(
+              measurement.getRotation().getDegrees()
+                  - estimator.getEstimatedPosition().getRotation().getDegrees());
+
+      assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
+    }
+  }
+
+  @Test
+  void testDiscardsOldVisionMeasurements() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new MecanumDrivePoseEstimator(
+            kinematics,
+            new Rotation2d(),
+            new MecanumDriveWheelPositions(),
+            new Pose2d(),
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9));
+
+    double time = 0;
+
+    // Add enough measurements to fill up the buffer
+    for (; time < 4; time += 0.02) {
+      estimator.updateWithTime(time, new Rotation2d(), new MecanumDriveWheelPositions());
+    }
+
+    var odometryPose = estimator.getEstimatedPosition();
+
+    // Apply a vision measurement made 3 seconds ago
+    // This test passes if this does not cause a ConcurrentModificationException.
+    estimator.addVisionMeasurement(
+        new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
+        1,
+        VecBuilder.fill(0.1, 0.1, 0.1));
+
+    assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
+    assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
+    assertEquals(
+        odometryPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().getRadians(),
+        "Incorrect Final Theta");
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
index fde2c39..9fcd852 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
@@ -165,11 +165,6 @@
 
     double t = 0.0;
 
-    System.out.print(
-        "time, est_x, est_y, est_theta, true_x, true_y, true_theta, "
-            + "distance_1, distance_2, distance_3, distance_4, "
-            + "angle_1, angle_2, angle_3, angle_4\n");
-
     final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
 
     double maxError = Double.NEGATIVE_INFINITY;
@@ -219,24 +214,6 @@
                   .minus(trajectory.getInitialPose().getRotation()),
               positions);
 
-      System.out.printf(
-          "%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",
-          t,
-          xHat.getX(),
-          xHat.getY(),
-          xHat.getRotation().getRadians(),
-          groundTruthState.poseMeters.getX(),
-          groundTruthState.poseMeters.getY(),
-          groundTruthState.poseMeters.getRotation().getRadians(),
-          positions[0].distanceMeters,
-          positions[1].distanceMeters,
-          positions[2].distanceMeters,
-          positions[3].distanceMeters,
-          positions[0].angle.getRadians(),
-          positions[1].angle.getRadians(),
-          positions[2].angle.getRadians(),
-          positions[3].angle.getRadians());
-
       double error =
           groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
       if (error > maxError) {
@@ -263,4 +240,118 @@
       assertEquals(0.0, maxError, 0.2, "Incorrect max error");
     }
   }
+
+  @Test
+  void testSimultaneousVisionMeasurements() {
+    // This tests for multiple vision measurements appled at the same time. The expected behavior
+    // is that all measurements affect the estimated pose. The alternative result is that only one
+    // vision measurement affects the outcome. If that were the case, after 1000 measurements, the
+    // estimated pose would converge to that measurement.
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1),
+            new Translation2d(-1, 1));
+
+    var fl = new SwerveModulePosition();
+    var fr = new SwerveModulePosition();
+    var bl = new SwerveModulePosition();
+    var br = new SwerveModulePosition();
+
+    var estimator =
+        new SwerveDrivePoseEstimator(
+            kinematics,
+            new Rotation2d(),
+            new SwerveModulePosition[] {fl, fr, bl, br},
+            new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9));
+
+    estimator.updateWithTime(0, new Rotation2d(), new SwerveModulePosition[] {fl, fr, bl, br});
+
+    var visionMeasurements =
+        new Pose2d[] {
+          new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+          new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
+          new Pose2d(2, 4, Rotation2d.fromRadians(180)),
+        };
+
+    for (int i = 0; i < 1000; i++) {
+      for (var measurement : visionMeasurements) {
+        estimator.addVisionMeasurement(measurement, 0);
+      }
+    }
+
+    for (var measurement : visionMeasurements) {
+      var errorLog =
+          "Estimator converged to one vision measurement: "
+              + estimator.getEstimatedPosition().toString()
+              + " -> "
+              + measurement.toString();
+
+      var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
+      var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
+      var dtheta =
+          Math.abs(
+              measurement.getRotation().getDegrees()
+                  - estimator.getEstimatedPosition().getRotation().getDegrees());
+
+      assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
+    }
+  }
+
+  @Test
+  void testDiscardsOldVisionMeasurements() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(-1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1));
+    var estimator =
+        new SwerveDrivePoseEstimator(
+            kinematics,
+            new Rotation2d(),
+            new SwerveModulePosition[] {
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition(),
+              new SwerveModulePosition()
+            },
+            new Pose2d(),
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9));
+
+    double time = 0;
+
+    // Add enough measurements to fill up the buffer
+    for (; time < 4; time += 0.02) {
+      estimator.updateWithTime(
+          time,
+          new Rotation2d(),
+          new SwerveModulePosition[] {
+            new SwerveModulePosition(),
+            new SwerveModulePosition(),
+            new SwerveModulePosition(),
+            new SwerveModulePosition()
+          });
+    }
+
+    var odometryPose = estimator.getEstimatedPosition();
+
+    // Apply a vision measurement made 3 seconds ago
+    // This test passes if this does not cause a ConcurrentModificationException.
+    estimator.addVisionMeasurement(
+        new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
+        1,
+        VecBuilder.fill(0.1, 0.1, 0.1));
+
+    assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
+    assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
+    assertEquals(
+        odometryPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().getRadians(),
+        "Incorrect Final Theta");
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java
index 68babdd..4dc1826 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java
@@ -38,18 +38,12 @@
     assertEquals(
         transformTo.getTranslation(),
         CoordinateSystem.convert(transformFrom.getTranslation(), coordFrom, coordTo));
-    assertEquals(
-        transformTo.getRotation(),
-        CoordinateSystem.convert(transformFrom.getRotation(), coordFrom, coordTo));
     assertEquals(transformTo, CoordinateSystem.convert(transformFrom, coordFrom, coordTo));
 
     // "to" to "from"
     assertEquals(
         transformFrom.getTranslation(),
         CoordinateSystem.convert(transformTo.getTranslation(), coordTo, coordFrom));
-    assertEquals(
-        transformFrom.getRotation(),
-        CoordinateSystem.convert(transformTo.getRotation(), coordTo, coordFrom));
     assertEquals(transformFrom, CoordinateSystem.convert(transformTo, coordTo, coordFrom));
   }
 
@@ -158,9 +152,7 @@
     // No rotation from EDN to NWU
     checkTransform3dConvert(
         new Transform3d(new Translation3d(1.0, 2.0, 3.0), new Rotation3d()),
-        new Transform3d(
-            new Translation3d(3.0, -1.0, -2.0),
-            new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-90.0))),
+        new Transform3d(new Translation3d(3.0, -1.0, -2.0), new Rotation3d()),
         CoordinateSystem.EDN(),
         CoordinateSystem.NWU());
 
@@ -171,7 +163,7 @@
             new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
         new Transform3d(
             new Translation3d(3.0, -1.0, -2.0),
-            new Rotation3d(Units.degreesToRadians(-45.0), 0.0, Units.degreesToRadians(-90.0))),
+            new Rotation3d(0.0, Units.degreesToRadians(-45.0), 0.0)),
         CoordinateSystem.EDN(),
         CoordinateSystem.NWU());
 
@@ -182,7 +174,7 @@
             new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
         new Transform3d(
             new Translation3d(3.0, -1.0, -2.0),
-            new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-135.0))),
+            new Rotation3d(0.0, 0.0, Units.degreesToRadians(-45.0))),
         CoordinateSystem.EDN(),
         CoordinateSystem.NWU());
 
@@ -193,10 +185,7 @@
             new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
         new Transform3d(
             new Translation3d(3.0, -1.0, -2.0),
-            new Rotation3d(
-                Units.degreesToRadians(-90.0),
-                Units.degreesToRadians(45.0),
-                Units.degreesToRadians(-90.0))),
+            new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
         CoordinateSystem.EDN(),
         CoordinateSystem.NWU());
   }
@@ -206,9 +195,7 @@
     // No rotation from EDN to NED
     checkTransform3dConvert(
         new Transform3d(new Translation3d(1.0, 2.0, 3.0), new Rotation3d()),
-        new Transform3d(
-            new Translation3d(3.0, 1.0, 2.0),
-            new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(90.0))),
+        new Transform3d(new Translation3d(3.0, 1.0, 2.0), new Rotation3d()),
         CoordinateSystem.EDN(),
         CoordinateSystem.NED());
 
@@ -219,7 +206,7 @@
             new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
         new Transform3d(
             new Translation3d(3.0, 1.0, 2.0),
-            new Rotation3d(Units.degreesToRadians(135.0), 0.0, Units.degreesToRadians(90.0))),
+            new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
         CoordinateSystem.EDN(),
         CoordinateSystem.NED());
 
@@ -230,7 +217,7 @@
             new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
         new Transform3d(
             new Translation3d(3.0, 1.0, 2.0),
-            new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(135.0))),
+            new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
         CoordinateSystem.EDN(),
         CoordinateSystem.NED());
 
@@ -241,10 +228,7 @@
             new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
         new Transform3d(
             new Translation3d(3.0, 1.0, 2.0),
-            new Rotation3d(
-                Units.degreesToRadians(90.0),
-                Units.degreesToRadians(-45.0),
-                Units.degreesToRadians(90.0))),
+            new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
         CoordinateSystem.EDN(),
         CoordinateSystem.NED());
   }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
index 780c816..d57362d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
@@ -8,12 +8,35 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
+import java.util.List;
 import org.junit.jupiter.api.Test;
 
 class Pose2dTest {
   private static final double kEpsilon = 1E-9;
 
   @Test
+  void testRotateBy() {
+    final double x = 1.0;
+    final double y = 2.0;
+    var initial = new Pose2d(new Translation2d(x, y), Rotation2d.fromDegrees(45.0));
+
+    var rotation = Rotation2d.fromDegrees(5.0);
+    var rotated = initial.rotateBy(rotation);
+
+    // Translation is rotated by CCW rotation matrix
+    double c = rotation.getCos();
+    double s = rotation.getSin();
+    assertAll(
+        () -> assertEquals(c * x - s * y, rotated.getX(), kEpsilon),
+        () -> assertEquals(s * x + c * y, rotated.getY(), kEpsilon),
+        () ->
+            assertEquals(
+                initial.getRotation().getDegrees() + rotation.getDegrees(),
+                rotated.getRotation().getDegrees(),
+                kEpsilon));
+  }
+
+  @Test
   void testTransformBy() {
     var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
     var transformation = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0));
@@ -65,4 +88,50 @@
         () -> assertEquals(0.0, transform.getY(), kEpsilon),
         () -> assertEquals(0.0, transform.getRotation().getDegrees(), kEpsilon));
   }
+
+  @Test
+  void testNearest() {
+    var origin = new Pose2d();
+
+    // Distance sort
+    // each poseX is X units away from the origin at a random angle.
+    final var pose1 =
+        new Pose2d(new Translation2d(1, Rotation2d.fromDegrees(45)), new Rotation2d());
+    final var pose2 =
+        new Pose2d(new Translation2d(2, Rotation2d.fromDegrees(90)), new Rotation2d());
+    final var pose3 =
+        new Pose2d(new Translation2d(3, Rotation2d.fromDegrees(135)), new Rotation2d());
+    final var pose4 =
+        new Pose2d(new Translation2d(4, Rotation2d.fromDegrees(180)), new Rotation2d());
+    final var pose5 =
+        new Pose2d(new Translation2d(5, Rotation2d.fromDegrees(270)), new Rotation2d());
+
+    assertEquals(pose3, origin.nearest(List.of(pose5, pose3, pose4)));
+    assertEquals(pose1, origin.nearest(List.of(pose1, pose2, pose3)));
+    assertEquals(pose2, origin.nearest(List.of(pose4, pose2, pose3)));
+
+    // Rotation component sort (when distance is the same)
+    // Use the same translation because using different angles at the same distance can cause
+    // rounding error.
+    final var translation = new Translation2d(1, new Rotation2d());
+
+    final var poseA = new Pose2d(translation, Rotation2d.fromDegrees(0));
+    final var poseB = new Pose2d(translation, Rotation2d.fromDegrees(30));
+    final var poseC = new Pose2d(translation, Rotation2d.fromDegrees(120));
+    final var poseD = new Pose2d(translation, Rotation2d.fromDegrees(90));
+    final var poseE = new Pose2d(translation, Rotation2d.fromDegrees(-180));
+
+    assertEquals(
+        poseA, new Pose2d(0, 0, Rotation2d.fromDegrees(360)).nearest(List.of(poseA, poseB, poseD)));
+    assertEquals(
+        poseB,
+        new Pose2d(0, 0, Rotation2d.fromDegrees(-335)).nearest(List.of(poseB, poseC, poseD)));
+    assertEquals(
+        poseC,
+        new Pose2d(0, 0, Rotation2d.fromDegrees(-120)).nearest(List.of(poseB, poseC, poseD)));
+    assertEquals(
+        poseD, new Pose2d(0, 0, Rotation2d.fromDegrees(85)).nearest(List.of(poseA, poseC, poseD)));
+    assertEquals(
+        poseE, new Pose2d(0, 0, Rotation2d.fromDegrees(170)).nearest(List.of(poseA, poseD, poseE)));
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
index f13819f..ee1d98c 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
@@ -6,16 +6,50 @@
 
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.util.Units;
+import java.util.Arrays;
 import org.junit.jupiter.api.Test;
 
 class Pose3dTest {
   private static final double kEpsilon = 1E-9;
 
   @Test
+  void testRotateBy() {
+    final double x = 1.0;
+    final double y = 2.0;
+    var initial =
+        new Pose3d(
+            new Translation3d(x, y, 0.0),
+            new Rotation3d(
+                Units.degreesToRadians(0.0),
+                Units.degreesToRadians(0.0),
+                Units.degreesToRadians(45.0)));
+
+    double yaw = Units.degreesToRadians(5.0);
+    var rotation = new Rotation3d(Units.degreesToRadians(0.0), Units.degreesToRadians(0.0), yaw);
+    var rotated = initial.rotateBy(rotation);
+
+    // Translation is rotated by CCW rotation matrix
+    double c = Math.cos(yaw);
+    double s = Math.sin(yaw);
+    assertAll(
+        () -> assertEquals(c * x - s * y, rotated.getX(), kEpsilon),
+        () -> assertEquals(s * x + c * y, rotated.getY(), kEpsilon),
+        () -> assertEquals(0.0, rotated.getZ(), kEpsilon),
+        () -> assertEquals(0.0, rotated.getRotation().getX(), kEpsilon),
+        () -> assertEquals(0.0, rotated.getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                initial.getRotation().getZ() + rotation.getZ(),
+                rotated.getRotation().getZ(),
+                kEpsilon));
+  }
+
+  @Test
   void testTransformByRotations() {
     var initialPose =
         new Pose3d(
@@ -49,10 +83,13 @@
                 Units.degreesToRadians(-45.0),
                 Units.degreesToRadians(0.0)));
 
-    // This sequence of rotations should diverge from the origin and eventually return to it. When
-    // each rotation occurs, it should be performed intrinsicly, i.e. 'from the viewpoint' of and
+    // This sequence of rotations should diverge from the origin and eventually
+    // return to it. When
+    // each rotation occurs, it should be performed intrinsicly, i.e. 'from the
+    // viewpoint' of and
     // with
-    // the axes of the pose that is being transformed, just like how the translation is done 'from
+    // the axes of the pose that is being transformed, just like how the translation
+    // is done 'from
     // the
     // viewpoint' of the pose that is being transformed.
     var finalPose =
@@ -153,4 +190,115 @@
 
     assertEquals(expected, pose.toPose2d());
   }
+
+  @Test
+  void testComplexTwists() {
+    var initial_poses =
+        Arrays.asList(
+            new Pose3d(
+                new Translation3d(0.698303, -0.959096, 0.271076),
+                new Rotation3d(new Quaternion(0.86403, -0.076866, 0.147234, 0.475254))),
+            new Pose3d(
+                new Translation3d(0.634892, -0.765209, 0.117543),
+                new Rotation3d(new Quaternion(0.84987, -0.070829, 0.162097, 0.496415))),
+            new Pose3d(
+                new Translation3d(0.584827, -0.590303, -0.02557),
+                new Rotation3d(new Quaternion(0.832743, -0.041991, 0.202188, 0.513708))),
+            new Pose3d(
+                new Translation3d(0.505038, -0.451479, -0.112835),
+                new Rotation3d(new Quaternion(0.816515, -0.002673, 0.226182, 0.531166))),
+            new Pose3d(
+                new Translation3d(0.428178, -0.329692, -0.189707),
+                new Rotation3d(new Quaternion(0.807886, 0.029298, 0.257788, 0.529157))));
+
+    var final_poses =
+        Arrays.asList(
+            new Pose3d(
+                new Translation3d(-0.230448, -0.511957, 0.198406),
+                new Rotation3d(new Quaternion(0.753984, 0.347016, 0.409105, 0.379106))),
+            new Pose3d(
+                new Translation3d(-0.088932, -0.343253, 0.095018),
+                new Rotation3d(new Quaternion(0.638738, 0.413016, 0.536281, 0.365833))),
+            new Pose3d(
+                new Translation3d(-0.107908, -0.317552, 0.133946),
+                new Rotation3d(new Quaternion(0.653444, 0.417069, 0.465505, 0.427046))),
+            new Pose3d(
+                new Translation3d(-0.123383, -0.156411, -0.047435),
+                new Rotation3d(new Quaternion(0.652983, 0.40644, 0.431566, 0.47135))),
+            new Pose3d(
+                new Translation3d(-0.084654, -0.019305, -0.030022),
+                new Rotation3d(new Quaternion(0.620243, 0.429104, 0.479384, 0.44873))));
+
+    final var eps = 1E-5;
+    for (int i = 0; i < initial_poses.size(); i++) {
+      var start = initial_poses.get(i);
+      var end = final_poses.get(i);
+
+      var twist = start.log(end);
+      var start_exp = start.exp(twist);
+
+      assertAll(
+          () -> assertEquals(start_exp.getX(), end.getX(), eps),
+          () -> assertEquals(start_exp.getY(), end.getY(), eps),
+          () -> assertEquals(start_exp.getZ(), end.getZ(), eps),
+          () ->
+              assertEquals(
+                  start_exp.getRotation().getQuaternion().getW(),
+                  end.getRotation().getQuaternion().getW(),
+                  eps),
+          () ->
+              assertEquals(
+                  start_exp.getRotation().getQuaternion().getX(),
+                  end.getRotation().getQuaternion().getX(),
+                  eps),
+          () ->
+              assertEquals(
+                  start_exp.getRotation().getQuaternion().getY(),
+                  end.getRotation().getQuaternion().getY(),
+                  eps),
+          () ->
+              assertEquals(
+                  start_exp.getRotation().getQuaternion().getZ(),
+                  end.getRotation().getQuaternion().getZ(),
+                  eps));
+    }
+  }
+
+  @Test
+  void testTwistNaN() {
+    var initial_poses =
+        Arrays.asList(
+            new Pose3d(
+                new Translation3d(6.32, 4.12, 0.00),
+                new Rotation3d(
+                    new Quaternion(-0.9999999999999999, 0.0, 0.0, 1.9208309264993548E-8))),
+            new Pose3d(
+                new Translation3d(3.75, 2.95, 0.00),
+                new Rotation3d(
+                    new Quaternion(0.9999999999999793, 0.0, 0.0, 2.0352360299846772E-7))));
+    var final_poses =
+        Arrays.asList(
+            new Pose3d(
+                new Translation3d(6.33, 4.15, 0.00),
+                new Rotation3d(
+                    new Quaternion(-0.9999999999999999, 0.0, 0.0, 2.416890209039172E-8))),
+            new Pose3d(
+                new Translation3d(3.66, 2.93, 0.00),
+                new Rotation3d(
+                    new Quaternion(0.9999999999999782, 0.0, 0.0, 2.0859477994905617E-7))));
+
+    for (int i = 0; i < initial_poses.size(); i++) {
+      var start = initial_poses.get(i);
+      var end = final_poses.get(i);
+
+      var twist = start.log(end);
+      assertAll(
+          () -> assertFalse(((Double) twist.dx).isNaN()),
+          () -> assertFalse(((Double) twist.dy).isNaN()),
+          () -> assertFalse(((Double) twist.dz).isNaN()),
+          () -> assertFalse(((Double) twist.rx).isNaN()),
+          () -> assertFalse(((Double) twist.ry).isNaN()),
+          () -> assertFalse(((Double) twist.rz).isNaN()));
+    }
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java
index 7c7e103..458b14d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java
@@ -4,7 +4,9 @@
 
 package edu.wpi.first.math.geometry;
 
+import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
 import edu.wpi.first.math.util.Units;
 import org.junit.jupiter.api.Test;
@@ -14,37 +16,91 @@
   void testInit() {
     // Identity
     var q1 = new Quaternion();
-    assertEquals(1.0, q1.getW());
-    assertEquals(0.0, q1.getX());
-    assertEquals(0.0, q1.getY());
-    assertEquals(0.0, q1.getZ());
+    assertAll(
+        () -> assertEquals(1.0, q1.getW()),
+        () -> assertEquals(0.0, q1.getX()),
+        () -> assertEquals(0.0, q1.getY()),
+        () -> assertEquals(0.0, q1.getZ()));
 
     // Normalized
     var q2 = new Quaternion(0.5, 0.5, 0.5, 0.5);
-    assertEquals(0.5, q2.getW());
-    assertEquals(0.5, q2.getX());
-    assertEquals(0.5, q2.getY());
-    assertEquals(0.5, q2.getZ());
+    assertAll(
+        () -> assertEquals(0.5, q2.getW()),
+        () -> assertEquals(0.5, q2.getX()),
+        () -> assertEquals(0.5, q2.getY()),
+        () -> assertEquals(0.5, q2.getZ()));
 
     // Unnormalized
     var q3 = new Quaternion(0.75, 0.3, 0.4, 0.5);
-    assertEquals(0.75, q3.getW());
-    assertEquals(0.3, q3.getX());
-    assertEquals(0.4, q3.getY());
-    assertEquals(0.5, q3.getZ());
+    assertAll(
+        () -> assertEquals(0.75, q3.getW()),
+        () -> assertEquals(0.3, q3.getX()),
+        () -> assertEquals(0.4, q3.getY()),
+        () -> assertEquals(0.5, q3.getZ()));
 
-    q3 = q3.normalize();
+    var q3_norm = q3.normalize();
     double norm = Math.sqrt(0.75 * 0.75 + 0.3 * 0.3 + 0.4 * 0.4 + 0.5 * 0.5);
-    assertEquals(0.75 / norm, q3.getW());
-    assertEquals(0.3 / norm, q3.getX());
-    assertEquals(0.4 / norm, q3.getY());
-    assertEquals(0.5 / norm, q3.getZ());
-    assertEquals(
-        1.0,
-        q3.getW() * q3.getW()
-            + q3.getX() * q3.getX()
-            + q3.getY() * q3.getY()
-            + q3.getZ() * q3.getZ());
+    assertAll(
+        () -> assertEquals(0.75 / norm, q3_norm.getW()),
+        () -> assertEquals(0.3 / norm, q3_norm.getX()),
+        () -> assertEquals(0.4 / norm, q3_norm.getY()),
+        () -> assertEquals(0.5 / norm, q3_norm.getZ()),
+        () -> assertEquals(1.0, q3_norm.dot(q3_norm)));
+  }
+
+  @Test
+  void testAddition() {
+    var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
+    var p = new Quaternion(0.5, 0.6, 0.7, 0.8);
+
+    var sum = q.plus(p);
+    assertAll(
+        () -> assertEquals(q.getW() + p.getW(), sum.getW()),
+        () -> assertEquals(q.getX() + p.getX(), sum.getX()),
+        () -> assertEquals(q.getY() + p.getY(), sum.getY()),
+        () -> assertEquals(q.getZ() + p.getZ(), sum.getZ()));
+  }
+
+  @Test
+  void testSubtraction() {
+    var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
+    var p = new Quaternion(0.5, 0.6, 0.7, 0.8);
+
+    var difference = q.minus(p);
+
+    assertAll(
+        () -> assertEquals(q.getW() - p.getW(), difference.getW()),
+        () -> assertEquals(q.getX() - p.getX(), difference.getX()),
+        () -> assertEquals(q.getY() - p.getY(), difference.getY()),
+        () -> assertEquals(q.getZ() - p.getZ(), difference.getZ()));
+  }
+
+  @Test
+  void testScalarMultiplication() {
+    var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
+    var scalar = 2;
+
+    var product = q.times(scalar);
+
+    assertAll(
+        () -> assertEquals(q.getW() * scalar, product.getW()),
+        () -> assertEquals(q.getX() * scalar, product.getX()),
+        () -> assertEquals(q.getY() * scalar, product.getY()),
+        () -> assertEquals(q.getZ() * scalar, product.getZ()));
+  }
+
+  @Test
+  void testScalarDivision() {
+    var q = new Quaternion(0.1, 0.2, 0.3, 0.4);
+    var scalar = 2;
+
+    var product = q.divide(scalar);
+
+    assertAll(
+        () -> assertEquals(q.getW() / scalar, product.getW()),
+        () -> assertEquals(q.getX() / scalar, product.getX()),
+        () -> assertEquals(q.getY() / scalar, product.getY()),
+        () -> assertEquals(q.getZ() / scalar, product.getZ()));
   }
 
   @Test
@@ -59,31 +115,131 @@
     // 90° CCW X rotation, 90° CCW Y rotation, and 90° CCW Z rotation should
     // produce a 90° CCW Y rotation
     var expected = yRot;
-    var actual = zRot.times(yRot).times(xRot);
-    assertEquals(expected.getW(), actual.getW(), 1e-9);
-    assertEquals(expected.getX(), actual.getX(), 1e-9);
-    assertEquals(expected.getY(), actual.getY(), 1e-9);
-    assertEquals(expected.getZ(), actual.getZ(), 1e-9);
+    final var actual = zRot.times(yRot).times(xRot);
+    assertAll(
+        () -> assertEquals(expected.getW(), actual.getW(), 1e-9),
+        () -> assertEquals(expected.getX(), actual.getX(), 1e-9),
+        () -> assertEquals(expected.getY(), actual.getY(), 1e-9),
+        () -> assertEquals(expected.getZ(), actual.getZ(), 1e-9));
 
     // Identity
     var q =
         new Quaternion(
             0.72760687510899891, 0.29104275004359953, 0.38805700005813276, 0.48507125007266594);
-    actual = q.times(q.inverse());
-    assertEquals(1.0, actual.getW());
-    assertEquals(0.0, actual.getX());
-    assertEquals(0.0, actual.getY());
-    assertEquals(0.0, actual.getZ());
+    final var actual2 = q.times(q.inverse());
+    assertAll(
+        () -> assertEquals(1.0, actual2.getW()),
+        () -> assertEquals(0.0, actual2.getX()),
+        () -> assertEquals(0.0, actual2.getY()),
+        () -> assertEquals(0.0, actual2.getZ()));
+  }
+
+  @Test
+  void testConjugate() {
+    var q = new Quaternion(0.75, 0.3, 0.4, 0.5);
+    var inv = q.conjugate();
+
+    assertAll(
+        () -> assertEquals(q.getW(), inv.getW()),
+        () -> assertEquals(-q.getX(), inv.getX()),
+        () -> assertEquals(-q.getY(), inv.getY()),
+        () -> assertEquals(-q.getZ(), inv.getZ()));
   }
 
   @Test
   void testInverse() {
     var q = new Quaternion(0.75, 0.3, 0.4, 0.5);
     var inv = q.inverse();
+    var norm = q.norm();
 
-    assertEquals(q.getW(), inv.getW());
-    assertEquals(-q.getX(), inv.getX());
-    assertEquals(-q.getY(), inv.getY());
-    assertEquals(-q.getZ(), inv.getZ());
+    assertAll(
+        () -> assertEquals(q.getW() / (norm * norm), inv.getW(), 1e-10),
+        () -> assertEquals(-q.getX() / (norm * norm), inv.getX(), 1e-10),
+        () -> assertEquals(-q.getY() / (norm * norm), inv.getY(), 1e-10),
+        () -> assertEquals(-q.getZ() / (norm * norm), inv.getZ(), 1e-10));
+  }
+
+  @Test
+  void testNorm() {
+    var q = new Quaternion(3, 4, 12, 84);
+
+    // pythagorean triples (3, 4, 5), (5, 12, 13), (13, 84, 85)
+    assertEquals(q.norm(), 85, 1e-10);
+  }
+
+  @Test
+  void testExponential() {
+    var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+    var q_exp =
+        new Quaternion(
+            2.81211398529184, -0.392521193481878, -0.588781790222817, -0.785042386963756);
+
+    assertEquals(q_exp, q.exp());
+  }
+
+  @Test
+  void testLogarithm() {
+    var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+    var q_log =
+        new Quaternion(1.7959088706354, 0.515190292664085, 0.772785438996128, 1.03038058532817);
+
+    assertEquals(q_log, q.log());
+
+    var zero = new Quaternion(0, 0, 0, 0);
+    var one = new Quaternion();
+
+    assertEquals(zero, one.log());
+
+    var i = new Quaternion(0, 1, 0, 0);
+    assertEquals(i.times(Math.PI / 2), i.log());
+
+    var j = new Quaternion(0, 0, 1, 0);
+    assertEquals(j.times(Math.PI / 2), j.log());
+
+    var k = new Quaternion(0, 0, 0, 1);
+    assertEquals(k.times(Math.PI / 2), k.log());
+    assertEquals(i.times(-Math.PI), one.times(-1).log());
+
+    var ln_half = Math.log(0.5);
+    assertEquals(new Quaternion(ln_half, -Math.PI, 0, 0), one.times(-0.5).log());
+  }
+
+  @Test
+  void testLogarithmIsInverseOfExponential() {
+    var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+
+    // These operations are order-dependent: ln(exp(q)) is congruent
+    // but not necessarily equal to exp(ln(q)) due to the multi-valued nature of the complex
+    // logarithm.
+
+    var q_log_exp = q.log().exp();
+
+    assertEquals(q, q_log_exp);
+
+    var start = new Quaternion(1, 2, 3, 4);
+    var expect = new Quaternion(5, 6, 7, 8);
+
+    var twist = start.log(expect);
+    var actual = start.exp(twist);
+
+    assertEquals(expect, actual);
+  }
+
+  @Test
+  void testDotProduct() {
+    var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+    var p = new Quaternion(5.5, 6.6, 7.7, 8.8);
+
+    assertEquals(
+        q.getW() * p.getW() + q.getX() * p.getX() + q.getY() * p.getY() + q.getZ() * p.getZ(),
+        q.dot(p));
+  }
+
+  @Test
+  void testDotProductAsEquality() {
+    var q = new Quaternion(1.1, 2.2, 3.3, 4.4);
+    var q_conj = q.conjugate();
+
+    assertAll(() -> assertEquals(q, q), () -> assertNotEquals(q, q_conj));
   }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
index 072a2e6..d80344d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
@@ -20,21 +20,60 @@
   private static final double kEpsilon = 1E-9;
 
   @Test
+  void testGimbalLockAccuracy() {
+    var rot1 = new Rotation3d(0, 0, Math.PI / 2);
+    var rot2 = new Rotation3d(Math.PI, 0, 0);
+    var rot3 = new Rotation3d(-Math.PI / 2, 0, 0);
+    final var result1 = rot1.plus(rot2).plus(rot3);
+    final var expected1 = new Rotation3d(0, -Math.PI / 2, Math.PI / 2);
+    assertAll(
+        () -> assertEquals(expected1, result1),
+        () -> assertEquals(Math.PI / 2, result1.getX() + result1.getZ(), kEpsilon),
+        () -> assertEquals(-Math.PI / 2, result1.getY(), kEpsilon));
+
+    rot1 = new Rotation3d(0, 0, Math.PI / 2);
+    rot2 = new Rotation3d(-Math.PI, 0, 0);
+    rot3 = new Rotation3d(Math.PI / 2, 0, 0);
+    final var result2 = rot1.plus(rot2).plus(rot3);
+    final var expected2 = new Rotation3d(0, Math.PI / 2, Math.PI / 2);
+    assertAll(
+        () -> assertEquals(expected2, result2),
+        () -> assertEquals(Math.PI / 2, result2.getZ() - result2.getX(), kEpsilon),
+        () -> assertEquals(Math.PI / 2, result2.getY(), kEpsilon));
+
+    rot1 = new Rotation3d(0, 0, Math.PI / 2);
+    rot2 = new Rotation3d(0, Math.PI / 3, 0);
+    rot3 = new Rotation3d(-Math.PI / 2, 0, 0);
+    final var result3 = rot1.plus(rot2).plus(rot3);
+    final var expected3 = new Rotation3d(0, Math.PI / 2, Math.PI / 6);
+    assertAll(
+        () -> assertEquals(expected3, result3),
+        () -> assertEquals(Math.PI / 6, result3.getZ() - result3.getX(), kEpsilon),
+        () -> assertEquals(Math.PI / 2, result3.getY(), kEpsilon));
+  }
+
+  @Test
   void testInitAxisAngleAndRollPitchYaw() {
     final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
     final var rot1 = new Rotation3d(xAxis, Math.PI / 3);
     final var rot2 = new Rotation3d(Math.PI / 3, 0.0, 0.0);
+    final var rvec1 = new Rotation3d(xAxis.times(Math.PI / 3));
     assertEquals(rot1, rot2);
+    assertEquals(rot1, rvec1);
 
     final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
     final var rot3 = new Rotation3d(yAxis, Math.PI / 3);
     final var rot4 = new Rotation3d(0.0, Math.PI / 3, 0.0);
+    final var rvec2 = new Rotation3d(yAxis.times(Math.PI / 3));
     assertEquals(rot3, rot4);
+    assertEquals(rot3, rvec2);
 
     final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
     final var rot5 = new Rotation3d(zAxis, Math.PI / 3);
     final var rot6 = new Rotation3d(0.0, 0.0, Math.PI / 3);
+    final var rvec3 = new Rotation3d(zAxis.times(Math.PI / 3));
     assertEquals(rot5, rot6);
+    assertEquals(rot5, rvec3);
   }
 
   @Test
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
index c8f5690..6ed8a61 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
@@ -8,6 +8,7 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
+import java.util.List;
 import org.junit.jupiter.api.Test;
 
 class Translation2dTest {
@@ -114,4 +115,20 @@
         () -> assertEquals(1.0, two.getX(), kEpsilon),
         () -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon));
   }
+
+  @Test
+  void testNearest() {
+    var origin = new Translation2d();
+
+    // each translationX is X units away from the origin at a random angle.
+    var translation1 = new Translation2d(1, Rotation2d.fromDegrees(45));
+    var translation2 = new Translation2d(2, Rotation2d.fromDegrees(90));
+    var translation3 = new Translation2d(3, Rotation2d.fromDegrees(135));
+    var translation4 = new Translation2d(4, Rotation2d.fromDegrees(180));
+    var translation5 = new Translation2d(5, Rotation2d.fromDegrees(270));
+
+    assertEquals(origin.nearest(List.of(translation5, translation3, translation4)), translation3);
+    assertEquals(origin.nearest(List.of(translation1, translation2, translation3)), translation1);
+    assertEquals(origin.nearest(List.of(translation4, translation2, translation3)), translation2);
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMapTest.java b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMapTest.java
new file mode 100644
index 0000000..fb74b92
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMapTest.java
@@ -0,0 +1,76 @@
+// 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.interpolation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class InterpolatingDoubleTreeMapTest {
+  @Test
+  void testInterpolationDouble() {
+    InterpolatingDoubleTreeMap table = new InterpolatingDoubleTreeMap();
+
+    table.put(125.0, 450.0);
+    table.put(200.0, 510.0);
+    table.put(268.0, 525.0);
+    table.put(312.0, 550.0);
+    table.put(326.0, 650.0);
+
+    // Key below minimum gives the smallest value
+    assertEquals(450.0, table.get(100.0));
+
+    // Minimum key gives exact value
+    assertEquals(450.0, table.get(125.0));
+
+    // Key gives interpolated value
+    assertEquals(480.0, table.get(162.5));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(510.0, table.get(200.0));
+
+    // Maximum key gives exact value
+    assertEquals(650.0, table.get(326.0));
+
+    // Key above maximum gives largest value
+    assertEquals(650.0, table.get(400.0));
+  }
+
+  @Test
+  void testInterpolationClear() {
+    InterpolatingDoubleTreeMap table = new InterpolatingDoubleTreeMap();
+
+    table.put(125.0, 450.0);
+    table.put(200.0, 510.0);
+    table.put(268.0, 525.0);
+    table.put(312.0, 550.0);
+    table.put(326.0, 650.0);
+
+    // Key below minimum gives the smallest value
+    assertEquals(450.0, table.get(100.0));
+
+    // Minimum key gives exact value
+    assertEquals(450.0, table.get(125.0));
+
+    // Key gives interpolated value
+    assertEquals(480.0, table.get(162.5));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(510.0, table.get(200.0));
+
+    // Maximum key gives exact value
+    assertEquals(650.0, table.get(326.0));
+
+    // Key above maximum gives largest value
+    assertEquals(650.0, table.get(400.0));
+
+    table.clear();
+
+    table.put(100.0, 250.0);
+    table.put(200.0, 500.0);
+
+    assertEquals(375.0, table.get(150.0));
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingTreeMapTest.java b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingTreeMapTest.java
new file mode 100644
index 0000000..b04b40a
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/interpolation/InterpolatingTreeMapTest.java
@@ -0,0 +1,48 @@
+// 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.interpolation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class InterpolatingTreeMapTest {
+  @Test
+  void testInterpolation() {
+    InterpolatingTreeMap<Double, Double> table =
+        new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Interpolator.forDouble());
+
+    table.put(125.0, 450.0);
+    table.put(200.0, 510.0);
+    table.put(268.0, 525.0);
+    table.put(312.0, 550.0);
+    table.put(326.0, 650.0);
+
+    // Key below minimum gives the smallest value
+    assertEquals(450.0, table.get(100.0));
+
+    // Minimum key gives exact value
+    assertEquals(450.0, table.get(125.0));
+
+    // Key gives interpolated value
+    assertEquals(480.0, table.get(162.5));
+
+    // Key at right of interpolation range gives exact value
+    assertEquals(510.0, table.get(200.0));
+
+    // Maximum key gives exact value
+    assertEquals(650.0, table.get(326.0));
+
+    // Key above maximum gives largest value
+    assertEquals(650.0, table.get(400.0));
+
+    table.clear();
+
+    table.put(100.0, 250.0);
+    table.put(200.0, 500.0);
+
+    assertEquals(375.0, table.get(150.0));
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
index b9c3785..2401342 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
@@ -7,14 +7,45 @@
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
 import org.junit.jupiter.api.Test;
 
 class ChassisSpeedsTest {
   private static final double kEpsilon = 1E-9;
 
   @Test
-  void testFieldRelativeConstruction() {
+  void testDiscretize() {
+    final var target = new ChassisSpeeds(1.0, 0.0, 0.5);
+    final var duration = 1.0;
+    final var dt = 0.01;
+
+    final var speeds = ChassisSpeeds.discretize(target, duration);
+    final var twist =
+        new Twist2d(
+            speeds.vxMetersPerSecond * dt,
+            speeds.vyMetersPerSecond * dt,
+            speeds.omegaRadiansPerSecond * dt);
+
+    var pose = new Pose2d();
+    for (double time = 0; time < duration; time += dt) {
+      pose = pose.exp(twist);
+    }
+
+    final var result = pose; // For lambda capture
+    assertAll(
+        () -> assertEquals(target.vxMetersPerSecond * duration, result.getX(), kEpsilon),
+        () -> assertEquals(target.vyMetersPerSecond * duration, result.getY(), kEpsilon),
+        () ->
+            assertEquals(
+                target.omegaRadiansPerSecond * duration,
+                result.getRotation().getRadians(),
+                kEpsilon));
+  }
+
+  @Test
+  void testFromFieldRelativeSpeeds() {
     final var chassisSpeeds =
         ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0));
 
@@ -23,4 +54,71 @@
         () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
         () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
   }
+
+  @Test
+  void testFromRobotRelativeSpeeds() {
+    final var chassisSpeeds =
+        ChassisSpeeds.fromRobotRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(45.0));
+
+    assertAll(
+        () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
+  }
+
+  @Test
+  void testPlus() {
+    final var left = new ChassisSpeeds(1.0, 0.5, 0.75);
+    final var right = new ChassisSpeeds(2.0, 1.5, 0.25);
+
+    final var chassisSpeeds = left.plus(right);
+
+    assertAll(
+        () -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond),
+        () -> assertEquals(2.0, chassisSpeeds.vyMetersPerSecond),
+        () -> assertEquals(1.0, chassisSpeeds.omegaRadiansPerSecond));
+  }
+
+  @Test
+  void testMinus() {
+    final var left = new ChassisSpeeds(1.0, 0.5, 0.75);
+    final var right = new ChassisSpeeds(2.0, 0.5, 0.25);
+
+    final var chassisSpeeds = left.minus(right);
+
+    assertAll(
+        () -> assertEquals(-1.0, chassisSpeeds.vxMetersPerSecond),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond),
+        () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond));
+  }
+
+  @Test
+  void testUnaryMinus() {
+    final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).unaryMinus();
+
+    assertAll(
+        () -> assertEquals(-1.0, chassisSpeeds.vxMetersPerSecond),
+        () -> assertEquals(-0.5, chassisSpeeds.vyMetersPerSecond),
+        () -> assertEquals(-0.75, chassisSpeeds.omegaRadiansPerSecond));
+  }
+
+  @Test
+  void testMultiplication() {
+    final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).times(2.0);
+
+    assertAll(
+        () -> assertEquals(2.0, chassisSpeeds.vxMetersPerSecond),
+        () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond),
+        () -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond));
+  }
+
+  @Test
+  void testDivision() {
+    final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).div(2.0);
+
+    assertAll(
+        () -> assertEquals(0.5, chassisSpeeds.vxMetersPerSecond),
+        () -> assertEquals(0.25, chassisSpeeds.vyMetersPerSecond),
+        () -> assertEquals(0.375, chassisSpeeds.omegaRadiansPerSecond));
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java
new file mode 100644
index 0000000..7f26180
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java
@@ -0,0 +1,63 @@
+// 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.kinematics;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveWheelSpeedsTest {
+  @Test
+  void testPlus() {
+    final var left = new DifferentialDriveWheelSpeeds(1.0, 0.5);
+    final var right = new DifferentialDriveWheelSpeeds(2.0, 1.5);
+
+    final var wheelSpeeds = left.plus(right);
+
+    assertAll(
+        () -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond),
+        () -> assertEquals(2.0, wheelSpeeds.rightMetersPerSecond));
+  }
+
+  @Test
+  void testMinus() {
+    final var left = new DifferentialDriveWheelSpeeds(1.0, 0.5);
+    final var right = new DifferentialDriveWheelSpeeds(2.0, 0.5);
+
+    final var wheelSpeeds = left.minus(right);
+
+    assertAll(
+        () -> assertEquals(-1.0, wheelSpeeds.leftMetersPerSecond),
+        () -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond));
+  }
+
+  @Test
+  void testUnaryMinus() {
+    final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).unaryMinus();
+
+    assertAll(
+        () -> assertEquals(-1.0, wheelSpeeds.leftMetersPerSecond),
+        () -> assertEquals(-0.5, wheelSpeeds.rightMetersPerSecond));
+  }
+
+  @Test
+  void testMultiplication() {
+    final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).times(2.0);
+
+    assertAll(
+        () -> assertEquals(2.0, wheelSpeeds.leftMetersPerSecond),
+        () -> assertEquals(1.0, wheelSpeeds.rightMetersPerSecond));
+  }
+
+  @Test
+  void testDivision() {
+    final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).div(2.0);
+
+    assertAll(
+        () -> assertEquals(0.5, wheelSpeeds.leftMetersPerSecond),
+        () -> assertEquals(0.25, wheelSpeeds.rightMetersPerSecond));
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java
new file mode 100644
index 0000000..22efb44
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java
@@ -0,0 +1,73 @@
+// 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.kinematics;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveWheelSpeedsTest {
+  @Test
+  void testPlus() {
+    final var left = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5);
+    final var right = new MecanumDriveWheelSpeeds(2.0, 1.5, 0.5, 1.0);
+
+    final var wheelSpeeds = left.plus(right);
+
+    assertAll(
+        () -> assertEquals(3.0, wheelSpeeds.frontLeftMetersPerSecond),
+        () -> assertEquals(2.0, wheelSpeeds.frontRightMetersPerSecond),
+        () -> assertEquals(2.5, wheelSpeeds.rearLeftMetersPerSecond),
+        () -> assertEquals(2.5, wheelSpeeds.rearRightMetersPerSecond));
+  }
+
+  @Test
+  void testMinus() {
+    final var left = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5);
+    final var right = new MecanumDriveWheelSpeeds(2.0, 0.5, 0.5, 1.0);
+
+    final var wheelSpeeds = left.minus(right);
+
+    assertAll(
+        () -> assertEquals(-1.0, wheelSpeeds.frontLeftMetersPerSecond),
+        () -> assertEquals(0.0, wheelSpeeds.frontRightMetersPerSecond),
+        () -> assertEquals(1.5, wheelSpeeds.rearLeftMetersPerSecond),
+        () -> assertEquals(0.5, wheelSpeeds.rearRightMetersPerSecond));
+  }
+
+  @Test
+  void testUnaryMinus() {
+    final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).unaryMinus();
+
+    assertAll(
+        () -> assertEquals(-1.0, wheelSpeeds.frontLeftMetersPerSecond),
+        () -> assertEquals(-0.5, wheelSpeeds.frontRightMetersPerSecond),
+        () -> assertEquals(-2.0, wheelSpeeds.rearLeftMetersPerSecond),
+        () -> assertEquals(-1.5, wheelSpeeds.rearRightMetersPerSecond));
+  }
+
+  @Test
+  void testMultiplication() {
+    final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).times(2.0);
+
+    assertAll(
+        () -> assertEquals(2.0, wheelSpeeds.frontLeftMetersPerSecond),
+        () -> assertEquals(1.0, wheelSpeeds.frontRightMetersPerSecond),
+        () -> assertEquals(4.0, wheelSpeeds.rearLeftMetersPerSecond),
+        () -> assertEquals(3.0, wheelSpeeds.rearRightMetersPerSecond));
+  }
+
+  @Test
+  void testDivision() {
+    final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).div(2.0);
+
+    assertAll(
+        () -> assertEquals(0.5, wheelSpeeds.frontLeftMetersPerSecond),
+        () -> assertEquals(0.25, wheelSpeeds.frontRightMetersPerSecond),
+        () -> assertEquals(1.0, wheelSpeeds.rearLeftMetersPerSecond),
+        () -> assertEquals(0.75, wheelSpeeds.rearRightMetersPerSecond));
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
index 43dd02a..f429a68 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
@@ -120,6 +120,28 @@
   }
 
   @Test
+  void testResetWheelAngle() {
+    Rotation2d fl = new Rotation2d(0);
+    Rotation2d fr = new Rotation2d(Math.PI / 2);
+    Rotation2d bl = new Rotation2d(Math.PI);
+    Rotation2d br = new Rotation2d(3 * Math.PI / 2);
+    m_kinematics.resetHeadings(fl, fr, bl, br);
+    var moduleStates = m_kinematics.toSwerveModuleStates(new ChassisSpeeds());
+
+    // Robot is stationary, but module angles are preserved.
+
+    assertAll(
+        () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(180.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(270.0, moduleStates[3].angle.getDegrees(), kEpsilon));
+  }
+
+  @Test
   void testTurnInPlaceInverseKinematics() {
     ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
     var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
@@ -371,4 +393,21 @@
         () -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
         () -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon));
   }
+
+  @Test
+  void testDesaturateNegativeSpeed() {
+    SwerveModuleState fl = new SwerveModuleState(1, new Rotation2d());
+    SwerveModuleState fr = new SwerveModuleState(1, new Rotation2d());
+    SwerveModuleState bl = new SwerveModuleState(-2, new Rotation2d());
+    SwerveModuleState br = new SwerveModuleState(-2, new Rotation2d());
+
+    SwerveModuleState[] arr = {fl, fr, bl, br};
+    SwerveDriveKinematics.desaturateWheelSpeeds(arr, 1);
+
+    assertAll(
+        () -> assertEquals(0.5, arr[0].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.5, arr[1].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(-1.0, arr[2].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(-1.0, arr[3].speedMetersPerSecond, kEpsilon));
+  }
 }
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
index 2182674..7f62933 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
@@ -122,98 +122,6 @@
             + discQIntegrated);
   }
 
-  // Test that the Taylor series discretization produces nearly identical results.
-  @Test
-  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);
-    }
-
-    //       T
-    // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-    //       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
-  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);
-    }
-
-    //       T
-    // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-    //       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
   void testDiscretizeR() {
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 72fb5ea..9b47148 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
@@ -31,6 +31,20 @@
   }
 
   @Test
+  void testZeroRKDP() {
+    var y1 =
+        NumericalIntegration.rkdp(
+            (x, u) -> {
+              return VecBuilder.fill(0);
+            },
+            VecBuilder.fill(0),
+            VecBuilder.fill(0),
+            0.1);
+
+    assertEquals(0.0, y1.get(0, 0), 1e-3);
+  }
+
+  @Test
   void testExponentialRKDP() {
     Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
 
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java
new file mode 100644
index 0000000..e9c535e
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java
@@ -0,0 +1,364 @@
+// 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.trajectory;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class ExponentialProfileTest {
+  private static final double kDt = 0.01;
+  private static final SimpleMotorFeedforward feedforward =
+      new SimpleMotorFeedforward(0, 2.5629, 0.43277);
+  private static final ExponentialProfile.Constraints constraints =
+      ExponentialProfile.Constraints.fromCharacteristics(12, 2.5629, 0.43277);
+
+  /**
+   * Asserts "val1" is within "eps" of "val2".
+   *
+   * @param val1 First operand in comparison.
+   * @param val2 Second operand in comparison.
+   * @param eps Tolerance for whether values are near to each other.
+   */
+  private static void assertNear(double val1, double val2, double eps) {
+    assertTrue(
+        Math.abs(val1 - val2) <= eps,
+        "Difference between " + val1 + " and " + val2 + " is greater than " + eps);
+  }
+
+  private static void assertNear(
+      ExponentialProfile.State val1, ExponentialProfile.State val2, double eps) {
+    assertAll(
+        () -> assertNear(val1.position, val2.position, eps),
+        () -> assertNear(val1.position, val2.position, eps));
+  }
+
+  private static ExponentialProfile.State checkDynamics(
+      ExponentialProfile profile, ExponentialProfile.State current, ExponentialProfile.State goal) {
+    var next = profile.calculate(kDt, current, goal);
+
+    var signal = feedforward.calculate(current.velocity, next.velocity, kDt);
+
+    assertTrue(Math.abs(signal) < constraints.maxInput + 1e-9);
+
+    return next;
+  }
+
+  @Test
+  void reachesGoal() {
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+
+    ExponentialProfile.State goal = new ExponentialProfile.State(10, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    for (int i = 0; i < 450; ++i) {
+      state = checkDynamics(profile, state, goal);
+    }
+    assertEquals(state, goal);
+  }
+
+  // Tests that decreasing the maximum velocity in the middle when it is already
+  // moving faster than the new max is handled correctly
+  @Test
+  void posContinuousUnderVelChange() {
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+
+    ExponentialProfile.State goal = new ExponentialProfile.State(10, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    for (int i = 0; i < 300; ++i) {
+      if (i == 150) {
+        profile =
+            new ExponentialProfile(
+                ExponentialProfile.Constraints.fromStateSpace(9, constraints.A, constraints.B));
+      }
+
+      state = checkDynamics(profile, state, goal);
+    }
+    assertEquals(state, goal);
+  }
+
+  // Tests that decreasing the maximum velocity in the middle when it is already
+  // moving faster than the new max is handled correctly
+  @Test
+  void posContinuousUnderVelChangeBackward() {
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+
+    ExponentialProfile.State goal = new ExponentialProfile.State(-10, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    for (int i = 0; i < 300; ++i) {
+      if (i == 150) {
+        profile =
+            new ExponentialProfile(
+                ExponentialProfile.Constraints.fromStateSpace(9, constraints.A, constraints.B));
+      }
+
+      state = checkDynamics(profile, state, goal);
+    }
+    assertEquals(state, goal);
+  }
+
+  // There is some somewhat tricky code for dealing with going backwards
+  @Test
+  void backwards() {
+    ExponentialProfile.State goal = new ExponentialProfile.State(-10, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+
+    for (int i = 0; i < 400; ++i) {
+      state = checkDynamics(profile, state, goal);
+    }
+    assertEquals(state, goal);
+  }
+
+  @Test
+  void switchGoalInMiddle() {
+    ExponentialProfile.State goal = new ExponentialProfile.State(-10, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+    for (int i = 0; i < 50; ++i) {
+      state = checkDynamics(profile, state, goal);
+    }
+    assertNotEquals(state, goal);
+
+    goal = new ExponentialProfile.State(0.0, 0.0);
+    for (int i = 0; i < 100; ++i) {
+      state = checkDynamics(profile, state, goal);
+    }
+    assertEquals(state, goal);
+  }
+
+  // Checks to make sure that it hits top speed
+  @Test
+  void topSpeed() {
+    ExponentialProfile.State goal = new ExponentialProfile.State(40, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+    double maxSpeed = 0;
+    for (int i = 0; i < 900; ++i) {
+      state = checkDynamics(profile, state, goal);
+      maxSpeed = Math.max(maxSpeed, state.velocity);
+    }
+
+    assertNear(constraints.maxVelocity(), maxSpeed, 10e-5);
+    assertEquals(state, goal);
+  }
+
+  @Test
+  void topSpeedBackward() {
+    ExponentialProfile.State goal = new ExponentialProfile.State(-40, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+    double maxSpeed = 0;
+    for (int i = 0; i < 900; ++i) {
+      state = checkDynamics(profile, state, goal);
+      maxSpeed = Math.min(maxSpeed, state.velocity);
+    }
+
+    assertNear(-constraints.maxVelocity(), maxSpeed, 10e-5);
+    assertEquals(state, goal);
+  }
+
+  @Test
+  void largeInitialVelocity() {
+    ExponentialProfile.State goal = new ExponentialProfile.State(40, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 8);
+
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+    for (int i = 0; i < 900; ++i) {
+      state = checkDynamics(profile, state, goal);
+    }
+
+    assertEquals(state, goal);
+  }
+
+  @Test
+  void largeNegativeInitialVelocity() {
+    ExponentialProfile.State goal = new ExponentialProfile.State(-40, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, -8);
+
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+    for (int i = 0; i < 900; ++i) {
+      state = checkDynamics(profile, state, goal);
+    }
+
+    assertEquals(state, goal);
+  }
+
+  @SuppressWarnings("PMD.TestClassWithoutTestCases")
+  static class TestCase {
+    public final ExponentialProfile.State initial;
+    public final ExponentialProfile.State goal;
+    public final ExponentialProfile.State inflectionPoint;
+
+    TestCase(
+        ExponentialProfile.State initial,
+        ExponentialProfile.State goal,
+        ExponentialProfile.State inflectionPoint) {
+      this.initial = initial;
+      this.goal = goal;
+      this.inflectionPoint = inflectionPoint;
+    }
+  }
+
+  @Test
+  void testHeuristic() {
+    List<TestCase> testCases =
+        List.of(
+            new TestCase(
+                new ExponentialProfile.State(0.0, -4),
+                new ExponentialProfile.State(0.75, -4),
+                new ExponentialProfile.State(1.3758, 4.4304)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -4),
+                new ExponentialProfile.State(1.4103, 4),
+                new ExponentialProfile.State(1.3758, 4.4304)),
+            new TestCase(
+                new ExponentialProfile.State(0.6603, 4),
+                new ExponentialProfile.State(0.75, -4),
+                new ExponentialProfile.State(1.3758, 4.4304)),
+            new TestCase(
+                new ExponentialProfile.State(0.6603, 4),
+                new ExponentialProfile.State(1.4103, 4),
+                new ExponentialProfile.State(1.3758, 4.4304)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -4),
+                new ExponentialProfile.State(0.5, -2),
+                new ExponentialProfile.State(0.4367, 3.7217)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -4),
+                new ExponentialProfile.State(0.546, 2),
+                new ExponentialProfile.State(0.4367, 3.7217)),
+            new TestCase(
+                new ExponentialProfile.State(0.6603, 4),
+                new ExponentialProfile.State(0.5, -2),
+                new ExponentialProfile.State(0.5560, -2.9616)),
+            new TestCase(
+                new ExponentialProfile.State(0.6603, 4),
+                new ExponentialProfile.State(0.546, 2),
+                new ExponentialProfile.State(0.5560, -2.9616)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -4),
+                new ExponentialProfile.State(-0.75, -4),
+                new ExponentialProfile.State(-0.7156, -4.4304)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -4),
+                new ExponentialProfile.State(-0.0897, 4),
+                new ExponentialProfile.State(-0.7156, -4.4304)),
+            new TestCase(
+                new ExponentialProfile.State(0.6603, 4),
+                new ExponentialProfile.State(-0.75, -4),
+                new ExponentialProfile.State(-0.7156, -4.4304)),
+            new TestCase(
+                new ExponentialProfile.State(0.6603, 4),
+                new ExponentialProfile.State(-0.0897, 4),
+                new ExponentialProfile.State(-0.7156, -4.4304)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -4),
+                new ExponentialProfile.State(-0.5, -4.5),
+                new ExponentialProfile.State(1.095, 4.314)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -4),
+                new ExponentialProfile.State(1.0795, 4.5),
+                new ExponentialProfile.State(-0.5122, -4.351)),
+            new TestCase(
+                new ExponentialProfile.State(0.6603, 4),
+                new ExponentialProfile.State(-0.5, -4.5),
+                new ExponentialProfile.State(1.095, 4.314)),
+            new TestCase(
+                new ExponentialProfile.State(0.6603, 4),
+                new ExponentialProfile.State(1.0795, 4.5),
+                new ExponentialProfile.State(-0.5122, -4.351)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -8),
+                new ExponentialProfile.State(0, 0),
+                new ExponentialProfile.State(-0.1384, 3.342)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, -8),
+                new ExponentialProfile.State(-1, 0),
+                new ExponentialProfile.State(-0.562, -6.792)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, 8),
+                new ExponentialProfile.State(1, 0),
+                new ExponentialProfile.State(0.562, 6.792)),
+            new TestCase(
+                new ExponentialProfile.State(0.0, 8),
+                new ExponentialProfile.State(-1, 0),
+                new ExponentialProfile.State(-0.785, -4.346)));
+
+    var profile = new ExponentialProfile(constraints);
+
+    for (var testCase : testCases) {
+      var state = profile.calculateInflectionPoint(testCase.initial, testCase.goal);
+      assertNear(testCase.inflectionPoint, state, 1e-3);
+    }
+  }
+
+  @Test
+  void timingToCurrent() {
+    ExponentialProfile.State goal = new ExponentialProfile.State(2, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+    for (int i = 0; i < 400; i++) {
+      state = checkDynamics(profile, state, goal);
+      assertNear(profile.timeLeftUntil(state, state), 0, 2e-2);
+    }
+  }
+
+  @Test
+  void timingToGoal() {
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+
+    ExponentialProfile.State goal = new ExponentialProfile.State(2, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    double predictedTimeLeft = profile.timeLeftUntil(state, goal);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      state = checkDynamics(profile, state, goal);
+
+      if (!reachedGoal && state.equals(goal)) {
+        // Expected value using for loop index is just an approximation since
+        // the time left in the profile doesn't increase linearly at the
+        // endpoints
+        assertNear(predictedTimeLeft, i / 100.0, 0.25);
+        reachedGoal = true;
+      }
+    }
+  }
+
+  @Test
+  void timingToNegativeGoal() {
+    ExponentialProfile profile = new ExponentialProfile(constraints);
+
+    ExponentialProfile.State goal = new ExponentialProfile.State(-2, 0);
+    ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
+
+    double predictedTimeLeft = profile.timeLeftUntil(state, goal);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      state = checkDynamics(profile, state, goal);
+
+      if (!reachedGoal && state.equals(goal)) {
+        // Expected value using for loop index is just an approximation since
+        // the time left in the profile doesn't increase linearly at the
+        // endpoints
+        assertNear(predictedTimeLeft, i / 100.0, 0.25);
+        reachedGoal = true;
+      }
+    }
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
index ee6cc8f..fb28b69 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
@@ -57,9 +57,9 @@
     TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0);
     TrapezoidProfile.State state = new TrapezoidProfile.State();
 
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 450; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
     }
     assertEquals(state, goal);
   }
@@ -67,21 +67,21 @@
   // Tests that decreasing the maximum velocity in the middle when it is already
   // moving faster than the new max is handled correctly
   @Test
-  void posContinousUnderVelChange() {
+  void posContinuousUnderVelChange() {
     TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
     TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0);
 
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
+    TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
 
     double lastPos = state.position;
     for (int i = 0; i < 1600; ++i) {
       if (i == 400) {
         constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+        profile = new TrapezoidProfile(constraints);
       }
 
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
       double estimatedVel = (state.position - lastPos) / kDt;
 
       if (i >= 400) {
@@ -105,9 +105,9 @@
     TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
     TrapezoidProfile.State state = new TrapezoidProfile.State();
 
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 400; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
     }
     assertEquals(state, goal);
   }
@@ -118,16 +118,16 @@
     TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
     TrapezoidProfile.State state = new TrapezoidProfile.State();
 
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 200; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
     }
     assertNotEquals(state, goal);
 
     goal = new TrapezoidProfile.State(0.0, 0.0);
+    profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 550; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
     }
     assertEquals(state, goal);
   }
@@ -139,15 +139,15 @@
     TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0);
     TrapezoidProfile.State state = new TrapezoidProfile.State();
 
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 200; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
     }
     assertNear(constraints.maxVelocity, state.velocity, 10e-5);
 
+    profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 2000; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
     }
     assertEquals(state, goal);
   }
@@ -158,9 +158,9 @@
     TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
     TrapezoidProfile.State state = new TrapezoidProfile.State();
 
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
     for (int i = 0; i < 400; i++) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
       assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
     }
   }
@@ -170,14 +170,13 @@
     TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
     TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
 
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
+    TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
 
     double predictedTimeLeft = profile.timeLeftUntil(goal.position);
     boolean reachedGoal = false;
     for (int i = 0; i < 400; i++) {
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
       if (!reachedGoal && state.equals(goal)) {
         // Expected value using for loop index is just an approximation since
         // the time left in the profile doesn't increase linearly at the
@@ -193,14 +192,13 @@
     TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
     TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
 
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
+    TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
 
     double predictedTimeLeft = profile.timeLeftUntil(1);
     boolean reachedGoal = false;
     for (int i = 0; i < 400; i++) {
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
       if (!reachedGoal && Math.abs(state.velocity - 1) < 10e-5) {
         assertNear(predictedTimeLeft, i / 100.0, 2e-2);
         reachedGoal = true;
@@ -213,14 +211,13 @@
     TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
     TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
 
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
+    TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
 
     double predictedTimeLeft = profile.timeLeftUntil(goal.position);
     boolean reachedGoal = false;
     for (int i = 0; i < 400; i++) {
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
       if (!reachedGoal && state.equals(goal)) {
         // Expected value using for loop index is just an approximation since
         // the time left in the profile doesn't increase linearly at the
@@ -236,14 +233,13 @@
     TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
     TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
 
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
+    TrapezoidProfile profile = new TrapezoidProfile(constraints);
+    TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
 
     double predictedTimeLeft = profile.timeLeftUntil(-1);
     boolean reachedGoal = false;
     for (int i = 0; i < 400; i++) {
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
+      state = profile.calculate(kDt, goal, state);
       if (!reachedGoal && Math.abs(state.velocity + 1) < 10e-5) {
         assertNear(predictedTimeLeft, i / 100.0, 2e-2);
         reachedGoal = true;
diff --git a/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
index e397af1..bbada9e 100644
--- a/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
@@ -2,8 +2,9 @@
 // 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 "frc/ComputerVisionUtil.h"
-#include "gtest/gtest.h"
 
 TEST(ComputerVisionUtilTest, ObjectToRobotPose) {
   frc::Pose3d robot{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 30_deg}};
diff --git a/wpimath/src/test/native/cpp/DARETest.cpp b/wpimath/src/test/native/cpp/DARETest.cpp
new file mode 100644
index 0000000..e19fa3c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETest.cpp
@@ -0,0 +1,299 @@
+// 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 <stdexcept>
+
+#include <Eigen/Core>
+#include <gtest/gtest.h>
+
+#include "DARETestUtil.h"
+#include "frc/DARE.h"
+#include "frc/EigenCore.h"
+#include "frc/fmt/Eigen.h"
+
+// 2x1
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R);
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
+    const Eigen::Matrix<double, 2, 1>& N);
+
+// 4x1
+extern template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
+    const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
+    const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R);
+extern template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
+    const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
+    const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R,
+    const Eigen::Matrix<double, 4, 1>& N);
+
+// 2x2
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R);
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
+    const Eigen::Matrix<double, 2, 2>& N);
+
+// 2x3
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R);
+extern template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R,
+    const Eigen::Matrix<double, 2, 3>& N);
+
+TEST(DARETest, NonInvertibleA_ABQR) {
+  // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
+  // Riccati Equation"
+
+  frc::Matrixd<4, 4> A{
+      {0.5, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}};
+  frc::Matrixd<4, 1> B{{0}, {0}, {0}, {1}};
+  frc::Matrixd<4, 4> Q{{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
+  frc::Matrixd<1, 1> R{0.25};
+
+  frc::Matrixd<4, 4> X = frc::DARE<4, 1>(A, B, Q, R);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, X);
+}
+
+TEST(DARETest, NonInvertibleA_ABQRN) {
+  // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
+  // Riccati Equation"
+
+  frc::Matrixd<4, 4> A{
+      {0.5, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}};
+  frc::Matrixd<4, 1> B{{0}, {0}, {0}, {1}};
+  frc::Matrixd<4, 4> Q{{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
+  frc::Matrixd<1, 1> R{0.25};
+
+  frc::Matrixd<4, 4> Aref{
+      {0.25, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}};
+  Q = (A - Aref).transpose() * Q * (A - Aref);
+  R = B.transpose() * Q * B + R;
+  frc::Matrixd<4, 1> N = (A - Aref).transpose() * Q * B;
+
+  frc::Matrixd<4, 4> X = frc::DARE<4, 1>(A, B, Q, R, N);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, N, X);
+}
+
+TEST(DARETest, InvertibleA_ABQR) {
+  frc::Matrixd<2, 2> A{{1, 1}, {0, 1}};
+  frc::Matrixd<2, 1> B{{0}, {1}};
+  frc::Matrixd<2, 2> Q{{1, 0}, {0, 0}};
+  frc::Matrixd<1, 1> R{{0.3}};
+
+  frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, X);
+}
+
+TEST(DARETest, InvertibleA_ABQRN) {
+  frc::Matrixd<2, 2> A{{1, 1}, {0, 1}};
+  frc::Matrixd<2, 1> B{{0}, {1}};
+  frc::Matrixd<2, 2> Q{{1, 0}, {0, 0}};
+  frc::Matrixd<1, 1> R{0.3};
+
+  frc::Matrixd<2, 2> Aref{{0.5, 1}, {0, 1}};
+  Q = (A - Aref).transpose() * Q * (A - Aref);
+  R = B.transpose() * Q * B + R;
+  frc::Matrixd<2, 1> N = (A - Aref).transpose() * Q * B;
+
+  frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R, N);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, N, X);
+}
+
+TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQR) {
+  // The first generalized eigenvalue of (S, T) is stable
+
+  frc::Matrixd<2, 2> A{{0, 1}, {0, 0}};
+  frc::Matrixd<2, 1> B{{0}, {1}};
+  frc::Matrixd<2, 2> Q{{1, 0}, {0, 1}};
+  frc::Matrixd<1, 1> R{1};
+
+  frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, X);
+}
+
+TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQRN) {
+  // The first generalized eigenvalue of (S, T) is stable
+
+  frc::Matrixd<2, 2> A{{0, 1}, {0, 0}};
+  frc::Matrixd<2, 1> B{{0}, {1}};
+  frc::Matrixd<2, 2> Q{{1, 0}, {0, 1}};
+  frc::Matrixd<1, 1> R{1};
+
+  frc::Matrixd<2, 2> Aref{{0, 0.5}, {0, 0}};
+  Q = (A - Aref).transpose() * Q * (A - Aref);
+  R = B.transpose() * Q * B + R;
+  frc::Matrixd<2, 1> N = (A - Aref).transpose() * Q * B;
+
+  frc::Matrixd<2, 2> X = frc::DARE<2, 1>(A, B, Q, R, N);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, N, X);
+}
+
+TEST(DARETest, IdentitySystem_ABQR) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+
+  Eigen::Matrix2d X = frc::DARE<2, 2>(A, B, Q, R);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, X);
+}
+
+TEST(DARETest, IdentitySystem_ABQRN) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
+
+  Eigen::Matrix2d X = frc::DARE<2, 2>(A, B, Q, R, N);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, N, X);
+}
+
+TEST(DARETest, MoreInputsThanStates_ABQR) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const frc::Matrixd<2, 3> B{{1.0, 0.0, 0.0}, {0.0, 0.5, 0.3}};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix3d R{Eigen::Matrix3d::Identity()};
+
+  Eigen::Matrix2d X = frc::DARE<2, 3>(A, B, Q, R);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, X);
+}
+
+TEST(DARETest, MoreInputsThanStates_ABQRN) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const frc::Matrixd<2, 3> B{{1.0, 0.0, 0.0}, {0.0, 0.5, 0.3}};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix3d R{Eigen::Matrix3d::Identity()};
+  const frc::Matrixd<2, 3> N{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}};
+
+  Eigen::Matrix2d X = frc::DARE<2, 3>(A, B, Q, R, N);
+  ExpectMatrixEqual(X, X.transpose(), 1e-10);
+  ExpectPositiveSemidefinite(X);
+  ExpectDARESolution(A, B, Q, R, N, X);
+}
+
+TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQR) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d Q{-Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument);
+}
+
+TEST(DARETest, QNotSymmetricPositiveSemidefinite_ABQRN) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d N{2.0 * Eigen::Matrix2d::Identity()};
+
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
+}
+
+TEST(DARETest, RNotSymmetricPositiveDefinite_ABQR) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+
+  const Eigen::Matrix2d R1{Eigen::Matrix2d::Zero()};
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R1)), std::invalid_argument);
+
+  const Eigen::Matrix2d R2{-Eigen::Matrix2d::Identity()};
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R2)), std::invalid_argument);
+}
+
+TEST(DARETest, RNotSymmetricPositiveDefinite_ABQRN) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
+
+  const Eigen::Matrix2d R1{Eigen::Matrix2d::Zero()};
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R1, N)), std::invalid_argument);
+
+  const Eigen::Matrix2d R2{-Eigen::Matrix2d::Identity()};
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R2, N)), std::invalid_argument);
+}
+
+TEST(DARETest, ABNotStabilizable_ABQR) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Zero()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument);
+}
+
+TEST(DARETest, ABNotStabilizable_ABQRN) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Zero()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d N{Eigen::Matrix2d::Identity()};
+
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
+}
+
+TEST(DARETest, ACNotDetectable_ABQR) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Zero()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R)), std::invalid_argument);
+}
+
+TEST(DARETest, ACNotDetectable_ABQRN) {
+  const Eigen::Matrix2d A{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d Q{Eigen::Matrix2d::Zero()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d N{Eigen::Matrix2d::Zero()};
+
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
+}
+
+TEST(DARETest, QDecomposition) {
+  // Ensures the decomposition of Q into CᵀC is correct
+
+  const Eigen::Matrix2d A{{1.0, 0.0}, {0.0, 0.0}};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+
+  // (A, C₁) should be detectable pair
+  const Eigen::Matrix2d C_1{{0.0, 0.0}, {1.0, 0.0}};
+  const Eigen::Matrix2d Q_1 = C_1.transpose() * C_1;
+  EXPECT_NO_THROW((frc::DARE<2, 2>(A, B, Q_1, R)));
+
+  // (A, C₂) shouldn't be detectable pair
+  const Eigen::Matrix2d C_2 = C_1.transpose();
+  const Eigen::Matrix2d Q_2 = C_2.transpose() * C_2;
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q_2, R)), std::invalid_argument);
+}
diff --git a/wpimath/src/test/native/cpp/DARETestUtil.cpp b/wpimath/src/test/native/cpp/DARETestUtil.cpp
new file mode 100644
index 0000000..452e6ce
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETestUtil.cpp
@@ -0,0 +1,72 @@
+// 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 "DARETestUtil.h"
+
+#include <Eigen/Eigenvalues>
+#include <fmt/core.h>
+#include <gtest/gtest.h>
+
+#include "frc/fmt/Eigen.h"
+
+void ExpectMatrixEqual(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs,
+                       double tolerance) {
+  for (int row = 0; row < lhs.rows(); ++row) {
+    for (int col = 0; col < lhs.cols(); ++col) {
+      EXPECT_NEAR(lhs(row, col), rhs(row, col), tolerance)
+          << fmt::format("row = {}, col = {}", row, col);
+    }
+  }
+
+  if (::testing::Test::HasFailure()) {
+    fmt::print("lhs =\n{}\n", lhs);
+    fmt::print("rhs =\n{}\n", rhs);
+    fmt::print("delta =\n{}\n", Eigen::MatrixXd{lhs - rhs});
+  }
+}
+
+void ExpectPositiveSemidefinite(const Eigen::Ref<const Eigen::MatrixXd>& X) {
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigX{X,
+                                                      Eigen::EigenvaluesOnly};
+  for (int i = 0; i < X.rows(); ++i) {
+    EXPECT_GE(eigX.eigenvalues()[i], 0.0);
+  }
+}
+
+void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
+                        const Eigen::Ref<const Eigen::MatrixXd>& B,
+                        const Eigen::Ref<const Eigen::MatrixXd>& Q,
+                        const Eigen::Ref<const Eigen::MatrixXd>& R,
+                        const Eigen::Ref<const Eigen::MatrixXd>& X) {
+  // Check that X is the solution to the DARE
+  // Y = AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+  // clang-format off
+  Eigen::MatrixXd Y =
+      A.transpose() * X * A
+      - X
+      - (A.transpose() * X * B * (B.transpose() * X * B + R).inverse()
+        * B.transpose() * X * A)
+      + Q;
+  // clang-format on
+  ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10);
+}
+
+void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
+                        const Eigen::Ref<const Eigen::MatrixXd>& B,
+                        const Eigen::Ref<const Eigen::MatrixXd>& Q,
+                        const Eigen::Ref<const Eigen::MatrixXd>& R,
+                        const Eigen::Ref<const Eigen::MatrixXd>& N,
+                        const Eigen::Ref<const Eigen::MatrixXd>& X) {
+  // Check that X is the solution to the DARE
+  // Y = AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+  // clang-format off
+  Eigen::MatrixXd Y =
+      A.transpose() * X * A
+      - X
+      - ((A.transpose() * X * B + N) * (B.transpose() * X * B + R).inverse()
+        * (B.transpose() * X * A + N.transpose()))
+      + Q;
+  // clang-format on
+  ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10);
+}
diff --git a/wpimath/src/test/native/cpp/DARETestUtil.h b/wpimath/src/test/native/cpp/DARETestUtil.h
new file mode 100644
index 0000000..f0d2f66
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETestUtil.h
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <Eigen/Core>
+
+void ExpectMatrixEqual(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs,
+                       double tolerance);
+
+void ExpectPositiveSemidefinite(const Eigen::Ref<const Eigen::MatrixXd>& X);
+
+void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
+                        const Eigen::Ref<const Eigen::MatrixXd>& B,
+                        const Eigen::Ref<const Eigen::MatrixXd>& Q,
+                        const Eigen::Ref<const Eigen::MatrixXd>& R,
+                        const Eigen::Ref<const Eigen::MatrixXd>& X);
+
+void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
+                        const Eigen::Ref<const Eigen::MatrixXd>& B,
+                        const Eigen::Ref<const Eigen::MatrixXd>& Q,
+                        const Eigen::Ref<const Eigen::MatrixXd>& R,
+                        const Eigen::Ref<const Eigen::MatrixXd>& N,
+                        const Eigen::Ref<const Eigen::MatrixXd>& X);
diff --git a/wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp b/wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp
new file mode 100644
index 0000000..3ba3d71
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETest_Inst_2x1.cpp
@@ -0,0 +1,13 @@
+// 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 "frc/DARE.h"
+
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R);
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 1>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
+    const Eigen::Matrix<double, 2, 1>& N);
diff --git a/wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp b/wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp
new file mode 100644
index 0000000..75b17b8
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETest_Inst_2x2.cpp
@@ -0,0 +1,13 @@
+// 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 "frc/DARE.h"
+
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R);
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 2>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
+    const Eigen::Matrix<double, 2, 2>& N);
diff --git a/wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp b/wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp
new file mode 100644
index 0000000..8d98553
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETest_Inst_2x3.cpp
@@ -0,0 +1,13 @@
+// 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 "frc/DARE.h"
+
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R);
+template Eigen::Matrix<double, 2, 2> frc::DARE<2, 3>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 3>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 3, 3>& R,
+    const Eigen::Matrix<double, 2, 3>& N);
diff --git a/wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp b/wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp
new file mode 100644
index 0000000..840285c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/DARETest_Inst_4x1.cpp
@@ -0,0 +1,13 @@
+// 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 "frc/DARE.h"
+
+template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
+    const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
+    const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R);
+template Eigen::Matrix<double, 4, 4> frc::DARE<4, 1>(
+    const Eigen::Matrix<double, 4, 4>& A, const Eigen::Matrix<double, 4, 1>& B,
+    const Eigen::Matrix<double, 4, 4>& Q, const Eigen::Matrix<double, 1, 1>& R,
+    const Eigen::Matrix<double, 4, 1>& N);
diff --git a/wpimath/src/test/native/cpp/EigenTest.cpp b/wpimath/src/test/native/cpp/EigenTest.cpp
index 03be9b3..74c8577 100644
--- a/wpimath/src/test/native/cpp/EigenTest.cpp
+++ b/wpimath/src/test/native/cpp/EigenTest.cpp
@@ -2,9 +2,10 @@
 // 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 "Eigen/LU"
+#include <Eigen/LU>
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
-#include "gtest/gtest.h"
 
 TEST(EigenTest, Multiplication) {
   frc::Matrixd<2, 2> m1{{2, 1}, {0, 1}};
@@ -29,17 +30,17 @@
 }
 
 TEST(EigenTest, Transpose) {
-  frc::Vectord<3> vec{1, 2, 3};
+  Eigen::Vector3d vec{1, 2, 3};
 
   const auto transpose = vec.transpose();
 
-  Eigen::RowVector<double, 3> expectedTranspose{1, 2, 3};
+  Eigen::RowVector3d expectedTranspose{1, 2, 3};
 
   EXPECT_TRUE(expectedTranspose.isApprox(transpose));
 }
 
 TEST(EigenTest, Inverse) {
-  frc::Matrixd<3, 3> mat{{1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}};
+  Eigen::Matrix3d mat{{1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}};
 
   const auto inverse = mat.inverse();
   const auto identity = Eigen::MatrixXd::Identity(3, 3);
diff --git a/wpimath/src/test/native/cpp/FormatterTest.cpp b/wpimath/src/test/native/cpp/FormatterTest.cpp
index 0e2f77c..0b6d590 100644
--- a/wpimath/src/test/native/cpp/FormatterTest.cpp
+++ b/wpimath/src/test/native/cpp/FormatterTest.cpp
@@ -5,10 +5,9 @@
 #include <vector>
 
 #include <fmt/format.h>
+#include <gtest/gtest.h>
 
 #include "frc/fmt/Eigen.h"
-#include "frc/fmt/Units.h"
-#include "gtest/gtest.h"
 #include "units/velocity.h"
 
 TEST(FormatterTest, Eigen) {
@@ -17,14 +16,14 @@
       "  0.000000  1.000000\n"
       "  2.000000  3.000000\n"
       "  4.000000  5.000000",
-      fmt::format("{}", A));
+      fmt::format("{:f}", A));
 
   Eigen::MatrixXd B{{0.0, 1.0}, {2.0, 3.0}, {4.0, 5.0}};
   EXPECT_EQ(
       "  0.000000  1.000000\n"
       "  2.000000  3.000000\n"
       "  4.000000  5.000000",
-      fmt::format("{}", B));
+      fmt::format("{:f}", B));
 
   Eigen::SparseMatrix<double> C{3, 2};
   std::vector<Eigen::Triplet<double>> triplets;
@@ -38,7 +37,7 @@
       "  0.000000  1.000000\n"
       "  2.000000  3.000000\n"
       "  4.000000  5.000000",
-      fmt::format("{}", C));
+      fmt::format("{:f}", C));
 }
 
 TEST(FormatterTest, Units) {
diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp
index a836a77..cee9831 100644
--- a/wpimath/src/test/native/cpp/MathUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp
@@ -4,8 +4,9 @@
 
 #include <limits>
 
+#include <gtest/gtest.h>
+
 #include "frc/MathUtil.h"
-#include "gtest/gtest.h"
 #include "units/angle.h"
 
 #define EXPECT_UNITS_EQ(a, b) EXPECT_DOUBLE_EQ((a).value(), (b).value())
@@ -117,3 +118,49 @@
   EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{-std::numbers::pi / 2}),
                   units::radian_t{-std::numbers::pi / 2});
 }
+
+TEST(MathUtilTest, IsNear) {
+  // The answer is always 42
+  // Positive integer checks
+  EXPECT_TRUE(frc::IsNear(42, 42, 1));
+  EXPECT_TRUE(frc::IsNear(42, 41, 2));
+  EXPECT_TRUE(frc::IsNear(42, 43, 2));
+  EXPECT_FALSE(frc::IsNear(42, 44, 1));
+
+  // Negative integer checks
+  EXPECT_TRUE(frc::IsNear(-42, -42, 1));
+  EXPECT_TRUE(frc::IsNear(-42, -41, 2));
+  EXPECT_TRUE(frc::IsNear(-42, -43, 2));
+  EXPECT_FALSE(frc::IsNear(-42, -44, 1));
+
+  // Mixed sign integer checks
+  EXPECT_FALSE(frc::IsNear(-42, 42, 1));
+  EXPECT_FALSE(frc::IsNear(-42, 41, 2));
+  EXPECT_FALSE(frc::IsNear(-42, 43, 2));
+  EXPECT_FALSE(frc::IsNear(42, -42, 1));
+  EXPECT_FALSE(frc::IsNear(42, -41, 2));
+  EXPECT_FALSE(frc::IsNear(42, -43, 2));
+
+  // Floating point checks
+  EXPECT_TRUE(frc::IsNear<double>(42, 41.5, 1));
+  EXPECT_TRUE(frc::IsNear<double>(42, 42.5, 1));
+  EXPECT_TRUE(frc::IsNear<double>(42, 41.5, 0.75));
+  EXPECT_TRUE(frc::IsNear<double>(42, 42.5, 0.75));
+
+  // Wraparound checks
+  EXPECT_TRUE(frc::IsNear(0_deg, 356_deg, 5_deg, 0_deg, 360_deg));
+  EXPECT_TRUE(frc::IsNear(0, -356, 5, 0, 360));
+  EXPECT_TRUE(frc::IsNear(0, 4, 5, 0, 360));
+  EXPECT_TRUE(frc::IsNear(0, -4, 5, 0, 360));
+  EXPECT_TRUE(frc::IsNear(400, 41, 5, 0, 360));
+  EXPECT_TRUE(frc::IsNear(400, -319, 5, 0, 360));
+  EXPECT_TRUE(frc::IsNear(400, 401, 5, 0, 360));
+  EXPECT_FALSE(frc::IsNear<double>(0, 356, 2.5, 0, 360));
+  EXPECT_FALSE(frc::IsNear<double>(0, -356, 2.5, 0, 360));
+  EXPECT_FALSE(frc::IsNear<double>(0, 4, 2.5, 0, 360));
+  EXPECT_FALSE(frc::IsNear<double>(0, -4, 2.5, 0, 360));
+  EXPECT_FALSE(frc::IsNear(400, 35, 5, 0, 360));
+  EXPECT_FALSE(frc::IsNear(400, -315, 5, 0, 360));
+  EXPECT_FALSE(frc::IsNear(400, 395, 5, 0, 360));
+  EXPECT_FALSE(frc::IsNear(0_deg, -4_deg, 2.5_deg, 0_deg, 360_deg));
+}
diff --git a/wpimath/src/test/native/cpp/StateSpaceTest.cpp b/wpimath/src/test/native/cpp/StateSpaceTest.cpp
index ee7842c..07907e2 100644
--- a/wpimath/src/test/native/cpp/StateSpaceTest.cpp
+++ b/wpimath/src/test/native/cpp/StateSpaceTest.cpp
@@ -2,11 +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.
 
-#include <gtest/gtest.h>
-
 #include <cmath>
 #include <random>
 
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/controller/LinearPlantInversionFeedforward.h"
 #include "frc/controller/LinearQuadraticRegulator.h"
diff --git a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
index 105a434..758dc30 100644
--- a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
@@ -2,10 +2,10 @@
 // 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 <array>
 
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/system/NumericalIntegration.h"
diff --git a/wpimath/src/test/native/cpp/UnitsTest.cpp b/wpimath/src/test/native/cpp/UnitsTest.cpp
index 1059260..1464dcd 100644
--- a/wpimath/src/test/native/cpp/UnitsTest.cpp
+++ b/wpimath/src/test/native/cpp/UnitsTest.cpp
@@ -7,10 +7,12 @@
 #include <string>
 #include <type_traits>
 
-#include "gtest/gtest.h"
+#include <gtest/gtest.h>
+
 #include "units/acceleration.h"
 #include "units/angle.h"
 #include "units/angular_acceleration.h"
+#include "units/angular_jerk.h"
 #include "units/angular_velocity.h"
 #include "units/area.h"
 #include "units/capacitance.h"
@@ -51,6 +53,7 @@
 using namespace units::acceleration;
 using namespace units::angle;
 using namespace units::angular_acceleration;
+using namespace units::angular_jerk;
 using namespace units::angular_velocity;
 using namespace units::area;
 using namespace units::capacitance;
@@ -1420,7 +1423,7 @@
 }
 #endif
 
-#if !defined(UNIT_LIB_DISABLE_FMT)
+#if __has_include(<fmt/format.h>) && !defined(UNIT_LIB_DISABLE_FMT)
 TEST_F(UnitContainer, fmtlib) {
   testing::internal::CaptureStdout();
   fmt::print("{}", degree_t(349.87));
@@ -2002,6 +2005,23 @@
   EXPECT_NEAR(1.537e-16, test, 5.0e-20);
 }
 
+TEST_F(UnitConversion, angular_jerk) {
+  double test;
+  bool same;
+
+  same =
+      std::is_same_v<radians_per_second_cubed,
+                   unit<std::ratio<1>, category::angular_jerk_unit>>;
+  EXPECT_TRUE(same);
+  same = traits::is_convertible_unit_v<deg_per_s_cu, radians_per_second_cubed>;
+  EXPECT_TRUE(same);
+
+  test = convert<degrees_per_second_cubed, radians_per_second_cubed>(1.0);
+  EXPECT_NEAR(0.0174533, test, 5.0e-8);
+  test = convert<turns_per_second_cubed, radians_per_second_cubed>(1.0);
+  EXPECT_NEAR(6.283185307, test, 5.0e-6);
+}
+
 TEST_F(UnitConversion, acceleration) {
   double test;
 
diff --git a/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp
new file mode 100644
index 0000000..b7853b1
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp
@@ -0,0 +1,76 @@
+// 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 <cmath>
+#include <numbers>
+
+#include <gtest/gtest.h>
+
+#include "frc/controller/ArmFeedforward.h"
+#include "units/acceleration.h"
+#include "units/length.h"
+#include "units/time.h"
+
+static constexpr auto Ks = 0.5_V;
+static constexpr auto Kv = 1.5_V * 1_s / 1_rad;
+static constexpr auto Ka = 2_V * 1_s * 1_s / 1_rad;
+static constexpr auto Kg = 1_V;
+
+TEST(ArmFeedforwardTest, Calculate) {
+  frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+  EXPECT_NEAR(
+      armFF.Calculate(std::numbers::pi * 1_rad / 3, 0_rad / 1_s).value(), 0.5,
+      0.002);
+  EXPECT_NEAR(
+      armFF.Calculate(std::numbers::pi * 1_rad / 3, 1_rad / 1_s).value(), 2.5,
+      0.002);
+  EXPECT_NEAR(armFF
+                  .Calculate(std::numbers::pi * 1_rad / 3, 1_rad / 1_s,
+                             2_rad / 1_s / 1_s)
+                  .value(),
+              6.5, 0.002);
+  EXPECT_NEAR(armFF
+                  .Calculate(std::numbers::pi * 1_rad / 3, -1_rad / 1_s,
+                             2_rad / 1_s / 1_s)
+                  .value(),
+              2.5, 0.002);
+}
+
+TEST(ArmFeedforwardTest, AchievableVelocity) {
+  frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+  EXPECT_NEAR(armFF
+                  .MaxAchievableVelocity(12_V, std::numbers::pi * 1_rad / 3,
+                                         1_rad / 1_s / 1_s)
+                  .value(),
+              6, 0.002);
+  EXPECT_NEAR(armFF
+                  .MinAchievableVelocity(11.5_V, std::numbers::pi * 1_rad / 3,
+                                         1_rad / 1_s / 1_s)
+                  .value(),
+              -9, 0.002);
+}
+
+TEST(ArmFeedforwardTest, AchievableAcceleration) {
+  frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+  EXPECT_NEAR(armFF
+                  .MaxAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
+                                             1_rad / 1_s)
+                  .value(),
+              4.75, 0.002);
+  EXPECT_NEAR(armFF
+                  .MaxAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
+                                             -1_rad / 1_s)
+                  .value(),
+              6.75, 0.002);
+  EXPECT_NEAR(armFF
+                  .MinAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
+                                             1_rad / 1_s)
+                  .value(),
+              -7.25, 0.002);
+  EXPECT_NEAR(armFF
+                  .MinAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
+                                             -1_rad / 1_s)
+                  .value(),
+              -5.25, 0.002);
+}
diff --git a/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp
index 4dfaa0e..f931590 100644
--- a/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp
@@ -2,8 +2,9 @@
 // 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 "frc/controller/BangBangController.h"
-#include "gtest/gtest.h"
 
 class BangBangInputOutputTest : public testing::Test {
  protected:
diff --git a/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp b/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp
index e582720..3484e6e 100644
--- a/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp
@@ -2,8 +2,9 @@
 // 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 "frc/controller/BangBangController.h"
-#include "gtest/gtest.h"
 
 class BangBangToleranceTest : public testing::Test {
  protected:
diff --git a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
index b2bcee6..9db64da 100644
--- a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
@@ -2,10 +2,10 @@
 // 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 <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/controller/ControlAffinePlantInversionFeedforward.h"
 #include "units/time.h"
diff --git a/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp
index 74d945c..5428d84 100644
--- a/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp
@@ -2,11 +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.
 
-#include <gtest/gtest.h>
-
 #include <cmath>
 
-#include "frc/EigenCore.h"
+#include <Eigen/Core>
+#include <gtest/gtest.h>
+
 #include "frc/controller/DifferentialDriveFeedforward.h"
 #include "frc/controller/LinearPlantInversionFeedforward.h"
 #include "frc/system/plant/LinearSystemId.h"
@@ -38,9 +38,9 @@
           auto [left, right] = differentialDriveFeedforward.Calculate(
               currentLeftVelocity, nextLeftVelocity, currentRightVelocity,
               nextRightVelocity, dt);
-          frc::Matrixd<2, 1> nextX = plant.CalculateX(
-              frc::Vectord<2>{currentLeftVelocity, currentRightVelocity},
-              frc::Vectord<2>{left, right}, dt);
+          Eigen::Vector2d nextX = plant.CalculateX(
+              Eigen::Vector2d{currentLeftVelocity, currentRightVelocity},
+              Eigen::Vector2d{left, right}, dt);
           EXPECT_NEAR(nextX(0), nextLeftVelocity.value(), 1e-6);
           EXPECT_NEAR(nextX(1), nextRightVelocity.value(), 1e-6);
         }
@@ -72,9 +72,9 @@
           auto [left, right] = differentialDriveFeedforward.Calculate(
               currentLeftVelocity, nextLeftVelocity, currentRightVelocity,
               nextRightVelocity, dt);
-          frc::Matrixd<2, 1> nextX = plant.CalculateX(
-              frc::Vectord<2>{currentLeftVelocity, currentRightVelocity},
-              frc::Vectord<2>{left, right}, dt);
+          Eigen::Vector2d nextX = plant.CalculateX(
+              Eigen::Vector2d{currentLeftVelocity, currentRightVelocity},
+              Eigen::Vector2d{left, right}, dt);
           EXPECT_NEAR(nextX(0), nextLeftVelocity.value(), 1e-6);
           EXPECT_NEAR(nextX(1), nextRightVelocity.value(), 1e-6);
         }
diff --git a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp
new file mode 100644
index 0000000..a2db70d
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp
@@ -0,0 +1,60 @@
+// 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 <cmath>
+
+#include <gtest/gtest.h>
+
+#include "frc/EigenCore.h"
+#include "frc/controller/ElevatorFeedforward.h"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "units/acceleration.h"
+#include "units/length.h"
+#include "units/time.h"
+
+static constexpr auto Ks = 0.5_V;
+static constexpr auto Kv = 1.5_V * 1_s / 1_m;
+static constexpr auto Ka = 2_V * 1_s * 1_s / 1_m;
+static constexpr auto Kg = 1_V;
+
+TEST(ElevatorFeedforwardTest, Calculate) {
+  frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
+
+  EXPECT_NEAR(elevatorFF.Calculate(0_m / 1_s).value(), Kg.value(), 0.002);
+  EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s).value(), 4.5, 0.002);
+  EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s, 1_m / 1_s / 1_s).value(), 6.5,
+              0.002);
+  EXPECT_NEAR(elevatorFF.Calculate(-2_m / 1_s, 1_m / 1_s / 1_s).value(), -0.5,
+              0.002);
+
+  frc::Matrixd<1, 1> A{-Kv.value() / Ka.value()};
+  frc::Matrixd<1, 1> B{1.0 / Ka.value()};
+  constexpr units::second_t dt = 20_ms;
+  frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
+
+  frc::Vectord<1> r{2.0};
+  frc::Vectord<1> nextR{3.0};
+  EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(),
+              elevatorFF.Calculate(2_mps, 3_mps, dt).value(), 0.002);
+}
+
+TEST(ElevatorFeedforwardTest, AchievableVelocity) {
+  frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
+  EXPECT_NEAR(elevatorFF.MaxAchievableVelocity(11_V, 1_m / 1_s / 1_s).value(),
+              5, 0.002);
+  EXPECT_NEAR(elevatorFF.MinAchievableVelocity(11_V, 1_m / 1_s / 1_s).value(),
+              -9, 0.002);
+}
+
+TEST(ElevatorFeedforwardTest, AchievableAcceleration) {
+  frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
+  EXPECT_NEAR(elevatorFF.MaxAchievableAcceleration(12_V, 2_m / 1_s).value(),
+              3.75, 0.002);
+  EXPECT_NEAR(elevatorFF.MaxAchievableAcceleration(12_V, -2_m / 1_s).value(),
+              7.25, 0.002);
+  EXPECT_NEAR(elevatorFF.MinAchievableAcceleration(12_V, 2_m / 1_s).value(),
+              -8.25, 0.002);
+  EXPECT_NEAR(elevatorFF.MinAchievableAcceleration(12_V, -2_m / 1_s).value(),
+              -4.75, 0.002);
+}
diff --git a/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
index 134a4e0..4b4a883 100644
--- a/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
@@ -4,10 +4,11 @@
 
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/MathUtil.h"
 #include "frc/controller/HolonomicDriveController.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 #include "units/angular_acceleration.h"
 #include "units/math.h"
 #include "units/time.h"
@@ -21,7 +22,7 @@
 
 TEST(HolonomicDriveControllerTest, ReachesReference) {
   frc::HolonomicDriveController controller{
-      frc2::PIDController{1.0, 0.0, 0.0}, frc2::PIDController{1.0, 0.0, 0.0},
+      frc::PIDController{1.0, 0.0, 0.0}, frc::PIDController{1.0, 0.0, 0.0},
       frc::ProfiledPIDController<units::radian>{
           1.0, 0.0, 0.0,
           frc::TrapezoidProfile<units::radian>::Constraints{
@@ -53,7 +54,7 @@
 
 TEST(HolonomicDriveControllerTest, DoesNotRotateUnnecessarily) {
   frc::HolonomicDriveController controller{
-      frc2::PIDController{1, 0, 0}, frc2::PIDController{1, 0, 0},
+      frc::PIDController{1, 0, 0}, frc::PIDController{1, 0, 0},
       frc::ProfiledPIDController<units::radian>{
           1, 0, 0,
           frc::TrapezoidProfile<units::radian>::Constraints{
diff --git a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
index 753ea34..d96c67b 100644
--- a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
@@ -4,12 +4,13 @@
 
 #include <cmath>
 
+#include <gtest/gtest.h>
+
 #include "frc/MathUtil.h"
 #include "frc/controller/LTVDifferentialDriveController.h"
 #include "frc/system/NumericalIntegration.h"
 #include "frc/system/plant/LinearSystemId.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 #include "units/math.h"
 
 #define EXPECT_NEAR_UNITS(val1, val2, eps) \
diff --git a/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp
index 56faf1d..ff2c7d3 100644
--- a/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp
@@ -2,10 +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.
 
+#include <gtest/gtest.h>
+
 #include "frc/MathUtil.h"
 #include "frc/controller/LTVUnicycleController.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 #include "units/math.h"
 
 #define EXPECT_NEAR_UNITS(val1, val2, eps) \
diff --git a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
index e7107d0..a92d4f0 100644
--- a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
@@ -2,10 +2,10 @@
 // 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 <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/controller/LinearPlantInversionFeedforward.h"
 #include "units/time.h"
diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
index 1127fc2..45f6473 100644
--- a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
@@ -2,10 +2,10 @@
 // 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 <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/controller/LinearQuadraticRegulator.h"
 #include "frc/system/LinearSystem.h"
@@ -158,10 +158,10 @@
   EXPECT_NEAR(0.51182128351092726, K(0, 1), 1e-10);
 
   // QRN overload
-  Matrixd<2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 2.0)}};
+  Matrixd<2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 5.0)}};
   Matrixd<1, 2> Kimf = GetImplicitModelFollowingK<2, 1>(A, B, Q, R, Aref, 5_ms);
   EXPECT_NEAR(0.0, Kimf(0, 0), 1e-10);
-  EXPECT_NEAR(-5.367540084534802e-05, Kimf(0, 1), 1e-10);
+  EXPECT_NEAR(-6.9190500116751458e-05, Kimf(0, 1), 1e-10);
 }
 
 TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {
diff --git a/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
index f1034f5..7992622 100644
--- a/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
@@ -2,14 +2,15 @@
 // 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 "frc/controller/PIDController.h"
-#include "gtest/gtest.h"
 
 class PIDInputOutputTest : public testing::Test {
  protected:
-  frc2::PIDController* controller;
+  frc::PIDController* controller;
 
-  void SetUp() override { controller = new frc2::PIDController{0, 0, 0}; }
+  void SetUp() override { controller = new frc::PIDController{0, 0, 0}; }
 
   void TearDown() override { delete controller; }
 };
@@ -49,3 +50,21 @@
   EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(),
                    controller->Calculate(0.0025, 0));
 }
+
+TEST_F(PIDInputOutputTest, IZoneNoOutput) {
+  controller->SetI(1);
+  controller->SetIZone(1);
+
+  double out = controller->Calculate(2, 0);
+
+  EXPECT_DOUBLE_EQ(0, out);
+}
+
+TEST_F(PIDInputOutputTest, IZoneOutput) {
+  controller->SetI(1);
+  controller->SetIZone(1);
+
+  double out = controller->Calculate(1, 0);
+
+  EXPECT_DOUBLE_EQ(-1 * controller->GetPeriod().value(), out);
+}
diff --git a/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp b/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp
index 0aec438..f37be89 100644
--- a/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp
@@ -2,27 +2,26 @@
 // 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 "frc/controller/PIDController.h"
-#include "gtest/gtest.h"
 
 static constexpr double kSetpoint = 50.0;
 static constexpr double kRange = 200;
 static constexpr double kTolerance = 10.0;
 
 TEST(PIDToleranceTest, InitialTolerance) {
-  frc2::PIDController controller{0.5, 0.0, 0.0};
+  frc::PIDController controller{0.5, 0.0, 0.0};
   controller.EnableContinuousInput(-kRange / 2, kRange / 2);
 
-  EXPECT_TRUE(controller.AtSetpoint());
+  EXPECT_FALSE(controller.AtSetpoint());
 }
 
 TEST(PIDToleranceTest, AbsoluteTolerance) {
-  frc2::PIDController controller{0.5, 0.0, 0.0};
+  frc::PIDController controller{0.5, 0.0, 0.0};
   controller.EnableContinuousInput(-kRange / 2, kRange / 2);
 
-  EXPECT_TRUE(controller.AtSetpoint())
-      << "Error was not in tolerance when it should have been. Error was "
-      << controller.GetPositionError();
+  EXPECT_FALSE(controller.AtSetpoint());
 
   controller.SetTolerance(kTolerance);
   controller.SetSetpoint(kSetpoint);
diff --git a/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
index 44d1f41..5e4f01a 100644
--- a/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
@@ -4,8 +4,9 @@
 
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/controller/ProfiledPIDController.h"
-#include "gtest/gtest.h"
 #include "units/angle.h"
 #include "units/angular_acceleration.h"
 #include "units/angular_velocity.h"
diff --git a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
index 2fd26bb..d39c679 100644
--- a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
@@ -2,10 +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.
 
+#include <gtest/gtest.h>
+
 #include "frc/MathUtil.h"
 #include "frc/controller/RamseteController.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 #include "units/math.h"
 
 #define EXPECT_NEAR_UNITS(val1, val2, eps) \
diff --git a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
index 6d10e05..7878011 100644
--- a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
@@ -2,10 +2,10 @@
 // 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 <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/controller/LinearPlantInversionFeedforward.h"
 #include "frc/controller/SimpleMotorFeedforward.h"
diff --git a/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp b/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp
deleted file mode 100644
index 8631d6f..0000000
--- a/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp
+++ /dev/null
@@ -1,124 +0,0 @@
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-
-#include <Eigen/Eigenvalues>
-#include <gtest/gtest.h>
-
-#include "drake/common/test_utilities/eigen_matrix_compare.h"
-// #include "drake/math/autodiff.h"
-
-using Eigen::MatrixXd;
-
-namespace drake {
-namespace math {
-namespace {
-void SolveDAREandVerify(const Eigen::Ref<const MatrixXd>& A,
-                        const Eigen::Ref<const MatrixXd>& B,
-                        const Eigen::Ref<const MatrixXd>& Q,
-                        const Eigen::Ref<const MatrixXd>& R) {
-  MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R);
-  // Check that X is positive semi-definite.
-  EXPECT_TRUE(
-      CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute));
-  int n = X.rows();
-  Eigen::SelfAdjointEigenSolver<MatrixXd> es(X);
-  for (int i = 0; i < n; i++) {
-    EXPECT_GE(es.eigenvalues()[i], 0);
-  }
-  // Check that X is the solution to the discrete time ARE.
-  // clang-format off
-  MatrixXd Y =
-      A.transpose() * X * A
-      - X
-      - (A.transpose() * X * B * (B.transpose() * X * B + R).inverse()
-        * B.transpose() * X * A)
-      + Q;
-  // clang-format on
-  EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10,
-                              MatrixCompareType::absolute));
-}
-
-void SolveDAREandVerify(const Eigen::Ref<const MatrixXd>& A,
-                        const Eigen::Ref<const MatrixXd>& B,
-                        const Eigen::Ref<const MatrixXd>& Q,
-                        const Eigen::Ref<const MatrixXd>& R,
-                        const Eigen::Ref<const MatrixXd>& N) {
-  MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R, N);
-  // Check that X is positive semi-definite.
-  EXPECT_TRUE(
-      CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute));
-  int n = X.rows();
-  Eigen::SelfAdjointEigenSolver<MatrixXd> es(X);
-  for (int i = 0; i < n; i++) {
-    EXPECT_GE(es.eigenvalues()[i], 0);
-  }
-  // Check that X is the solution to the discrete time ARE.
-  // clang-format off
-  MatrixXd Y =
-      A.transpose() * X * A
-      - X
-      - ((A.transpose() * X * B + N) * (B.transpose() * X * B + R).inverse()
-        * (B.transpose() * X * A + N.transpose()))
-      + Q;
-  // clang-format on
-  EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10,
-                              MatrixCompareType::absolute));
-}
-
-GTEST_TEST(DARE, SolveDAREandVerify) {
-  // Test 1: non-invertible A
-  // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
-  // Riccati Equation"
-  int n1 = 4, m1 = 1;
-  MatrixXd A1(n1, n1), B1(n1, m1), Q1(n1, n1), R1(m1, m1);
-  A1 << 0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0;
-  B1 << 0, 0, 0, 1;
-  Q1 << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
-  R1 << 0.25;
-  SolveDAREandVerify(A1, B1, Q1, R1);
-
-  MatrixXd Aref1(n1, n1);
-  Aref1 << 0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0;
-  SolveDAREandVerify(A1, B1, (A1 - Aref1).transpose() * Q1 * (A1 - Aref1),
-      B1.transpose() * Q1 * B1 + R1, (A1 - Aref1).transpose() * Q1 * B1);
-
-  // Test 2: invertible A
-  int n2 = 2, m2 = 1;
-  MatrixXd A2(n2, n2), B2(n2, m2), Q2(n2, n2), R2(m2, m2);
-  A2 << 1, 1, 0, 1;
-  B2 << 0, 1;
-  Q2 << 1, 0, 0, 0;
-  R2 << 0.3;
-  SolveDAREandVerify(A2, B2, Q2, R2);
-
-  MatrixXd Aref2(n2, n2);
-  Aref2 << 0.5, 1, 0, 1;
-  SolveDAREandVerify(A2, B2, (A2 - Aref2).transpose() * Q2 * (A2 - Aref2),
-      B2.transpose() * Q2 * B2 + R2, (A2 - Aref2).transpose() * Q2 * B2);
-
-  // Test 3: the first generalized eigenvalue of (S,T) is stable
-  int n3 = 2, m3 = 1;
-  MatrixXd A3(n3, n3), B3(n3, m3), Q3(n3, n3), R3(m3, m3);
-  A3 << 0, 1, 0, 0;
-  B3 << 0, 1;
-  Q3 << 1, 0, 0, 1;
-  R3 << 1;
-  SolveDAREandVerify(A3, B3, Q3, R3);
-
-  MatrixXd Aref3(n3, n3);
-  Aref3 << 0, 0.5, 0, 0;
-  SolveDAREandVerify(A3, B3, (A3 - Aref3).transpose() * Q3 * (A3 - Aref3),
-      B3.transpose() * Q3 * B3 + R3, (A3 - Aref3).transpose() * Q3 * B3);
-
-  // Test 4: A = B = Q = R = I_2 (2-by-2 identity matrix)
-  const Eigen::MatrixXd A4{Eigen::Matrix2d::Identity()};
-  const Eigen::MatrixXd B4{Eigen::Matrix2d::Identity()};
-  const Eigen::MatrixXd Q4{Eigen::Matrix2d::Identity()};
-  const Eigen::MatrixXd R4{Eigen::Matrix2d::Identity()};
-  SolveDAREandVerify(A4, B4, Q4, R4);
-
-  const Eigen::MatrixXd N4{Eigen::Matrix2d::Identity()};
-  SolveDAREandVerify(A4, B4, Q4, R4, N4);
-}
-}  // namespace
-}  // namespace math
-}  // namespace drake
diff --git a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
index b2ee87d..d62decd 100644
--- a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
@@ -2,10 +2,10 @@
 // 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 <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/estimator/AngleStatistics.h"
 
diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
index 1d623ca..3103068 100644
--- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
@@ -7,13 +7,14 @@
 #include <tuple>
 #include <utility>
 
+#include <gtest/gtest.h>
+
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/DifferentialDrivePoseEstimator.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 #include "units/angle.h"
 #include "units/length.h"
 #include "units/time.h"
@@ -138,6 +139,7 @@
               0.15);
 
   if (checkError) {
+    // NOLINTNEXTLINE(bugprone-integer-division)
     EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.05);
     EXPECT_LT(maxError, 0.2);
   }
@@ -207,3 +209,87 @@
     }
   }
 }
+
+TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
+  // This tests for multiple vision measurements appled at the same time.
+  // The expected behavior is that all measurements affect the estimated pose.
+  // The alternative result is that only one vision measurement affects the
+  // outcome. If that were the case, after 1000 measurements, the estimated
+  // pose would converge to that measurement.
+  frc::DifferentialDriveKinematics kinematics{1.0_m};
+
+  frc::DifferentialDrivePoseEstimator estimator{
+      kinematics,
+      frc::Rotation2d{},
+      0_m,
+      0_m,
+      frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
+      {0.02, 0.02, 0.01},
+      {0.1, 0.1, 0.1}};
+
+  estimator.UpdateWithTime(0_s, frc::Rotation2d{}, 0_m, 0_m);
+
+  for (int i = 0; i < 1000; i++) {
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+}
+
+TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
+  frc::DifferentialDriveKinematics kinematics{1_m};
+
+  frc::DifferentialDrivePoseEstimator estimator{
+      kinematics,      frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
+      {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
+
+  // Add enough measurements to fill up the buffer
+  for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
+    estimator.UpdateWithTime(time, frc::Rotation2d{}, 0_m, 0_m);
+  }
+
+  auto odometryPose = estimator.GetEstimatedPosition();
+
+  // Apply a vision measurement from 3 seconds ago
+  estimator.AddVisionMeasurement(
+      frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
+      1_s, {0.1, 0.1, 0.1});
+
+  EXPECT_NEAR(odometryPose.X().value(),
+              estimator.GetEstimatedPosition().X().value(), 1e-6);
+  EXPECT_NEAR(odometryPose.Y().value(),
+              estimator.GetEstimatedPosition().Y().value(), 1e-6);
+  EXPECT_NEAR(odometryPose.Rotation().Radians().value(),
+              estimator.GetEstimatedPosition().Rotation().Radians().value(),
+              1e-6);
+}
diff --git a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
index 1924b73..2a5ce80 100644
--- a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
@@ -2,12 +2,12 @@
 // 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 <array>
 #include <cmath>
 
-#include "Eigen/QR"
+#include <Eigen/QR>
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/ExtendedKalmanFilter.h"
diff --git a/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
index fc373ca..6697ec0 100644
--- a/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
@@ -2,12 +2,12 @@
 // 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 <array>
 #include <cmath>
 
-#include "Eigen/Core"
+#include <Eigen/Core>
+#include <gtest/gtest.h>
+
 #include "frc/estimator/KalmanFilter.h"
 #include "frc/system/plant/DCMotor.h"
 #include "frc/system/plant/LinearSystemId.h"
diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
index 13dc5aa..8b3a586 100644
--- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
@@ -6,11 +6,12 @@
 #include <random>
 #include <tuple>
 
+#include <gtest/gtest.h>
+
 #include "frc/estimator/MecanumDrivePoseEstimator.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/MecanumDriveKinematics.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 
 void testFollowTrajectory(
     const frc::MecanumDriveKinematics& kinematics,
@@ -129,6 +130,7 @@
               0.15);
 
   if (checkError) {
+    // NOLINTNEXTLINE(bugprone-integer-division)
     EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.051);
     EXPECT_LT(maxError, 0.2);
   }
@@ -206,3 +208,90 @@
     }
   }
 }
+
+TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
+  // This tests for multiple vision measurements appled at the same time.
+  // The expected behavior is that all measurements affect the estimated pose.
+  // The alternative result is that only one vision measurement affects the
+  // outcome. If that were the case, after 1000 measurements, the estimated
+  // pose would converge to that measurement.
+  frc::MecanumDriveKinematics kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+  frc::MecanumDriveWheelPositions wheelPositions;
+
+  frc::MecanumDrivePoseEstimator estimator{
+      kinematics,      frc::Rotation2d{},
+      wheelPositions,  frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
+      {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
+
+  estimator.UpdateWithTime(0_s, frc::Rotation2d{}, wheelPositions);
+
+  for (int i = 0; i < 1000; i++) {
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+}
+
+TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
+  frc::MecanumDriveKinematics kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+  frc::MecanumDrivePoseEstimator estimator{
+      kinematics,    frc::Rotation2d{}, frc::MecanumDriveWheelPositions{},
+      frc::Pose2d{}, {0.1, 0.1, 0.1},   {0.45, 0.45, 0.45}};
+
+  // Add enough measurements to fill up the buffer
+  for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
+    estimator.UpdateWithTime(time, frc::Rotation2d{},
+                             frc::MecanumDriveWheelPositions{});
+  }
+
+  auto odometryPose = estimator.GetEstimatedPosition();
+
+  // Apply a vision measurement from 3 seconds ago
+  estimator.AddVisionMeasurement(
+      frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
+      1_s, {0.1, 0.1, 0.1});
+
+  EXPECT_NEAR(odometryPose.X().value(),
+              estimator.GetEstimatedPosition().X().value(), 1e-6);
+  EXPECT_NEAR(odometryPose.Y().value(),
+              estimator.GetEstimatedPosition().Y().value(), 1e-6);
+  EXPECT_NEAR(odometryPose.Rotation().Radians().value(),
+              estimator.GetEstimatedPosition().Rotation().Radians().value(),
+              1e-6);
+}
diff --git a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
index 19a734b..4471dc9 100644
--- a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
@@ -6,8 +6,6 @@
 
 #include "frc/estimator/MerweScaledSigmaPoints.h"
 
-namespace drake::math {
-namespace {
 TEST(MerweScaledSigmaPointsTest, ZeroMean) {
   frc::MerweScaledSigmaPoints<2> sigmaPoints;
   auto points = sigmaPoints.SquareRootSigmaPoints(
@@ -30,5 +28,3 @@
                                    {2.0, 2.0, 2.00548, 2.0, 1.99452}})
           .norm() < 1e-3);
 }
-}  // namespace
-}  // namespace drake::math
diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
index 554ca59..f1024ef 100644
--- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
@@ -8,12 +8,13 @@
 #include <tuple>
 
 #include <fmt/format.h>
+#include <gtest/gtest.h>
+#include <wpi/timestamp.h>
 
 #include "frc/estimator/SwerveDrivePoseEstimator.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/SwerveDriveKinematics.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 
 void testFollowTrajectory(
     const frc::SwerveDriveKinematics<4>& kinematics,
@@ -132,6 +133,7 @@
               0.15);
 
   if (checkError) {
+    // NOLINTNEXTLINE(bugprone-integer-division)
     EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.058);
     EXPECT_LT(maxError, 0.2);
   }
@@ -215,3 +217,97 @@
     }
   }
 }
+
+TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
+  // This tests for multiple vision measurements appled at the same time.
+  // The expected behavior is that all measurements affect the estimated pose.
+  // The alternative result is that only one vision measurement affects the
+  // outcome. If that were the case, after 1000 measurements, the estimated
+  // pose would converge to that measurement.
+  frc::SwerveDriveKinematics<4> kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+  frc::SwerveModulePosition fl;
+  frc::SwerveModulePosition fr;
+  frc::SwerveModulePosition bl;
+  frc::SwerveModulePosition br;
+
+  frc::SwerveDrivePoseEstimator<4> estimator{
+      kinematics,       frc::Rotation2d{},
+      {fl, fr, bl, br}, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
+      {0.1, 0.1, 0.1},  {0.45, 0.45, 0.45}};
+
+  estimator.UpdateWithTime(0_s, frc::Rotation2d{}, {fl, fr, bl, br});
+
+  for (int i = 0; i < 1000; i++) {
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
+    estimator.AddVisionMeasurement(
+        frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+
+  {
+    auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
+    auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
+    auto dtheta = units::math::abs(
+        estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
+
+    EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
+  }
+}
+
+TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
+  frc::SwerveDriveKinematics<4> kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+  frc::SwerveModulePosition fl;
+  frc::SwerveModulePosition fr;
+  frc::SwerveModulePosition bl;
+  frc::SwerveModulePosition br;
+
+  frc::SwerveDrivePoseEstimator<4> estimator{
+      kinematics,    frc::Rotation2d{}, {fl, fr, bl, br},
+      frc::Pose2d{}, {0.1, 0.1, 0.1},   {0.45, 0.45, 0.45}};
+
+  // Add enough measurements to fill up the buffer
+  for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
+    estimator.UpdateWithTime(time, frc::Rotation2d{}, {fl, fr, bl, br});
+  }
+
+  auto odometryPose = estimator.GetEstimatedPosition();
+
+  // Apply a vision measurement from 3 seconds ago
+  estimator.AddVisionMeasurement(
+      frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
+      1_s, {0.1, 0.1, 0.1});
+
+  EXPECT_NEAR(odometryPose.X().value(),
+              estimator.GetEstimatedPosition().X().value(), 1e-6);
+  EXPECT_NEAR(odometryPose.Y().value(),
+              estimator.GetEstimatedPosition().Y().value(), 1e-6);
+  EXPECT_NEAR(odometryPose.Rotation().Radians().value(),
+              estimator.GetEstimatedPosition().Rotation().Radians().value(),
+              1e-6);
+}
diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
index 68f9c40..0f97a88 100644
--- a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
@@ -2,12 +2,12 @@
 // 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 <array>
 #include <cmath>
 
-#include "Eigen/QR"
+#include <Eigen/QR>
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/AngleStatistics.h"
diff --git a/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp
index 2f64908..d232739 100644
--- a/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/DebouncerTest.cpp
@@ -2,10 +2,10 @@
 // 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 <wpi/timestamp.h>
 
 #include "frc/filter/Debouncer.h"
-#include "gtest/gtest.h"
 #include "units/time.h"
 
 static units::second_t now = 0_s;
diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
index 8299a71..e0dea2d 100644
--- a/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
@@ -8,7 +8,8 @@
 #include <numbers>
 #include <random>
 
-#include "gtest/gtest.h"
+#include <gtest/gtest.h>
+
 #include "units/time.h"
 
 // Filter constants
diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
index 152737d..9195525 100644
--- a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
@@ -10,9 +10,9 @@
 #include <numbers>
 #include <random>
 
+#include <gtest/gtest.h>
 #include <wpi/array.h>
 
-#include "gtest/gtest.h"
 #include "units/time.h"
 
 // Filter constants
diff --git a/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp b/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp
index 8151a45..f0f7d99 100644
--- a/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp
@@ -2,8 +2,9 @@
 // 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 "frc/filter/MedianFilter.h"
-#include "gtest/gtest.h"
 
 TEST(MedianFilterTest, MedianFilterNotFullTestEven) {
   frc::MedianFilter<double> filter{10};
diff --git a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
index 5dbe8c8..9657543 100644
--- a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
@@ -2,10 +2,10 @@
 // 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 <wpi/timestamp.h>
 
 #include "frc/filter/SlewRateLimiter.h"
-#include "gtest/gtest.h"
 #include "units/length.h"
 #include "units/time.h"
 #include "units/velocity.h"
diff --git a/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
index fc44fa5..198f90f 100644
--- a/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
@@ -2,10 +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.
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/CoordinateSystem.h"
 #include "frc/geometry/Pose3d.h"
 #include "frc/geometry/Transform3d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
@@ -37,9 +38,6 @@
   EXPECT_EQ(transformTo.Translation(),
             CoordinateSystem::Convert(transformFrom.Translation(), coordFrom,
                                       coordTo));
-  EXPECT_EQ(
-      transformTo.Rotation(),
-      CoordinateSystem::Convert(transformFrom.Rotation(), coordFrom, coordTo));
   EXPECT_EQ(transformTo,
             CoordinateSystem::Convert(transformFrom, coordFrom, coordTo));
 
@@ -47,9 +45,6 @@
   EXPECT_EQ(
       transformFrom.Translation(),
       CoordinateSystem::Convert(transformTo.Translation(), coordTo, coordFrom));
-  EXPECT_EQ(
-      transformFrom.Rotation(),
-      CoordinateSystem::Convert(transformTo.Rotation(), coordTo, coordFrom));
   EXPECT_EQ(transformFrom,
             CoordinateSystem::Convert(transformTo, coordTo, coordFrom));
 }
@@ -106,29 +101,28 @@
   // No rotation from EDN to NWU
   CheckTransform3dConvert(
       Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{}},
-      Transform3d{Translation3d{3_m, -1_m, -2_m},
-                  Rotation3d{-90_deg, 0_deg, -90_deg}},
+      Transform3d{Translation3d{3_m, -1_m, -2_m}, Rotation3d{}},
       CoordinateSystem::EDN(), CoordinateSystem::NWU());
 
   // 45° roll from EDN to NWU
   CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
                                       Rotation3d{45_deg, 0_deg, 0_deg}},
                           Transform3d{Translation3d{3_m, -1_m, -2_m},
-                                      Rotation3d{-45_deg, 0_deg, -90_deg}},
+                                      Rotation3d{0_deg, -45_deg, 0_deg}},
                           CoordinateSystem::EDN(), CoordinateSystem::NWU());
 
   // 45° pitch from EDN to NWU
   CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
                                       Rotation3d{0_deg, 45_deg, 0_deg}},
                           Transform3d{Translation3d{3_m, -1_m, -2_m},
-                                      Rotation3d{-90_deg, 0_deg, -135_deg}},
+                                      Rotation3d{0_deg, 0_deg, -45_deg}},
                           CoordinateSystem::EDN(), CoordinateSystem::NWU());
 
   // 45° yaw from EDN to NWU
   CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
                                       Rotation3d{0_deg, 0_deg, 45_deg}},
                           Transform3d{Translation3d{3_m, -1_m, -2_m},
-                                      Rotation3d{-90_deg, 45_deg, -90_deg}},
+                                      Rotation3d{45_deg, 0_deg, 0_deg}},
                           CoordinateSystem::EDN(), CoordinateSystem::NWU());
 }
 
@@ -136,28 +130,27 @@
   // No rotation from EDN to NED
   CheckTransform3dConvert(
       Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{}},
-      Transform3d{Translation3d{3_m, 1_m, 2_m},
-                  Rotation3d{90_deg, 0_deg, 90_deg}},
+      Transform3d{Translation3d{3_m, 1_m, 2_m}, Rotation3d{}},
       CoordinateSystem::EDN(), CoordinateSystem::NED());
 
   // 45° roll from EDN to NED
   CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
                                       Rotation3d{45_deg, 0_deg, 0_deg}},
                           Transform3d{Translation3d{3_m, 1_m, 2_m},
-                                      Rotation3d{135_deg, 0_deg, 90_deg}},
+                                      Rotation3d{0_deg, 45_deg, 0_deg}},
                           CoordinateSystem::EDN(), CoordinateSystem::NED());
 
   // 45° pitch from EDN to NED
   CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
                                       Rotation3d{0_deg, 45_deg, 0_deg}},
                           Transform3d{Translation3d{3_m, 1_m, 2_m},
-                                      Rotation3d{90_deg, 0_deg, 135_deg}},
+                                      Rotation3d{0_deg, 0_deg, 45_deg}},
                           CoordinateSystem::EDN(), CoordinateSystem::NED());
 
   // 45° yaw from EDN to NED
   CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
                                       Rotation3d{0_deg, 0_deg, 45_deg}},
                           Transform3d{Translation3d{3_m, 1_m, 2_m},
-                                      Rotation3d{90_deg, -45_deg, 90_deg}},
+                                      Rotation3d{45_deg, 0_deg, 0_deg}},
                           CoordinateSystem::EDN(), CoordinateSystem::NED());
 }
diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
index 5ce6819..bd5a9aa 100644
--- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
@@ -3,12 +3,32 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <cmath>
+#include <cstdlib>
+
+#include <gtest/gtest.h>
 
 #include "frc/geometry/Pose2d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
+TEST(Pose2dTest, RotateBy) {
+  constexpr auto x = 1_m;
+  constexpr auto y = 2_m;
+  const Pose2d initial{x, y, 45_deg};
+
+  const Rotation2d rotation{5_deg};
+  const auto rotated = initial.RotateBy(rotation);
+
+  // Translation is rotated by CCW rotation matrix
+  double c = rotation.Cos();
+  double s = rotation.Sin();
+  EXPECT_DOUBLE_EQ(c * x.value() - s * y.value(), rotated.X().value());
+  EXPECT_DOUBLE_EQ(s * x.value() + c * y.value(), rotated.Y().value());
+  EXPECT_DOUBLE_EQ(
+      initial.Rotation().Degrees().value() + rotation.Degrees().value(),
+      rotated.Rotation().Degrees().value());
+}
+
 TEST(Pose2dTest, TransformBy) {
   const Pose2d initial{1_m, 2_m, 45_deg};
   const Transform2d transform{Translation2d{5_m, 0_m}, 5_deg};
@@ -26,9 +46,9 @@
 
   const auto finalRelativeToInitial = final.RelativeTo(initial);
 
-  EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value());
+  EXPECT_NEAR(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value(), 1e-9);
   EXPECT_NEAR(0.0, finalRelativeToInitial.Y().value(), 1e-9);
-  EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Rotation().Degrees().value());
+  EXPECT_NEAR(0.0, finalRelativeToInitial.Rotation().Degrees().value(), 1e-9);
 }
 
 TEST(Pose2dTest, Equality) {
@@ -49,9 +69,76 @@
 
   const auto transform = final - initial;
 
-  EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), transform.X().value());
+  EXPECT_NEAR(5.0 * std::sqrt(2.0), transform.X().value(), 1e-9);
   EXPECT_NEAR(0.0, transform.Y().value(), 1e-9);
-  EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Degrees().value());
+  EXPECT_NEAR(0.0, transform.Rotation().Degrees().value(), 1e-9);
+}
+
+TEST(Pose2dTest, Nearest) {
+  const Pose2d origin{0_m, 0_m, 0_deg};
+
+  const Pose2d pose1{Translation2d{1_m, Rotation2d{45_deg}}, 0_deg};
+  const Pose2d pose2{Translation2d{2_m, Rotation2d{90_deg}}, 0_deg};
+  const Pose2d pose3{Translation2d{3_m, Rotation2d{135_deg}}, 0_deg};
+  const Pose2d pose4{Translation2d{4_m, Rotation2d{180_deg}}, 0_deg};
+  const Pose2d pose5{Translation2d{5_m, Rotation2d{270_deg}}, 0_deg};
+
+  EXPECT_DOUBLE_EQ(pose3.X().value(),
+                   origin.Nearest({pose5, pose3, pose4}).X().value());
+  EXPECT_DOUBLE_EQ(pose3.Y().value(),
+                   origin.Nearest({pose5, pose3, pose4}).Y().value());
+
+  EXPECT_DOUBLE_EQ(pose1.X().value(),
+                   origin.Nearest({pose1, pose2, pose3}).X().value());
+  EXPECT_DOUBLE_EQ(pose1.Y().value(),
+                   origin.Nearest({pose1, pose2, pose3}).Y().value());
+
+  EXPECT_DOUBLE_EQ(pose2.X().value(),
+                   origin.Nearest({pose4, pose2, pose3}).X().value());
+  EXPECT_DOUBLE_EQ(pose2.Y().value(),
+                   origin.Nearest({pose4, pose2, pose3}).Y().value());
+
+  // Rotation component sort (when distance is the same)
+  // Use the same translation because using different angles at the same
+  // distance can cause rounding error.
+  const Translation2d translation{1_m, Rotation2d{0_deg}};
+
+  const Pose2d poseA{translation, 0_deg};
+  const Pose2d poseB{translation, Rotation2d{30_deg}};
+  const Pose2d poseC{translation, Rotation2d{120_deg}};
+  const Pose2d poseD{translation, Rotation2d{90_deg}};
+  const Pose2d poseE{translation, Rotation2d{-180_deg}};
+
+  EXPECT_DOUBLE_EQ(poseA.Rotation().Degrees().value(),
+                   Pose2d(0_m, 0_m, Rotation2d{360_deg})
+                       .Nearest({poseA, poseB, poseD})
+                       .Rotation()
+                       .Degrees()
+                       .value());
+  EXPECT_DOUBLE_EQ(poseB.Rotation().Degrees().value(),
+                   Pose2d(0_m, 0_m, Rotation2d{-335_deg})
+                       .Nearest({poseB, poseC, poseD})
+                       .Rotation()
+                       .Degrees()
+                       .value());
+  EXPECT_DOUBLE_EQ(poseC.Rotation().Degrees().value(),
+                   Pose2d(0_m, 0_m, Rotation2d{-120_deg})
+                       .Nearest({poseB, poseC, poseD})
+                       .Rotation()
+                       .Degrees()
+                       .value());
+  EXPECT_DOUBLE_EQ(poseD.Rotation().Degrees().value(),
+                   Pose2d(0_m, 0_m, Rotation2d{85_deg})
+                       .Nearest({poseA, poseC, poseD})
+                       .Rotation()
+                       .Degrees()
+                       .value());
+  EXPECT_DOUBLE_EQ(poseE.Rotation().Degrees().value(),
+                   Pose2d(0_m, 0_m, Rotation2d{170_deg})
+                       .Nearest({poseA, poseD, poseE})
+                       .Rotation()
+                       .Degrees()
+                       .value());
 }
 
 TEST(Pose2dTest, Constexpr) {
diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
index 8c4452c..6d689db 100644
--- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
@@ -4,11 +4,34 @@
 
 #include <cmath>
 
+#include <gtest/gtest.h>
+#include <wpi/array.h>
+
 #include "frc/geometry/Pose3d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
+TEST(Pose3dTest, RotateBy) {
+  constexpr auto x = 1_m;
+  constexpr auto y = 2_m;
+  const Pose3d initial{x, y, 0_m, Rotation3d{0_deg, 0_deg, 45_deg}};
+
+  constexpr units::radian_t yaw = 5_deg;
+  const Rotation3d rotation{0_deg, 0_deg, yaw};
+  const auto rotated = initial.RotateBy(rotation);
+
+  // Translation is rotated by CCW rotation matrix
+  double c = std::cos(yaw.value());
+  double s = std::sin(yaw.value());
+  EXPECT_DOUBLE_EQ(c * x.value() - s * y.value(), rotated.X().value());
+  EXPECT_DOUBLE_EQ(s * x.value() + c * y.value(), rotated.Y().value());
+  EXPECT_DOUBLE_EQ(0.0, rotated.Z().value());
+  EXPECT_DOUBLE_EQ(0.0, rotated.Rotation().X().value());
+  EXPECT_DOUBLE_EQ(0.0, rotated.Rotation().Y().value());
+  EXPECT_DOUBLE_EQ(initial.Rotation().Z().value() + rotation.Z().value(),
+                   rotated.Rotation().Z().value());
+}
+
 TEST(Pose3dTest, TestTransformByRotations) {
   const double kEpsilon = 1E-9;
 
@@ -95,3 +118,86 @@
 
   EXPECT_EQ(expected, pose.ToPose2d());
 }
+
+TEST(Pose3dTest, ComplexTwists) {
+  wpi::array<Pose3d, 5> initial_poses{
+      Pose3d{0.698303_m, -0.959096_m, 0.271076_m,
+             Rotation3d{Quaternion{0.86403, -0.076866, 0.147234, 0.475254}}},
+      Pose3d{0.634892_m, -0.765209_m, 0.117543_m,
+             Rotation3d{Quaternion{0.84987, -0.070829, 0.162097, 0.496415}}},
+      Pose3d{0.584827_m, -0.590303_m, -0.02557_m,
+             Rotation3d{Quaternion{0.832743, -0.041991, 0.202188, 0.513708}}},
+      Pose3d{0.505038_m, -0.451479_m, -0.112835_m,
+             Rotation3d{Quaternion{0.816515, -0.002673, 0.226182, 0.531166}}},
+      Pose3d{0.428178_m, -0.329692_m, -0.189707_m,
+             Rotation3d{Quaternion{0.807886, 0.029298, 0.257788, 0.529157}}},
+  };
+
+  wpi::array<Pose3d, 5> final_poses{
+      Pose3d{-0.230448_m, -0.511957_m, 0.198406_m,
+             Rotation3d{Quaternion{0.753984, 0.347016, 0.409105, 0.379106}}},
+      Pose3d{-0.088932_m, -0.343253_m, 0.095018_m,
+             Rotation3d{Quaternion{0.638738, 0.413016, 0.536281, 0.365833}}},
+      Pose3d{-0.107908_m, -0.317552_m, 0.133946_m,
+             Rotation3d{Quaternion{0.653444, 0.417069, 0.465505, 0.427046}}},
+      Pose3d{-0.123383_m, -0.156411_m, -0.047435_m,
+             Rotation3d{Quaternion{0.652983, 0.40644, 0.431566, 0.47135}}},
+      Pose3d{-0.084654_m, -0.019305_m, -0.030022_m,
+             Rotation3d{Quaternion{0.620243, 0.429104, 0.479384, 0.44873}}},
+  };
+
+  for (size_t i = 0; i < initial_poses.size(); i++) {
+    auto start = initial_poses[i];
+    auto end = final_poses[i];
+
+    auto twist = start.Log(end);
+    auto start_exp = start.Exp(twist);
+
+    auto eps = 1E-5;
+
+    EXPECT_NEAR(start_exp.X().value(), end.X().value(), eps);
+    EXPECT_NEAR(start_exp.Y().value(), end.Y().value(), eps);
+    EXPECT_NEAR(start_exp.Z().value(), end.Z().value(), eps);
+    EXPECT_NEAR(start_exp.Rotation().GetQuaternion().W(),
+                end.Rotation().GetQuaternion().W(), eps);
+    EXPECT_NEAR(start_exp.Rotation().GetQuaternion().X(),
+                end.Rotation().GetQuaternion().X(), eps);
+    EXPECT_NEAR(start_exp.Rotation().GetQuaternion().Y(),
+                end.Rotation().GetQuaternion().Y(), eps);
+    EXPECT_NEAR(start_exp.Rotation().GetQuaternion().Z(),
+                end.Rotation().GetQuaternion().Z(), eps);
+  }
+}
+
+TEST(Pose3dTest, TwistNaN) {
+  wpi::array<Pose3d, 2> initial_poses{
+      Pose3d{6.32_m, 4.12_m, 0.00_m,
+             Rotation3d{Quaternion{-0.9999999999999999, 0.0, 0.0,
+                                   1.9208309264993548E-8}}},
+      Pose3d{3.75_m, 2.95_m, 0.00_m,
+             Rotation3d{Quaternion{0.9999999999999793, 0.0, 0.0,
+                                   2.0352360299846772E-7}}},
+  };
+
+  wpi::array<Pose3d, 2> final_poses{
+      Pose3d{6.33_m, 4.15_m, 0.00_m,
+             Rotation3d{Quaternion{-0.9999999999999999, 0.0, 0.0,
+                                   2.416890209039172E-8}}},
+      Pose3d{3.66_m, 2.93_m, 0.00_m,
+             Rotation3d{Quaternion{0.9999999999999782, 0.0, 0.0,
+                                   2.0859477994905617E-7}}},
+  };
+
+  for (size_t i = 0; i < initial_poses.size(); i++) {
+    auto start = initial_poses[i];
+    auto end = final_poses[i];
+    auto twist = start.Log(end);
+
+    EXPECT_FALSE(std::isnan(twist.dx.value()));
+    EXPECT_FALSE(std::isnan(twist.dy.value()));
+    EXPECT_FALSE(std::isnan(twist.dz.value()));
+    EXPECT_FALSE(std::isnan(twist.rx.value()));
+    EXPECT_FALSE(std::isnan(twist.ry.value()));
+    EXPECT_FALSE(std::isnan(twist.rz.value()));
+  }
+}
diff --git a/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp b/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp
index 5b95abb..ef0e95e 100644
--- a/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp
@@ -4,8 +4,9 @@
 
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Quaternion.h"
-#include "gtest/gtest.h"
 #include "units/angle.h"
 #include "units/math.h"
 
@@ -43,6 +44,54 @@
                             q3.Z() * q3.Z());
 }
 
+TEST(QuaternionTest, Addition) {
+  Quaternion q{0.1, 0.2, 0.3, 0.4};
+  Quaternion p{0.5, 0.6, 0.7, 0.8};
+
+  auto sum = q + p;
+
+  EXPECT_DOUBLE_EQ(q.W() + p.W(), sum.W());
+  EXPECT_DOUBLE_EQ(q.X() + p.X(), sum.X());
+  EXPECT_DOUBLE_EQ(q.Y() + p.Y(), sum.Y());
+  EXPECT_DOUBLE_EQ(q.Z() + p.Z(), sum.Z());
+}
+
+TEST(QuaternionTest, Subtraction) {
+  Quaternion q{0.1, 0.2, 0.3, 0.4};
+  Quaternion p{0.5, 0.6, 0.7, 0.8};
+
+  auto difference = q - p;
+
+  EXPECT_DOUBLE_EQ(q.W() - p.W(), difference.W());
+  EXPECT_DOUBLE_EQ(q.X() - p.X(), difference.X());
+  EXPECT_DOUBLE_EQ(q.Y() - p.Y(), difference.Y());
+  EXPECT_DOUBLE_EQ(q.Z() - p.Z(), difference.Z());
+}
+
+TEST(QuaternionTest, ScalarMultiplication) {
+  Quaternion q{0.1, 0.2, 0.3, 0.4};
+  auto scalar = 2;
+
+  auto product = q * scalar;
+
+  EXPECT_DOUBLE_EQ(q.W() * scalar, product.W());
+  EXPECT_DOUBLE_EQ(q.X() * scalar, product.X());
+  EXPECT_DOUBLE_EQ(q.Y() * scalar, product.Y());
+  EXPECT_DOUBLE_EQ(q.Z() * scalar, product.Z());
+}
+
+TEST(QuaternionTest, ScalarDivision) {
+  Quaternion q{0.1, 0.2, 0.3, 0.4};
+  auto scalar = 2;
+
+  auto product = q / scalar;
+
+  EXPECT_DOUBLE_EQ(q.W() / scalar, product.W());
+  EXPECT_DOUBLE_EQ(q.X() / scalar, product.X());
+  EXPECT_DOUBLE_EQ(q.Y() / scalar, product.Y());
+  EXPECT_DOUBLE_EQ(q.Z() / scalar, product.Z());
+}
+
 TEST(QuaternionTest, Multiply) {
   // 90° CCW rotations around each axis
   double c = units::math::cos(90_deg / 2.0);
@@ -64,19 +113,110 @@
   Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
                0.48507125007266594};
   actual = q * q.Inverse();
-  EXPECT_DOUBLE_EQ(1.0, actual.W());
-  EXPECT_DOUBLE_EQ(0.0, actual.X());
-  EXPECT_DOUBLE_EQ(0.0, actual.Y());
-  EXPECT_DOUBLE_EQ(0.0, actual.Z());
+  EXPECT_NEAR(1.0, actual.W(), 1e-9);
+  EXPECT_NEAR(0.0, actual.X(), 1e-9);
+  EXPECT_NEAR(0.0, actual.Y(), 1e-9);
+  EXPECT_NEAR(0.0, actual.Z(), 1e-9);
+}
+
+TEST(QuaternionTest, Conjugate) {
+  Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
+               0.48507125007266594};
+  auto conj = q.Conjugate();
+
+  EXPECT_DOUBLE_EQ(q.W(), conj.W());
+  EXPECT_DOUBLE_EQ(-q.X(), conj.X());
+  EXPECT_DOUBLE_EQ(-q.Y(), conj.Y());
+  EXPECT_DOUBLE_EQ(-q.Z(), conj.Z());
 }
 
 TEST(QuaternionTest, Inverse) {
   Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
                0.48507125007266594};
+  auto norm = q.Norm();
+
   auto inv = q.Inverse();
 
-  EXPECT_DOUBLE_EQ(q.W(), inv.W());
-  EXPECT_DOUBLE_EQ(-q.X(), inv.X());
-  EXPECT_DOUBLE_EQ(-q.Y(), inv.Y());
-  EXPECT_DOUBLE_EQ(-q.Z(), inv.Z());
+  EXPECT_DOUBLE_EQ(q.W() / (norm * norm), inv.W());
+  EXPECT_DOUBLE_EQ(-q.X() / (norm * norm), inv.X());
+  EXPECT_DOUBLE_EQ(-q.Y() / (norm * norm), inv.Y());
+  EXPECT_DOUBLE_EQ(-q.Z() / (norm * norm), inv.Z());
+}
+
+TEST(QuaternionTest, Norm) {
+  Quaternion q{3, 4, 12, 84};
+  auto norm = q.Norm();
+
+  EXPECT_NEAR(85, norm, 1e-9);
+}
+
+TEST(QuaternionTest, Exponential) {
+  Quaternion q{1.1, 2.2, 3.3, 4.4};
+  Quaternion expect{2.81211398529184, -0.392521193481878, -0.588781790222817,
+                    -0.785042386963756};
+
+  auto q_exp = q.Exp();
+
+  EXPECT_EQ(expect, q_exp);
+}
+
+TEST(QuaternionTest, Logarithm) {
+  Quaternion q{1.1, 2.2, 3.3, 4.4};
+  Quaternion expect{1.7959088706354, 0.515190292664085, 0.772785438996128,
+                    1.03038058532817};
+
+  auto q_log = q.Log();
+
+  EXPECT_EQ(expect, q_log);
+
+  Quaternion zero{0, 0, 0, 0};
+  Quaternion one{1, 0, 0, 0};
+  Quaternion i{0, 1, 0, 0};
+  Quaternion j{0, 0, 1, 0};
+  Quaternion k{0, 0, 0, 1};
+  Quaternion ln_half{std::log(0.5), -std::numbers::pi, 0, 0};
+
+  EXPECT_EQ(zero, one.Log());
+  EXPECT_EQ(i * std::numbers::pi / 2, i.Log());
+  EXPECT_EQ(j * std::numbers::pi / 2, j.Log());
+  EXPECT_EQ(k * std::numbers::pi / 2, k.Log());
+
+  EXPECT_EQ(i * -std::numbers::pi, (one * -1).Log());
+  EXPECT_EQ(ln_half, (one * -0.5).Log());
+}
+
+TEST(QuaternionTest, LogarithmAndExponentialInverse) {
+  Quaternion q{1.1, 2.2, 3.3, 4.4};
+
+  // These operations are order-dependent: ln(exp(q)) is congruent but not
+  // necessarily equal to exp(ln(q)) due to the multi-valued nature of the
+  // complex logarithm.
+
+  auto q_log_exp = q.Log().Exp();
+
+  EXPECT_EQ(q, q_log_exp);
+
+  Quaternion start{1, 2, 3, 4};
+  Quaternion expect{5, 6, 7, 8};
+
+  auto twist = start.Log(expect);
+  auto actual = start.Exp(twist);
+
+  EXPECT_EQ(expect, actual);
+}
+
+TEST(QuaternionTest, DotProduct) {
+  Quaternion q{1.1, 2.2, 3.3, 4.4};
+  Quaternion p{5.5, 6.6, 7.7, 8.8};
+
+  EXPECT_NEAR(q.W() * p.W() + q.X() * p.X() + q.Y() * p.Y() + q.Z() * p.Z(),
+              q.Dot(p), 1e-9);
+}
+
+TEST(QuaternionTest, DotProductAsEquality) {
+  Quaternion q{1.1, 2.2, 3.3, 4.4};
+  auto q_conj = q.Conjugate();
+
+  EXPECT_NEAR(q.Dot(q), q.Norm() * q.Norm(), 1e-9);
+  EXPECT_GT(std::abs(q.Dot(q_conj) - q.Norm() * q_conj.Norm()), 1e-9);
 }
diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
index 4e2e683..de2fc17 100644
--- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
@@ -5,8 +5,9 @@
 #include <cmath>
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Rotation2d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
index 4709ed0..01b37e5 100644
--- a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
@@ -5,52 +5,93 @@
 #include <cmath>
 #include <numbers>
 
+#include <Eigen/Core>
+#include <gtest/gtest.h>
 #include <wpi/MathExtras.h>
 
-#include "frc/EigenCore.h"
 #include "frc/geometry/Rotation3d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
+TEST(Rotation3dTest, GimbalLockAccuracy) {
+  auto rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}};
+  auto rot2 = Rotation3d{units::radian_t{std::numbers::pi}, 0_rad, 0_rad};
+  auto rot3 = Rotation3d{-units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
+  const auto result1 = rot1 + rot2 + rot3;
+  const auto expected1 =
+      Rotation3d{0_rad, -units::radian_t{std::numbers::pi / 2},
+                 units::radian_t{std::numbers::pi / 2}};
+  EXPECT_EQ(expected1, result1);
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 2, (result1.X() + result1.Z()).value());
+  EXPECT_DOUBLE_EQ(-std::numbers::pi / 2, result1.Y().value());
+
+  rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}};
+  rot2 = Rotation3d{units::radian_t{-std::numbers::pi}, 0_rad, 0_rad};
+  rot3 = Rotation3d{units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
+  const auto result2 = rot1 + rot2 + rot3;
+  const auto expected2 =
+      Rotation3d{0_rad, units::radian_t{std::numbers::pi / 2},
+                 units::radian_t{std::numbers::pi / 2}};
+  EXPECT_EQ(expected2, result2);
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 2, (result2.Z() - result2.X()).value());
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 2, result2.Y().value());
+
+  rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}};
+  rot2 = Rotation3d{0_rad, units::radian_t{std::numbers::pi / 3}, 0_rad};
+  rot3 = Rotation3d{-units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
+  const auto result3 = rot1 + rot2 + rot3;
+  const auto expected3 =
+      Rotation3d{0_rad, units::radian_t{std::numbers::pi / 2},
+                 units::radian_t{std::numbers::pi / 6}};
+  EXPECT_EQ(expected3, result3);
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 6, (result3.Z() - result3.X()).value());
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 2, result3.Y().value());
+}
+
 TEST(Rotation3dTest, InitAxisAngleAndRollPitchYaw) {
   const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
   const Rotation3d rot1{xAxis, units::radian_t{std::numbers::pi / 3}};
   const Rotation3d rot2{units::radian_t{std::numbers::pi / 3}, 0_rad, 0_rad};
+  const Rotation3d rvec1{Eigen::Vector3d{xAxis * std::numbers::pi / 3}};
   EXPECT_EQ(rot1, rot2);
+  EXPECT_EQ(rot1, rvec1);
 
   const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
   const Rotation3d rot3{yAxis, units::radian_t{std::numbers::pi / 3}};
   const Rotation3d rot4{0_rad, units::radian_t{std::numbers::pi / 3}, 0_rad};
+  const Rotation3d rvec2{Eigen::Vector3d{yAxis * std::numbers::pi / 3}};
   EXPECT_EQ(rot3, rot4);
+  EXPECT_EQ(rot3, rvec2);
 
   const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
   const Rotation3d rot5{zAxis, units::radian_t{std::numbers::pi / 3}};
   const Rotation3d rot6{0_rad, 0_rad, units::radian_t{std::numbers::pi / 3}};
+  const Rotation3d rvec3{Eigen::Vector3d{zAxis * std::numbers::pi / 3}};
   EXPECT_EQ(rot5, rot6);
+  EXPECT_EQ(rot5, rvec3);
 }
 
 TEST(Rotation3dTest, InitRotationMatrix) {
   // No rotation
-  const Matrixd<3, 3> R1 = Matrixd<3, 3>::Identity();
+  const Eigen::Matrix3d R1 = Eigen::Matrix3d::Identity();
   const Rotation3d rot1{R1};
   EXPECT_EQ(Rotation3d{}, rot1);
 
   // 90 degree CCW rotation around z-axis
-  Matrixd<3, 3> R2;
-  R2.block<3, 1>(0, 0) = Vectord<3>{0.0, 1.0, 0.0};
-  R2.block<3, 1>(0, 1) = Vectord<3>{-1.0, 0.0, 0.0};
-  R2.block<3, 1>(0, 2) = Vectord<3>{0.0, 0.0, 1.0};
+  Eigen::Matrix3d R2;
+  R2.block<3, 1>(0, 0) = Eigen::Vector3d{0.0, 1.0, 0.0};
+  R2.block<3, 1>(0, 1) = Eigen::Vector3d{-1.0, 0.0, 0.0};
+  R2.block<3, 1>(0, 2) = Eigen::Vector3d{0.0, 0.0, 1.0};
   const Rotation3d rot2{R2};
   const Rotation3d expected2{0_deg, 0_deg, 90_deg};
   EXPECT_EQ(expected2, rot2);
 
   // Matrix that isn't orthogonal
-  const Matrixd<3, 3> R3{{1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}};
+  const Eigen::Matrix3d R3{{1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}};
   EXPECT_THROW(Rotation3d{R3}, std::domain_error);
 
   // Matrix that's orthogonal but not special orthogonal
-  const Matrixd<3, 3> R4 = Matrixd<3, 3>::Identity() * 2.0;
+  const Eigen::Matrix3d R4 = Eigen::Matrix3d::Identity() * 2.0;
   EXPECT_THROW(Rotation3d{R4}, std::domain_error);
 }
 
diff --git a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
index 1b0934d..49c9466 100644
--- a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
@@ -4,11 +4,12 @@
 
 #include <cmath>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
 #include "frc/geometry/Transform2d.h"
 #include "frc/geometry/Translation2d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
diff --git a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
index 904ec9c..8a6bbc0 100644
--- a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
@@ -4,11 +4,12 @@
 
 #include <cmath>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Pose3d.h"
 #include "frc/geometry/Rotation3d.h"
 #include "frc/geometry/Transform3d.h"
 #include "frc/geometry/Translation3d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
index 5493c43..1c92c0a 100644
--- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
@@ -4,8 +4,9 @@
 
 #include <cmath>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Translation2d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
@@ -94,6 +95,37 @@
   EXPECT_DOUBLE_EQ(std::sqrt(3.0), two.Y().value());
 }
 
+TEST(Translation2dTest, Nearest) {
+  const Translation2d origin{0_m, 0_m};
+
+  const Translation2d translation1{1_m, Rotation2d{45_deg}};
+  const Translation2d translation2{2_m, Rotation2d{90_deg}};
+  const Translation2d translation3{3_m, Rotation2d{135_deg}};
+  const Translation2d translation4{4_m, Rotation2d{180_deg}};
+  const Translation2d translation5{5_m, Rotation2d{270_deg}};
+
+  EXPECT_DOUBLE_EQ(
+      origin.Nearest({translation5, translation3, translation4}).X().value(),
+      translation3.X().value());
+  EXPECT_DOUBLE_EQ(
+      origin.Nearest({translation5, translation3, translation4}).Y().value(),
+      translation3.Y().value());
+
+  EXPECT_DOUBLE_EQ(
+      origin.Nearest({translation1, translation2, translation3}).X().value(),
+      translation1.X().value());
+  EXPECT_DOUBLE_EQ(
+      origin.Nearest({translation1, translation2, translation3}).Y().value(),
+      translation1.Y().value());
+
+  EXPECT_DOUBLE_EQ(
+      origin.Nearest({translation4, translation2, translation3}).X().value(),
+      translation2.X().value());
+  EXPECT_DOUBLE_EQ(
+      origin.Nearest({translation4, translation2, translation3}).Y().value(),
+      translation2.Y().value());
+}
+
 TEST(Translation2dTest, Constexpr) {
   constexpr Translation2d defaultCtor;
   constexpr Translation2d componentCtor{1_m, 2_m};
diff --git a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
index 58c611e..b7aa8ce 100644
--- a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
@@ -4,8 +4,9 @@
 
 #include <cmath>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Translation3d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
diff --git a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
index 33970cd..7f121c0 100644
--- a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
@@ -5,8 +5,9 @@
 #include <cmath>
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Pose2d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
diff --git a/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
index 0d8a8f4..245ec17 100644
--- a/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
@@ -5,8 +5,9 @@
 #include <cmath>
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Pose3d.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
diff --git a/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp b/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
index 8a920f6..e448db2 100644
--- a/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
+++ b/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
@@ -4,10 +4,11 @@
 
 #include <cmath>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
 #include "frc/interpolation/TimeInterpolatableBuffer.h"
-#include "gtest/gtest.h"
 #include "units/time.h"
 
 TEST(TimeInterpolatableBufferTest, Interpolation) {
diff --git a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
index 4e0b3e5..97de03b 100644
--- a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
@@ -2,12 +2,34 @@
 // 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 <cmath>
+
+#include <gtest/gtest.h>
+
 #include "frc/kinematics/ChassisSpeeds.h"
-#include "gtest/gtest.h"
 
 static constexpr double kEpsilon = 1E-9;
 
-TEST(ChassisSpeedsTest, FieldRelativeConstruction) {
+TEST(ChassisSpeedsTest, Discretize) {
+  constexpr frc::ChassisSpeeds target{1_mps, 0_mps, 0.5_rad_per_s};
+  constexpr units::second_t duration = 1_s;
+  constexpr units::second_t dt = 10_ms;
+
+  const auto speeds = frc::ChassisSpeeds::Discretize(target, duration);
+  const frc::Twist2d twist{speeds.vx * dt, speeds.vy * dt, speeds.omega * dt};
+
+  frc::Pose2d pose;
+  for (units::second_t time = 0_s; time < duration; time += dt) {
+    pose = pose.Exp(twist);
+  }
+
+  EXPECT_NEAR((target.vx * duration).value(), pose.X().value(), kEpsilon);
+  EXPECT_NEAR((target.vy * duration).value(), pose.Y().value(), kEpsilon);
+  EXPECT_NEAR((target.omega * duration).value(),
+              pose.Rotation().Radians().value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, FromFieldRelativeSpeeds) {
   const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
       1.0_mps, 0.0_mps, 0.5_rad_per_s, -90.0_deg);
 
@@ -15,3 +37,64 @@
   EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon);
   EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
 }
+
+TEST(ChassisSpeedsTest, FromRobotRelativeSpeeds) {
+  const auto chassisSpeeds = frc::ChassisSpeeds::FromRobotRelativeSpeeds(
+      1.0_mps, 0.0_mps, 0.5_rad_per_s, 45.0_deg);
+
+  EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vx.value(), kEpsilon);
+  EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vy.value(), kEpsilon);
+  EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, Plus) {
+  const frc::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
+  const frc::ChassisSpeeds right{2.0_mps, 1.5_mps, 0.25_rad_per_s};
+
+  const frc::ChassisSpeeds result = left + right;
+
+  EXPECT_NEAR(3.0, result.vx.value(), kEpsilon);
+  EXPECT_NEAR(2.0, result.vy.value(), kEpsilon);
+  EXPECT_NEAR(1.0, result.omega.value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, Minus) {
+  const frc::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
+  const frc::ChassisSpeeds right{2.0_mps, 0.5_mps, 0.25_rad_per_s};
+
+  const frc::ChassisSpeeds result = left - right;
+
+  EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
+  EXPECT_NEAR(0, result.vy.value(), kEpsilon);
+  EXPECT_NEAR(0.5, result.omega.value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, UnaryMinus) {
+  const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
+
+  const frc::ChassisSpeeds result = -speeds;
+
+  EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
+  EXPECT_NEAR(-0.5, result.vy.value(), kEpsilon);
+  EXPECT_NEAR(-0.75, result.omega.value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, Multiplication) {
+  const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
+
+  const frc::ChassisSpeeds result = speeds * 2;
+
+  EXPECT_NEAR(2.0, result.vx.value(), kEpsilon);
+  EXPECT_NEAR(1.0, result.vy.value(), kEpsilon);
+  EXPECT_NEAR(1.5, result.omega.value(), kEpsilon);
+}
+
+TEST(ChassisSpeedsTest, Division) {
+  const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
+
+  const frc::ChassisSpeeds result = speeds / 2;
+
+  EXPECT_NEAR(0.5, result.vx.value(), kEpsilon);
+  EXPECT_NEAR(0.25, result.vy.value(), kEpsilon);
+  EXPECT_NEAR(0.375, result.omega.value(), kEpsilon);
+}
diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
index 4af5ac8..782fed0 100644
--- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
@@ -4,9 +4,10 @@
 
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/kinematics/ChassisSpeeds.h"
 #include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "gtest/gtest.h"
 #include "units/angular_velocity.h"
 #include "units/length.h"
 #include "units/velocity.h"
diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
index e480941..a228357 100644
--- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
@@ -4,9 +4,10 @@
 
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/kinematics/DifferentialDriveOdometry.h"
-#include "gtest/gtest.h"
 
 static constexpr double kEpsilon = 1E-9;
 
diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelSpeedsTest.cpp
new file mode 100644
index 0000000..92cc583
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelSpeedsTest.cpp
@@ -0,0 +1,54 @@
+// 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 "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+
+TEST(DifferentialDriveWheelSpeedsTest, Plus) {
+  const frc::DifferentialDriveWheelSpeeds left{1.0_mps, 0.5_mps};
+  const frc::DifferentialDriveWheelSpeeds right{2.0_mps, 1.5_mps};
+
+  const frc::DifferentialDriveWheelSpeeds result = left + right;
+
+  EXPECT_EQ(3.0, result.left.value());
+  EXPECT_EQ(2.0, result.right.value());
+}
+
+TEST(DifferentialDriveWheelSpeedsTest, Minus) {
+  const frc::DifferentialDriveWheelSpeeds left{1.0_mps, 0.5_mps};
+  const frc::DifferentialDriveWheelSpeeds right{2.0_mps, 0.5_mps};
+
+  const frc::DifferentialDriveWheelSpeeds result = left - right;
+
+  EXPECT_EQ(-1.0, result.left.value());
+  EXPECT_EQ(0, result.right.value());
+}
+
+TEST(DifferentialDriveWheelSpeedsTest, UnaryMinus) {
+  const frc::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
+
+  const frc::DifferentialDriveWheelSpeeds result = -speeds;
+
+  EXPECT_EQ(-1.0, result.left.value());
+  EXPECT_EQ(-0.5, result.right.value());
+}
+
+TEST(DifferentialDriveWheelSpeedsTest, Multiplication) {
+  const frc::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
+
+  const frc::DifferentialDriveWheelSpeeds result = speeds * 2;
+
+  EXPECT_EQ(2.0, result.left.value());
+  EXPECT_EQ(1.0, result.right.value());
+}
+
+TEST(DifferentialDriveWheelSpeedsTest, Division) {
+  const frc::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
+
+  const frc::DifferentialDriveWheelSpeeds result = speeds / 2;
+
+  EXPECT_EQ(0.5, result.left.value());
+  EXPECT_EQ(0.25, result.right.value());
+}
diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
index 364163e..04a3f1f 100644
--- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
@@ -4,9 +4,10 @@
 
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Translation2d.h"
 #include "frc/kinematics/MecanumDriveKinematics.h"
-#include "gtest/gtest.h"
 #include "units/angular_velocity.h"
 
 using namespace frc;
diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
index bfaf91b..be20655 100644
--- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
@@ -5,9 +5,10 @@
 #include <limits>
 #include <random>
 
+#include <gtest/gtest.h>
+
 #include "frc/kinematics/MecanumDriveOdometry.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelSpeedsTest.cpp
new file mode 100644
index 0000000..c10cfb5
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelSpeedsTest.cpp
@@ -0,0 +1,64 @@
+// 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 "frc/kinematics/MecanumDriveWheelSpeeds.h"
+
+TEST(MecanumDriveWheelSpeedsTest, Plus) {
+  const frc::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+  const frc::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps, 1.0_mps};
+
+  const frc::MecanumDriveWheelSpeeds result = left + right;
+
+  EXPECT_EQ(3.0, result.frontLeft.value());
+  EXPECT_EQ(2.0, result.frontRight.value());
+  EXPECT_EQ(2.5, result.rearLeft.value());
+  EXPECT_EQ(2.5, result.rearRight.value());
+}
+
+TEST(MecanumDriveWheelSpeedsTest, Minus) {
+  const frc::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+  const frc::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps, 1.0_mps};
+
+  const frc::MecanumDriveWheelSpeeds result = left - right;
+
+  EXPECT_EQ(-1.0, result.frontLeft.value());
+  EXPECT_EQ(-1.0, result.frontRight.value());
+  EXPECT_EQ(1.5, result.rearLeft.value());
+  EXPECT_EQ(0.5, result.rearRight.value());
+}
+
+TEST(MecanumDriveWheelSpeedsTest, UnaryMinus) {
+  const frc::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+
+  const frc::MecanumDriveWheelSpeeds result = -speeds;
+
+  EXPECT_EQ(-1.0, result.frontLeft.value());
+  EXPECT_EQ(-0.5, result.frontRight.value());
+  EXPECT_EQ(-2.0, result.rearLeft.value());
+  EXPECT_EQ(-1.5, result.rearRight.value());
+}
+
+TEST(MecanumDriveWheelSpeedsTest, Multiplication) {
+  const frc::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+
+  const frc::MecanumDriveWheelSpeeds result = speeds * 2;
+
+  EXPECT_EQ(2.0, result.frontLeft.value());
+  EXPECT_EQ(1.0, result.frontRight.value());
+  EXPECT_EQ(4.0, result.rearLeft.value());
+  EXPECT_EQ(3.0, result.rearRight.value());
+}
+
+TEST(MecanumDriveWheelSpeedsTest, Division) {
+  const frc::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps, 1.5_mps};
+
+  const frc::MecanumDriveWheelSpeeds result = speeds / 2;
+
+  EXPECT_EQ(0.5, result.frontLeft.value());
+  EXPECT_EQ(0.25, result.frontRight.value());
+  EXPECT_EQ(1.0, result.rearLeft.value());
+  EXPECT_EQ(0.75, result.rearRight.value());
+}
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
index 8e0dc8f..e7726e6 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
@@ -4,9 +4,10 @@
 
 #include <numbers>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Translation2d.h"
 #include "frc/kinematics/SwerveDriveKinematics.h"
-#include "gtest/gtest.h"
 #include "units/angular_velocity.h"
 
 using namespace frc;
@@ -125,6 +126,25 @@
   EXPECT_NEAR(bl.angle.Degrees().value(), -135.0, kEpsilon);
   EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
 }
+TEST_F(SwerveDriveKinematicsTest, ResetWheelAngle) {
+  Rotation2d fl = {0_deg};
+  Rotation2d fr = {90_deg};
+  Rotation2d bl = {180_deg};
+  Rotation2d br = {270_deg};
+  m_kinematics.ResetHeadings(fl, fr, bl, br);
+  auto [flMod, frMod, blMod, brMod] =
+      m_kinematics.ToSwerveModuleStates(ChassisSpeeds{});
+
+  EXPECT_NEAR(flMod.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(frMod.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(blMod.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(brMod.speed.value(), 0.0, kEpsilon);
+
+  EXPECT_NEAR(flMod.angle.Degrees().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(frMod.angle.Degrees().value(), 90.0, kEpsilon);
+  EXPECT_NEAR(blMod.angle.Degrees().value(), 180.0, kEpsilon);
+  EXPECT_NEAR(brMod.angle.Degrees().value(), 270.0, kEpsilon);
+}
 
 TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
   SwerveModuleState fl{106.629_mps, 135_deg};
@@ -274,3 +294,18 @@
   EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
   EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
 }
+
+TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeSpeed) {
+  SwerveModuleState state1{1.0_mps, 0_deg};
+  SwerveModuleState state2{1.0_mps, 0_deg};
+  SwerveModuleState state3{-2.0_mps, 0_deg};
+  SwerveModuleState state4{-2.0_mps, 0_deg};
+
+  wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
+  SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 1.0_mps);
+
+  EXPECT_NEAR(arr[0].speed.value(), 0.5, kEpsilon);
+  EXPECT_NEAR(arr[1].speed.value(), 0.5, kEpsilon);
+  EXPECT_NEAR(arr[2].speed.value(), -1.0, kEpsilon);
+  EXPECT_NEAR(arr[3].speed.value(), -1.0, kEpsilon);
+}
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
index 8c67ab5..265b0cc 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
@@ -5,12 +5,13 @@
 #include <limits>
 #include <random>
 
+#include <gtest/gtest.h>
+
 #include "frc/kinematics/SwerveDriveKinematics.h"
 #include "frc/kinematics/SwerveDriveOdometry.h"
 #include "frc/trajectory/Trajectory.h"
 #include "frc/trajectory/TrajectoryConfig.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 
 using namespace frc;
 
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModulePositionTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModulePositionTest.cpp
new file mode 100644
index 0000000..14155f1
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveModulePositionTest.cpp
@@ -0,0 +1,24 @@
+// 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 "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/SwerveModulePosition.h"
+
+TEST(SwerveModulePositionTest, Equality) {
+  frc::SwerveModulePosition position1{2_m, 90_deg};
+  frc::SwerveModulePosition position2{2_m, 90_deg};
+
+  EXPECT_EQ(position1, position2);
+}
+
+TEST(SwerveModulePositionTest, Inequality) {
+  frc::SwerveModulePosition position1{1_m, 90_deg};
+  frc::SwerveModulePosition position2{2_m, 90_deg};
+  frc::SwerveModulePosition position3{1_m, 89_deg};
+
+  EXPECT_NE(position1, position2);
+  EXPECT_NE(position1, position3);
+}
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
index 4880bef..efc3975 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
@@ -2,9 +2,10 @@
 // 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 "frc/geometry/Rotation2d.h"
 #include "frc/kinematics/SwerveModuleState.h"
-#include "gtest/gtest.h"
 
 static constexpr double kEpsilon = 1E-9;
 
@@ -39,3 +40,19 @@
   EXPECT_NEAR(optimizedB.speed.value(), -2.0, kEpsilon);
   EXPECT_NEAR(optimizedB.angle.Degrees().value(), -2.0, kEpsilon);
 }
+
+TEST(SwerveModuleStateTest, Equality) {
+  frc::SwerveModuleState state1{2_mps, 90_deg};
+  frc::SwerveModuleState state2{2_mps, 90_deg};
+
+  EXPECT_EQ(state1, state2);
+}
+
+TEST(SwerveModuleStateTest, Inequality) {
+  frc::SwerveModuleState state1{1_mps, 90_deg};
+  frc::SwerveModuleState state2{2_mps, 90_deg};
+  frc::SwerveModuleState state3{1_mps, 89_deg};
+
+  EXPECT_NE(state1, state2);
+  EXPECT_NE(state1, state3);
+}
diff --git a/wpimath/src/test/native/cpp/main.cpp b/wpimath/src/test/native/cpp/main.cpp
index 09072ee..e993c1f 100644
--- a/wpimath/src/test/native/cpp/main.cpp
+++ b/wpimath/src/test/native/cpp/main.cpp
@@ -2,7 +2,7 @@
 // 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 <gtest/gtest.h>
 
 int main(int argc, char** argv) {
   ::testing::InitGoogleTest(&argc, argv);
diff --git a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
index 7422a7f..c98d190 100644
--- a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
+++ b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
@@ -5,12 +5,13 @@
 #include <chrono>
 #include <vector>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
 #include "frc/spline/QuinticHermiteSpline.h"
 #include "frc/spline/SplineHelper.h"
 #include "frc/spline/SplineParameterizer.h"
-#include "gtest/gtest.h"
 #include "units/length.h"
 
 using namespace frc;
diff --git a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
index e45df7b..a0df9bb 100644
--- a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
+++ b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
@@ -4,12 +4,13 @@
 
 #include <chrono>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
 #include "frc/spline/QuinticHermiteSpline.h"
 #include "frc/spline/SplineHelper.h"
 #include "frc/spline/SplineParameterizer.h"
-#include "gtest/gtest.h"
 #include "units/angle.h"
 #include "units/length.h"
 
diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
index d735338..c42fcf0 100644
--- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
@@ -2,11 +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.
 
-#include <gtest/gtest.h>
-
 #include <functional>
 
-#include "Eigen/Eigenvalues"
+#include <Eigen/Eigenvalues>
+#include <gtest/gtest.h>
+
 #include "frc/EigenCore.h"
 #include "frc/system/Discretization.h"
 #include "frc/system/NumericalIntegration.h"
@@ -114,98 +114,6 @@
       << discQIntegrated;
 }
 
-// Test that the Taylor series discretization produces nearly identical results.
-TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
-  frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
-  frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}};
-
-  constexpr auto dt = 1_s;
-
-  frc::Matrixd<2, 2> discQTaylor;
-  frc::Matrixd<2, 2> discA;
-  frc::Matrixd<2, 2> discATaylor;
-
-  // Continuous Q should be positive semidefinite
-  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont{contQ};
-  for (int i = 0; i < contQ.rows(); ++i) {
-    EXPECT_GE(esCont.eigenvalues()[i], 0);
-  }
-
-  //       T
-  // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-  //       0
-  frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
-      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());
-      },
-      0_s, frc::Matrixd<2, 2>::Zero(), dt);
-
-  frc::DiscretizeA<2>(contA, dt, &discA);
-  frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
-
-  EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-10)
-      << "Expected these to be nearly equal:\ndiscQTaylor:\n"
-      << discQTaylor << "\ndiscQIntegrated:\n"
-      << discQIntegrated;
-  EXPECT_LT((discA - discATaylor).norm(), 1e-10);
-
-  // Discrete Q should be positive semidefinite
-  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc{discQTaylor};
-  for (int i = 0; i < discQTaylor.rows(); ++i) {
-    EXPECT_GE(esDisc.eigenvalues()[i], 0);
-  }
-}
-
-// Test that the Taylor series discretization produces nearly identical results.
-TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
-  frc::Matrixd<2, 2> contA{{0, 1}, {0, -1500}};
-  frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}};
-
-  constexpr auto dt = 5_ms;
-
-  frc::Matrixd<2, 2> discQTaylor;
-  frc::Matrixd<2, 2> discA;
-  frc::Matrixd<2, 2> discATaylor;
-
-  // Continuous Q should be positive semidefinite
-  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
-  for (int i = 0; i < contQ.rows(); ++i) {
-    EXPECT_GE(esCont.eigenvalues()[i], 0);
-  }
-
-  //       T
-  // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-  //       0
-  frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
-      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());
-      },
-      0_s, frc::Matrixd<2, 2>::Zero(), dt);
-
-  frc::DiscretizeA<2>(contA, dt, &discA);
-  frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
-
-  EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-3)
-      << "Expected these to be nearly equal:\ndiscQTaylor:\n"
-      << discQTaylor << "\ndiscQIntegrated:\n"
-      << discQIntegrated;
-  EXPECT_LT((discA - discATaylor).norm(), 1e-10);
-
-  // Discrete Q should be positive semidefinite
-  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
-  for (int i = 0; i < discQTaylor.rows(); ++i) {
-    EXPECT_GE(esDisc.eigenvalues()[i], 0);
-  }
-}
-
 // Test that DiscretizeR() works
 TEST(DiscretizationTest, DiscretizeR) {
   frc::Matrixd<2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};
diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
index dbc4284..4dc6263 100644
--- a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
+++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
@@ -5,6 +5,7 @@
 #include <frc/system/LinearSystem.h>
 #include <frc/system/plant/DCMotor.h>
 #include <frc/system/plant/LinearSystemId.h>
+
 #include <gtest/gtest.h>
 
 #include "frc/system/plant/LinearSystemId.h"
diff --git a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
index b1793ad..1c73195 100644
--- a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
@@ -2,10 +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.
 
-#include <gtest/gtest.h>
-
 #include <cmath>
 
+#include <gtest/gtest.h>
+
+#include "frc/EigenCore.h"
 #include "frc/system/NumericalIntegration.h"
 
 // Tests that integrating dx/dt = e^x works.
@@ -30,6 +31,16 @@
   EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
 }
 
+// Tests that integrating dx/dt = 0 works with RKDP
+TEST(NumericalIntegrationTest, ZeroRKDP) {
+  frc::Vectord<1> y1 = frc::RKDP(
+      [](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^x works with RKDP
 TEST(NumericalIntegrationTest, ExponentialRKDP) {
   frc::Vectord<1> y0{0.0};
diff --git a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
index 70f1938..e502e95 100644
--- a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
+++ b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
@@ -2,10 +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.
 
-#include <gtest/gtest.h>
-
 #include <cmath>
 
+#include <gtest/gtest.h>
+
+#include "frc/EigenCore.h"
 #include "frc/system/RungeKuttaTimeVarying.h"
 
 namespace {
diff --git a/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
index e2f7112..fafcbd4 100644
--- a/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
@@ -5,9 +5,10 @@
 #include <memory>
 #include <vector>
 
+#include <gtest/gtest.h>
+
 #include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
-#include "gtest/gtest.h"
 #include "trajectory/TestTrajectory.h"
 #include "units/acceleration.h"
 #include "units/angle.h"
diff --git a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
index e3723b5..09abc76 100644
--- a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
@@ -5,9 +5,10 @@
 #include <memory>
 #include <vector>
 
+#include <gtest/gtest.h>
+
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
-#include "gtest/gtest.h"
 #include "trajectory/TestTrajectory.h"
 #include "units/time.h"
 
diff --git a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
index e3d6b7f..5282638 100644
--- a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
@@ -5,11 +5,12 @@
 #include <memory>
 #include <vector>
 
+#include <gtest/gtest.h>
+
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
 #include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
-#include "gtest/gtest.h"
 #include "trajectory/TestTrajectory.h"
 #include "units/acceleration.h"
 #include "units/length.h"
diff --git a/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
index 8d9e221..c39bb15 100644
--- a/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
@@ -4,10 +4,11 @@
 
 #include <vector>
 
+#include <gtest/gtest.h>
+
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/trajectory/constraint/EllipticalRegionConstraint.h"
 #include "frc/trajectory/constraint/MaxVelocityConstraint.h"
-#include "gtest/gtest.h"
 #include "trajectory/TestTrajectory.h"
 #include "units/acceleration.h"
 #include "units/angle.h"
diff --git a/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp
new file mode 100644
index 0000000..8df3aa1
--- /dev/null
+++ b/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp
@@ -0,0 +1,338 @@
+// 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 "frc/trajectory/ExponentialProfile.h"  // NOLINT(build/include_order)
+
+#include <chrono>
+#include <cmath>
+#include <tuple>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "frc/controller/SimpleMotorFeedforward.h"
+#include "units/acceleration.h"
+#include "units/frequency.h"
+#include "units/length.h"
+#include "units/math.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+
+static constexpr auto kDt = 10_ms;
+static constexpr auto kV = 2.5629_V / 1_mps;
+static constexpr auto kA = 0.43277_V / 1_mps_sq;
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+#define EXPECT_LT_OR_NEAR_UNITS(val1, val2, eps) \
+  if (val1 <= val2) {                            \
+    EXPECT_LE(val1, val2);                       \
+  } else {                                       \
+    EXPECT_NEAR_UNITS(val1, val2, eps);          \
+  }
+
+frc::ExponentialProfile<units::meter, units::volts>::State CheckDynamics(
+    frc::ExponentialProfile<units::meter, units::volts> profile,
+    frc::ExponentialProfile<units::meter, units::volts>::Constraints
+        constraints,
+    frc::SimpleMotorFeedforward<units::meter> feedforward,
+    frc::ExponentialProfile<units::meter, units::volts>::State current,
+    frc::ExponentialProfile<units::meter, units::volts>::State goal) {
+  auto next = profile.Calculate(kDt, current, goal);
+  auto signal = feedforward.Calculate(current.velocity, next.velocity, kDt);
+
+  EXPECT_LE(units::math::abs(signal), constraints.maxInput + 1e-9_V);
+
+  return next;
+}
+
+TEST(ExponentialProfileTest, ReachesGoal) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{10_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+  for (int i = 0; i < 450; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+// Tests that decreasing the maximum velocity in the middle when it is already
+// moving faster than the new max is handled correctly
+TEST(ExponentialProfileTest, PosContinousUnderVelChange) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{10_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+  for (int i = 0; i < 300; ++i) {
+    if (i == 150) {
+      constraints.maxInput = 9_V;
+      profile =
+          frc::ExponentialProfile<units::meter, units::volts>{constraints};
+    }
+
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+// Tests that decreasing the maximum velocity in the middle when it is already
+// moving faster than the new max is handled correctly
+TEST(ExponentialProfileTest, PosContinousUnderVelChangeBackward) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+  for (int i = 0; i < 300; ++i) {
+    if (i == 150) {
+      constraints.maxInput = 9_V;
+      profile =
+          frc::ExponentialProfile<units::meter, units::volts>{constraints};
+    }
+
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+// There is some somewhat tricky code for dealing with going backwards
+TEST(ExponentialProfileTest, Backwards) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state;
+
+  for (int i = 0; i < 400; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+TEST(ExponentialProfileTest, SwitchGoalInMiddle) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+  for (int i = 0; i < 50; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+  }
+  EXPECT_NE(state, goal);
+
+  goal = {0.0_m, 0.0_mps};
+  for (int i = 0; i < 100; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+// Checks to make sure that it hits top speed on long trajectories
+TEST(ExponentialProfileTest, TopSpeed) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{40_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state;
+
+  units::meters_per_second_t maxSpeed = 0_mps;
+
+  for (int i = 0; i < 900; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+    maxSpeed = units::math::max(state.velocity, maxSpeed);
+  }
+
+  EXPECT_NEAR_UNITS(constraints.MaxVelocity(), maxSpeed, 1e-5_mps);
+  EXPECT_EQ(state, goal);
+}
+
+// Checks to make sure that it hits top speed on long trajectories
+TEST(ExponentialProfileTest, TopSpeedBackward) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{-40_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state;
+
+  units::meters_per_second_t maxSpeed = 0_mps;
+
+  for (int i = 0; i < 900; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+    maxSpeed = units::math::min(state.velocity, maxSpeed);
+  }
+
+  EXPECT_NEAR_UNITS(-constraints.MaxVelocity(), maxSpeed, 1e-5_mps);
+  EXPECT_EQ(state, goal);
+}
+
+// Checks to make sure that it hits top speed on long trajectories
+TEST(ExponentialProfileTest, HighInitialSpeed) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{40_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 8_mps};
+
+  for (int i = 0; i < 900; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+  }
+
+  EXPECT_EQ(state, goal);
+}
+
+// Checks to make sure that it hits top speed on long trajectories
+TEST(ExponentialProfileTest, HighInitialSpeedBackward) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{-40_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, -8_mps};
+
+  for (int i = 0; i < 900; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+  }
+
+  EXPECT_EQ(state, goal);
+}
+
+TEST(ExponentialProfileTest, TestHeuristic) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  std::vector<std::tuple<
+      frc::ExponentialProfile<units::meter, units::volts>::State,  // initial
+      frc::ExponentialProfile<units::meter, units::volts>::State,  // goal
+      frc::ExponentialProfile<units::meter, units::volts>::State>  // inflection
+                                                                   // point
+              >
+      testCases{
+          // red > green and purple => always positive => false
+          {{0_m, -4_mps}, {0.75_m, -4_mps}, {1.3758_m, 4.4304_mps}},
+          {{0_m, -4_mps}, {1.4103_m, 4_mps}, {1.3758_m, 4.4304_mps}},
+          {{0.6603_m, 4_mps}, {0.75_m, -4_mps}, {1.3758_m, 4.4304_mps}},
+          {{0.6603_m, 4_mps}, {1.4103_m, 4_mps}, {1.3758_m, 4.4304_mps}},
+
+          // purple > red > green => positive if v0 < 0 => c && !d && a
+          {{0_m, -4_mps}, {0.5_m, -2_mps}, {0.4367_m, 3.7217_mps}},
+          {{0_m, -4_mps}, {0.546_m, 2_mps}, {0.4367_m, 3.7217_mps}},
+          {{0.6603_m, 4_mps}, {0.5_m, -2_mps}, {0.5560_m, -2.9616_mps}},
+          {{0.6603_m, 4_mps}, {0.546_m, 2_mps}, {0.5560_m, -2.9616_mps}},
+
+          // red < green and purple => always negative => true => !c && !d
+          {{0_m, -4_mps}, {-0.75_m, -4_mps}, {-0.7156_m, -4.4304_mps}},
+          {{0_m, -4_mps}, {-0.0897_m, 4_mps}, {-0.7156_m, -4.4304_mps}},
+          {{0.6603_m, 4_mps}, {-0.75_m, -4_mps}, {-0.7156_m, -4.4304_mps}},
+          {{0.6603_m, 4_mps}, {-0.0897_m, 4_mps}, {-0.7156_m, -4.4304_mps}},
+
+          // green > red > purple => positive if vf < 0 => !c && d && b
+          {{0_m, -4_mps}, {-0.5_m, -4.5_mps}, {1.095_m, 4.314_mps}},
+          {{0_m, -4_mps}, {1.0795_m, 4.5_mps}, {-0.5122_m, -4.351_mps}},
+          {{0.6603_m, 4_mps}, {-0.5_m, -4.5_mps}, {1.095_m, 4.314_mps}},
+          {{0.6603_m, 4_mps}, {1.0795_m, 4.5_mps}, {-0.5122_m, -4.351_mps}},
+
+          // tests for initial velocity > V/kV
+          {{0_m, -8_mps}, {0_m, 0_mps}, {-0.1384_m, 3.342_mps}},
+          {{0_m, -8_mps}, {-1_m, 0_mps}, {-0.562_m, -6.792_mps}},
+          {{0_m, 8_mps}, {1_m, 0_mps}, {0.562_m, 6.792_mps}},
+          {{0_m, 8_mps}, {-1_m, 0_mps}, {-0.785_m, -4.346_mps}},
+      };
+
+  for (auto& testCase : testCases) {
+    auto state = profile.CalculateInflectionPoint(std::get<0>(testCase),
+                                                  std::get<1>(testCase));
+    EXPECT_NEAR_UNITS(std::get<2>(testCase).position / 1_m,
+                      state.position / 1_m, 1e-3);
+    EXPECT_NEAR_UNITS(std::get<2>(testCase).velocity / 1_mps,
+                      state.velocity / 1_mps, 1e-3);
+  }
+}
+
+TEST(ExponentialProfileTest, TimingToCurrent) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{2_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+  for (int i = 0; i < 900; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+    EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state, state), 0_s, 2e-2_s);
+  }
+
+  EXPECT_EQ(state, goal);
+}
+
+TEST(ExponentialProfileTest, TimingToGoal) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{2_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+  auto prediction = profile.TimeLeftUntil(state, goal);
+  auto reachedGoal = false;
+
+  for (int i = 0; i < 900; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+    if (!reachedGoal && state == goal) {
+      EXPECT_NEAR_UNITS(prediction, i * 0.01_s, 0.25_s);
+      reachedGoal = true;
+    }
+  }
+
+  EXPECT_EQ(state, goal);
+}
+
+TEST(ExponentialProfileTest, TimingToNegativeGoal) {
+  frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
+      12_V, -kV / kA, 1 / kA};
+  frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
+  frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
+                                                        0.43277_V / 1_mps_sq};
+  frc::ExponentialProfile<units::meter, units::volts>::State goal{-2_m, 0_mps};
+  frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
+
+  auto prediction = profile.TimeLeftUntil(state, goal);
+  auto reachedGoal = false;
+
+  for (int i = 0; i < 900; ++i) {
+    state = CheckDynamics(profile, constraints, feedforward, state, goal);
+    if (!reachedGoal && state == goal) {
+      EXPECT_NEAR_UNITS(prediction, i * 0.01_s, 0.25_s);
+      reachedGoal = true;
+    }
+  }
+
+  EXPECT_EQ(state, goal);
+}
diff --git a/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
index 0bf6b1a..8ec3a70 100644
--- a/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
@@ -4,10 +4,11 @@
 
 #include <vector>
 
+#include <gtest/gtest.h>
+
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/trajectory/constraint/MaxVelocityConstraint.h"
 #include "frc/trajectory/constraint/RectangularRegionConstraint.h"
-#include "gtest/gtest.h"
 #include "trajectory/TestTrajectory.h"
 #include "units/acceleration.h"
 #include "units/length.h"
diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp
index 2b733a8..2bfd541 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp
@@ -2,9 +2,10 @@
 // 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 "frc/trajectory/TrajectoryConfig.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 
 TEST(TrajectoryConcatenateTest, States) {
   auto t1 = frc::TrajectoryGenerator::GenerateTrajectory(
diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
index 5892461..8b50160 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
@@ -4,11 +4,12 @@
 
 #include <vector>
 
+#include <gtest/gtest.h>
+
 #include "frc/trajectory/Trajectory.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
 #include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
-#include "gtest/gtest.h"
 #include "trajectory/TestTrajectory.h"
 #include "units/math.h"
 
diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
index 90c6dc0..9411696 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
@@ -2,9 +2,10 @@
 // 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 "frc/trajectory/TrajectoryConfig.h"
 #include "frc/trajectory/TrajectoryUtil.h"
-#include "gtest/gtest.h"
 #include "trajectory/TestTrajectory.h"
 
 using namespace frc;
diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
index 5c77a5b..f088fa3 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
@@ -4,10 +4,11 @@
 
 #include <vector>
 
+#include <gtest/gtest.h>
+
 #include "frc/trajectory/Trajectory.h"
 #include "frc/trajectory/TrajectoryConfig.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
 
 void TestSameShapedTrajectory(std::vector<frc::Trajectory::State> statesA,
                               std::vector<frc::Trajectory::State> statesB) {
diff --git a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
index 6a35261..b2e5b18 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
@@ -7,7 +7,8 @@
 #include <chrono>
 #include <cmath>
 
-#include "gtest/gtest.h"
+#include <gtest/gtest.h>
+
 #include "units/acceleration.h"
 #include "units/length.h"
 #include "units/math.h"
@@ -31,9 +32,9 @@
   frc::TrapezoidProfile<units::meter>::State goal{3_m, 0_mps};
   frc::TrapezoidProfile<units::meter>::State state;
 
+  frc::TrapezoidProfile<units::meter> profile{constraints};
   for (int i = 0; i < 450; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
   }
   EXPECT_EQ(state, goal);
 }
@@ -45,17 +46,18 @@
                                                                0.75_mps_sq};
   frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};
 
-  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
-  auto state = profile.Calculate(kDt);
+  frc::TrapezoidProfile<units::meter> profile{constraints};
+  auto state = profile.Calculate(kDt, goal,
+                                 frc::TrapezoidProfile<units::meter>::State{});
 
   auto lastPos = state.position;
   for (int i = 0; i < 1600; ++i) {
     if (i == 400) {
       constraints.maxVelocity = 0.75_mps;
+      profile = frc::TrapezoidProfile<units::meter>{constraints};
     }
 
-    profile = frc::TrapezoidProfile<units::meter>{constraints, goal, state};
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
     auto estimatedVel = (state.position - lastPos) / kDt;
 
     if (i >= 400) {
@@ -79,9 +81,9 @@
   frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
   frc::TrapezoidProfile<units::meter>::State state;
 
+  frc::TrapezoidProfile<units::meter> profile{constraints};
   for (int i = 0; i < 400; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
   }
   EXPECT_EQ(state, goal);
 }
@@ -92,16 +94,16 @@
   frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
   frc::TrapezoidProfile<units::meter>::State state;
 
+  frc::TrapezoidProfile<units::meter> profile{constraints};
   for (int i = 0; i < 200; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
   }
   EXPECT_NE(state, goal);
 
   goal = {0.0_m, 0.0_mps};
+  profile = frc::TrapezoidProfile<units::meter>{constraints};
   for (int i = 0; i < 550; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
   }
   EXPECT_EQ(state, goal);
 }
@@ -113,15 +115,15 @@
   frc::TrapezoidProfile<units::meter>::State goal{4_m, 0_mps};
   frc::TrapezoidProfile<units::meter>::State state;
 
+  frc::TrapezoidProfile<units::meter> profile{constraints};
   for (int i = 0; i < 200; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
   }
   EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
 
+  profile = frc::TrapezoidProfile<units::meter>{constraints};
   for (int i = 0; i < 2000; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
   }
   EXPECT_EQ(state, goal);
 }
@@ -132,9 +134,9 @@
   frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
   frc::TrapezoidProfile<units::meter>::State state;
 
+  frc::TrapezoidProfile<units::meter> profile{constraints};
   for (int i = 0; i < 400; i++) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
     EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
   }
 }
@@ -146,14 +148,14 @@
                                                                0.75_mps_sq};
   frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
 
-  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
-  auto state = profile.Calculate(kDt);
+  frc::TrapezoidProfile<units::meter> profile{constraints};
+  auto state = profile.Calculate(kDt, goal,
+                                 frc::TrapezoidProfile<units::meter>::State{});
 
   auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
     if (!reachedGoal && state == goal) {
       // Expected value using for loop index is just an approximation since the
       // time left in the profile doesn't increase linearly at the endpoints
@@ -170,14 +172,14 @@
                                                                0.75_mps_sq};
   frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
 
-  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
-  auto state = profile.Calculate(kDt);
+  frc::TrapezoidProfile<units::meter> profile{constraints};
+  auto state = profile.Calculate(kDt, goal,
+                                 frc::TrapezoidProfile<units::meter>::State{});
 
   auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
     if (!reachedGoal &&
         (units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
       EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
@@ -193,14 +195,14 @@
                                                                0.75_mps_sq};
   frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
 
-  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
-  auto state = profile.Calculate(kDt);
+  frc::TrapezoidProfile<units::meter> profile{constraints};
+  auto state = profile.Calculate(kDt, goal,
+                                 frc::TrapezoidProfile<units::meter>::State{});
 
   auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
     if (!reachedGoal && state == goal) {
       // Expected value using for loop index is just an approximation since the
       // time left in the profile doesn't increase linearly at the endpoints
@@ -217,14 +219,14 @@
                                                                0.75_mps_sq};
   frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
 
-  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
-  auto state = profile.Calculate(kDt);
+  frc::TrapezoidProfile<units::meter> profile{constraints};
+  auto state = profile.Calculate(kDt, goal,
+                                 frc::TrapezoidProfile<units::meter>::State{});
 
   auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
-    state = profile.Calculate(kDt);
+    state = profile.Calculate(kDt, goal, state);
     if (!reachedGoal &&
         (units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
       EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
diff --git a/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h b/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h
deleted file mode 100644
index d6bcbb8..0000000
--- a/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h
+++ /dev/null
@@ -1,116 +0,0 @@
-#pragma once
-
-#include <algorithm>
-#include <cmath>
-#include <limits>
-
-#include <Eigen/Core>
-#include <gtest/gtest.h>
-
-// #include "drake/common/text_logging.h"
-
-namespace drake {
-
-enum class MatrixCompareType { absolute, relative };
-
-/**
- * Compares two matrices to determine whether they are equal to within a certain
- * threshold.
- *
- * @param m1 The first matrix to compare.
- * @param m2 The second matrix to compare.
- * @param tolerance The tolerance for determining equivalence.
- * @param compare_type Whether the tolereance is absolute or relative.
- * @param explanation A pointer to a string variable for saving an explanation
- * of why @p m1 and @p m2 are unequal. This parameter is optional and defaults
- * to `nullptr`. If this is `nullptr` and @p m1 and @p m2 are not equal, an
- * explanation is logged as an error message.
- * @return true if the two matrices are equal based on the specified tolerance.
- */
-template <typename DerivedA, typename DerivedB>
-[[nodiscard]] ::testing::AssertionResult CompareMatrices(
-    const Eigen::MatrixBase<DerivedA>& m1,
-    const Eigen::MatrixBase<DerivedB>& m2, double tolerance = 0.0,
-    MatrixCompareType compare_type = MatrixCompareType::absolute) {
-  if (m1.rows() != m2.rows() || m1.cols() != m2.cols()) {
-    return ::testing::AssertionFailure()
-           << "Matrix size mismatch: (" << m1.rows() << " x " << m1.cols()
-           << " vs. " << m2.rows() << " x " << m2.cols() << ")";
-  }
-
-  for (int ii = 0; ii < m1.rows(); ii++) {
-    for (int jj = 0; jj < m1.cols(); jj++) {
-      // First handle the corner cases of positive infinity, negative infinity,
-      // and NaN
-      const auto both_positive_infinity =
-          m1(ii, jj) == std::numeric_limits<double>::infinity() &&
-          m2(ii, jj) == std::numeric_limits<double>::infinity();
-
-      const auto both_negative_infinity =
-          m1(ii, jj) == -std::numeric_limits<double>::infinity() &&
-          m2(ii, jj) == -std::numeric_limits<double>::infinity();
-
-      using std::isnan;
-      const auto both_nan = isnan(m1(ii, jj)) && isnan(m2(ii, jj));
-
-      if (both_positive_infinity || both_negative_infinity || both_nan)
-        continue;
-
-      // Check for case where one value is NaN and the other is not
-      if ((isnan(m1(ii, jj)) && !isnan(m2(ii, jj))) ||
-          (!isnan(m1(ii, jj)) && isnan(m2(ii, jj)))) {
-        return ::testing::AssertionFailure() << "NaN mismatch at (" << ii
-                                             << ", " << jj << "):\nm1 =\n"
-                                             << m1 << "\nm2 =\n"
-                                             << m2;
-      }
-
-      // Determine whether the difference between the two matrices is less than
-      // the tolerance.
-      using std::abs;
-      const auto delta = abs(m1(ii, jj) - m2(ii, jj));
-
-      if (compare_type == MatrixCompareType::absolute) {
-        // Perform comparison using absolute tolerance.
-
-        if (delta > tolerance) {
-          return ::testing::AssertionFailure()
-                 << "Values at (" << ii << ", " << jj
-                 << ") exceed tolerance: " << m1(ii, jj) << " vs. "
-                 << m2(ii, jj) << ", diff = " << delta
-                 << ", tolerance = " << tolerance << "\nm1 =\n"
-                 << m1 << "\nm2 =\n"
-                 << m2 << "\ndelta=\n"
-                 << (m1 - m2);
-        }
-      } else {
-        // Perform comparison using relative tolerance, see:
-        // http://realtimecollisiondetection.net/blog/?p=89
-        using std::max;
-        const auto max_value = max(abs(m1(ii, jj)), abs(m2(ii, jj)));
-        const auto relative_tolerance =
-            tolerance * max(decltype(max_value){1}, max_value);
-
-        if (delta > relative_tolerance) {
-          return ::testing::AssertionFailure()
-                 << "Values at (" << ii << ", " << jj
-                 << ") exceed tolerance: " << m1(ii, jj) << " vs. "
-                 << m2(ii, jj) << ", diff = " << delta
-                 << ", tolerance = " << tolerance
-                 << ", relative tolerance = " << relative_tolerance
-                 << "\nm1 =\n"
-                 << m1 << "\nm2 =\n"
-                 << m2 << "\ndelta=\n"
-                 << (m1 - m2);
-        }
-      }
-    }
-  }
-
-  return ::testing::AssertionSuccess() << "m1 =\n"
-                                       << m1
-                                       << "\nis approximately equal to m2 =\n"
-                                       << m2;
-}
-
-}  // namespace drake
diff --git a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
index 5e7b165..6273532 100644
--- a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
+++ b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
@@ -6,7 +6,6 @@
 
 #include <array>
 
-#include "frc/EigenCore.h"
 #include "units/time.h"
 
 namespace frc {