diff --git a/wpimath/src/dev/native/cpp/main.cpp b/wpimath/src/dev/native/cpp/main.cpp
index 54952b7..447d3f2 100644
--- a/wpimath/src/dev/native/cpp/main.cpp
+++ b/wpimath/src/dev/native/cpp/main.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 <numbers>
+
 #include <fmt/core.h>
-#include <wpi/numbers>
 
 int main() {
-  fmt::print("{}\n", wpi::numbers::pi);
+  fmt::print("{}\n", std::numbers::pi);
 }
diff --git a/wpimath/src/generate/Nat.java.jinja b/wpimath/src/generate/Nat.java.jinja
index 31451d2..cbc0ddb 100644
--- a/wpimath/src/generate/Nat.java.jinja
+++ b/wpimath/src/generate/Nat.java.jinja
@@ -16,7 +16,6 @@
  *
  * @param <T> The {@link Num} this represents.
  */
-@SuppressWarnings({"MethodName", "unused"})
 public interface Nat<T extends Num> {
   /**
    * The number this interface represents.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java b/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java
new file mode 100644
index 0000000..7b1c377
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java
@@ -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.
+
+package edu.wpi.first.math;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Transform3d;
+
+public final class ComputerVisionUtil {
+  private ComputerVisionUtil() {
+    throw new AssertionError("utility class");
+  }
+
+  /**
+   * Returns the robot's pose in the field coordinate system given an object's field-relative pose,
+   * the transformation from the camera's pose to the object's pose (obtained via computer vision),
+   * and the transformation from the robot's pose to the camera's pose.
+   *
+   * <p>The object could be a target or a fiducial marker.
+   *
+   * @param objectInField An object's field-relative pose.
+   * @param cameraToObject The transformation from the camera's pose to the object's pose. This
+   *     comes from computer vision.
+   * @param robotToCamera The transformation from the robot's pose to the camera's pose. This can
+   *     either be a constant for a rigidly mounted camera, or variable if the camera is mounted to
+   *     a turret. If the camera was mounted 3 inches in front of the "origin" (usually physical
+   *     center) of the robot, this would be new Transform3d(Units.inchesToMeters(3.0), 0.0, 0.0,
+   *     new Rotation3d()).
+   * @return The robot's field-relative pose.
+   */
+  public static Pose3d objectToRobotPose(
+      Pose3d objectInField, Transform3d cameraToObject, Transform3d robotToCamera) {
+    final var objectToCamera = cameraToObject.inverse();
+    final var cameraToRobot = robotToCamera.inverse();
+    return objectInField.plus(objectToCamera).plus(cameraToRobot);
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/Drake.java b/wpimath/src/main/java/edu/wpi/first/math/Drake.java
index 766f7e9..55bc5b2 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Drake.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Drake.java
@@ -10,7 +10,7 @@
   private Drake() {}
 
   /**
-   * Solves the discrete alegebraic Riccati equation.
+   * Solves the discrete algebraic Riccati equation.
    *
    * @param A System matrix.
    * @param B Input matrix.
@@ -18,7 +18,6 @@
    * @param R Input cost matrix.
    * @return Solution of DARE.
    */
-  @SuppressWarnings({"LocalVariableName", "ParameterName"})
   public static SimpleMatrix discreteAlgebraicRiccatiEquation(
       SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) {
     var S = new SimpleMatrix(A.numRows(), A.numCols());
@@ -34,7 +33,7 @@
   }
 
   /**
-   * Solves the discrete alegebraic Riccati equation.
+   * Solves the discrete algebraic Riccati equation.
    *
    * @param <States> Number of states.
    * @param <Inputs> Number of inputs.
@@ -44,7 +43,6 @@
    * @param R Input cost matrix.
    * @return Solution of DARE.
    */
-  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
   public static <States extends Num, Inputs extends Num>
       Matrix<States, States> discreteAlgebraicRiccatiEquation(
           Matrix<States, States> A,
@@ -57,7 +55,7 @@
   }
 
   /**
-   * Solves the discrete alegebraic Riccati equation.
+   * Solves the discrete algebraic Riccati equation.
    *
    * @param A System matrix.
    * @param B Input matrix.
@@ -66,7 +64,6 @@
    * @param N State-input cross-term cost matrix.
    * @return Solution of DARE.
    */
-  @SuppressWarnings({"LocalVariableName", "ParameterName"})
   public static SimpleMatrix discreteAlgebraicRiccatiEquation(
       SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R, SimpleMatrix N) {
     // See
@@ -88,7 +85,7 @@
   }
 
   /**
-   * Solves the discrete alegebraic Riccati equation.
+   * Solves the discrete algebraic Riccati equation.
    *
    * @param <States> Number of states.
    * @param <Inputs> Number of inputs.
@@ -99,7 +96,6 @@
    * @param N State-input cross-term cost matrix.
    * @return Solution of DARE.
    */
-  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
   public static <States extends Num, Inputs extends Num>
       Matrix<States, States> discreteAlgebraicRiccatiEquation(
           Matrix<States, States> A,
diff --git a/wpimath/src/main/java/edu/wpi/first/math/InterpolatingMatrixTreeMap.java b/wpimath/src/main/java/edu/wpi/first/math/InterpolatingMatrixTreeMap.java
new file mode 100644
index 0000000..a7b922c
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/InterpolatingMatrixTreeMap.java
@@ -0,0 +1,91 @@
+// 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 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.
+ */
+public class InterpolatingMatrixTreeMap<K extends Number, R extends Num, C extends Num> {
+  private final TreeMap<K, Matrix<R, C>> m_map = new TreeMap<>();
+
+  /**
+   * Inserts a key-value pair.
+   *
+   * @param key The key.
+   * @param value The value.
+   */
+  public void put(K key, Matrix<R, C> 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 a linear interpolation between the
+   * keys before and after the provided one.
+   *
+   * @param key The key.
+   * @return The value associated with the given key.
+   */
+  public Matrix<R, C> get(K key) {
+    Matrix<R, C> 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);
+      }
+      Matrix<R, C> floor = m_map.get(floorKey);
+      Matrix<R, C> ceiling = m_map.get(ceilingKey);
+
+      return interpolate(floor, ceiling, inverseInterpolate(ceilingKey, key, floorKey));
+    } else {
+      return val;
+    }
+  }
+
+  /**
+   * Return the value interpolated between val1 and val2 by the interpolant d.
+   *
+   * @param val1 The lower part of the interpolation range.
+   * @param val2 The upper part of the interpolation range.
+   * @param d The interpolant in the range [0, 1].
+   * @return The interpolated value.
+   */
+  public Matrix<R, C> interpolate(Matrix<R, C> val1, Matrix<R, C> val2, double d) {
+    var dydx = val2.minus(val1);
+    return dydx.times(d).plus(val1);
+  }
+
+  /**
+   * Return where within interpolation range [0, 1] q is between down and up.
+   *
+   * @param up Upper part of interpolation range.
+   * @param q Query.
+   * @param down Lower part of interpolation range.
+   * @return Interpolant in range [0, 1].
+   */
+  public double inverseInterpolate(K up, K q, K down) {
+    double upperToLower = up.doubleValue() - down.doubleValue();
+    if (upperToLower <= 0) {
+      return 0.0;
+    }
+    double queryToLower = q.doubleValue() - down.doubleValue();
+    if (queryToLower <= 0) {
+      return 0.0;
+    }
+    return queryToLower / upperToLower;
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java b/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java
index e5b1952..2ee010f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java
@@ -24,7 +24,6 @@
    * @param data The data to fill the matrix with.
    * @return The constructed matrix.
    */
-  @SuppressWarnings("LineLength")
   public final Matrix<R, C> fill(double... data) {
     if (Objects.requireNonNull(data).length != this.m_rows.getNum() * this.m_cols.getNum()) {
       throw new IllegalArgumentException(
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 25b9f92..95ed5bf 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
@@ -35,6 +35,63 @@
 
   /**
    * Returns 0.0 if the given value is within the specified range around zero. The remaining range
+   * between the deadband and the maximum magnitude is scaled from 0.0 to the maximum magnitude.
+   *
+   * @param value Value to clip.
+   * @param deadband Range around zero.
+   * @param maxMagnitude The maximum magnitude of the input. Can be infinite.
+   * @return The value after the deadband is applied.
+   */
+  public static double applyDeadband(double value, double deadband, double maxMagnitude) {
+    if (Math.abs(value) > deadband) {
+      if (maxMagnitude / deadband > 1.0e12) {
+        // If max magnitude is sufficiently large, the implementation encounters
+        // roundoff error.  Implementing the limiting behavior directly avoids
+        // the problem.
+        return value > 0.0 ? value - deadband : value + deadband;
+      }
+      if (value > 0.0) {
+        // Map deadband to 0 and map max to max.
+        //
+        // y - y₁ = m(x - x₁)
+        // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
+        // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
+        //
+        // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max).
+        // x₁ = deadband
+        // y₁ = 0
+        // x₂ = max
+        // y₂ = max
+        //
+        // y = (max - 0)/(max - deadband) (x - deadband) + 0
+        // y = max/(max - deadband) (x - deadband)
+        // y = max (x - deadband)/(max - deadband)
+        return maxMagnitude * (value - deadband) / (maxMagnitude - deadband);
+      } else {
+        // Map -deadband to 0 and map -max to -max.
+        //
+        // y - y₁ = m(x - x₁)
+        // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
+        // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
+        //
+        // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
+        // x₁ = -deadband
+        // y₁ = 0
+        // x₂ = -max
+        // y₂ = -max
+        //
+        // y = (-max - 0)/(-max + deadband) (x + deadband) + 0
+        // y = max/(max - deadband) (x + deadband)
+        // y = max (x + deadband)/(max - deadband)
+        return maxMagnitude * (value + deadband) / (maxMagnitude - deadband);
+      }
+    } else {
+      return 0.0;
+    }
+  }
+
+  /**
+   * Returns 0.0 if the given value is within the specified range around zero. The remaining range
    * between the deadband and 1.0 is scaled from 0.0 to 1.0.
    *
    * @param value Value to clip.
@@ -42,15 +99,7 @@
    * @return The value after the deadband is applied.
    */
   public static double applyDeadband(double value, double deadband) {
-    if (Math.abs(value) > deadband) {
-      if (value > 0.0) {
-        return (value - deadband) / (1.0 - deadband);
-      } else {
-        return (value + deadband) / (1.0 - deadband);
-      }
-    } else {
-      return 0.0;
-    }
+    return applyDeadband(value, deadband, 1);
   }
 
   /**
@@ -93,7 +142,6 @@
    * @param t How far between the two values to interpolate. This is clamped to [0, 1].
    * @return The interpolated value.
    */
-  @SuppressWarnings("ParameterName")
   public static double interpolate(double startValue, double endValue, double t) {
     return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1);
   }
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 113758b..b8e7c28 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
@@ -323,16 +323,42 @@
    * <p>The matrix equation could also be written as x = A<sup>-1</sup>b. Where the pseudo inverse
    * is used if A is not square.
    *
+   * <p>Note that this method does not support solving using a QR decomposition with full-pivoting,
+   * as only column-pivoting is supported. For full-pivoting, use {@link
+   * #solveFullPivHouseholderQr}.
+   *
    * @param <C2> Columns in b.
    * @param b The right-hand side of the equation to solve.
    * @return The solution to the linear system.
    */
-  @SuppressWarnings("ParameterName")
   public final <C2 extends Num> Matrix<C, C2> solve(Matrix<R, C2> b) {
     return new Matrix<>(this.m_storage.solve(Objects.requireNonNull(b).m_storage));
   }
 
   /**
+   * Solves the least-squares problem Ax=B using a QR decomposition with full pivoting, where this
+   * matrix is A.
+   *
+   * @param <R2> Number of rows in B.
+   * @param <C2> Number of columns in B.
+   * @param other The B matrix.
+   * @return The solution matrix.
+   */
+  public final <R2 extends Num, C2 extends Num> Matrix<C, C2> solveFullPivHouseholderQr(
+      Matrix<R2, C2> other) {
+    Matrix<C, C2> solution = new Matrix<>(new SimpleMatrix(this.getNumCols(), other.getNumCols()));
+    WPIMathJNI.solveFullPivHouseholderQr(
+        this.getData(),
+        this.getNumRows(),
+        this.getNumCols(),
+        other.getData(),
+        other.getNumRows(),
+        other.getNumCols(),
+        solution.getData());
+    return solution;
+  }
+
+  /**
    * Computes the matrix exponential using Eigen's solver. This method only works for square
    * matrices, and will otherwise throw an {@link MatrixDimensionException}.
    *
@@ -437,7 +463,6 @@
    * @param b Scalar.
    * @return The element by element power of "this" and b.
    */
-  @SuppressWarnings("ParameterName")
   public final Matrix<R, C> elementPower(double b) {
     return new Matrix<>(this.m_storage.elementPower(b));
   }
@@ -450,7 +475,6 @@
    * @param b Scalar.
    * @return The element by element power of "this" and b.
    */
-  @SuppressWarnings("ParameterName")
   public final Matrix<R, C> elementPower(int b) {
     return new Matrix<>(this.m_storage.elementPower((double) b));
   }
@@ -549,10 +573,9 @@
    * Decompose "this" matrix using Cholesky Decomposition. If the "this" matrix is zeros, it will
    * return the zero matrix.
    *
-   * @param lowerTriangular Whether or not we want to decompose to the lower triangular Cholesky
-   *     matrix.
+   * @param lowerTriangular Whether we want to decompose to the lower triangular Cholesky matrix.
    * @return The decomposed matrix.
-   * @throws RuntimeException if the matrix could not be decomposed(ie. is not positive
+   * @throws RuntimeException if the matrix could not be decomposed(i.e. is not positive
    *     semidefinite).
    */
   public Matrix<R, C> lltDecompose(boolean lowerTriangular) {
@@ -646,10 +669,10 @@
    * same symbolic meaning they both must be either Double.NaN, Double.POSITIVE_INFINITY, or
    * Double.NEGATIVE_INFINITY.
    *
-   * <p>NOTE:It is recommend to use {@link Matrix#isEqual(Matrix, double)} over this method when
+   * <p>NOTE:It is recommended to use {@link Matrix#isEqual(Matrix, double)} over this method when
    * checking if two matrices are equal as {@link Matrix#isEqual(Matrix, double)} will return false
    * if an element is uncountable. This method should only be used when uncountable elements need to
-   * compared.
+   * be compared.
    *
    * @param other The {@link Matrix} to check against this one.
    * @param tolerance The tolerance to check equality with.
@@ -677,6 +700,20 @@
         this.m_storage.getDDRM(), other.m_storage.getDDRM(), tolerance);
   }
 
+  /**
+   * Performs an inplace Cholesky rank update (or downdate).
+   *
+   * <p>If this matrix contains L where A = LLᵀ before the update, it will contain L where LLᵀ = A +
+   * σvvᵀ after the update.
+   *
+   * @param v Vector to use for the update.
+   * @param sigma Sigma to use for the update.
+   * @param lowerTriangular Whether this matrix is lower triangular.
+   */
+  public void rankUpdate(Matrix<R, N1> v, double sigma, boolean lowerTriangular) {
+    WPIMathJNI.rankUpdate(this.getData(), this.getNumRows(), v.getData(), sigma, lowerTriangular);
+  }
+
   @Override
   public String toString() {
     return m_storage.toString();
diff --git a/wpimath/src/main/java/edu/wpi/first/math/MatrixUtils.java b/wpimath/src/main/java/edu/wpi/first/math/MatrixUtils.java
deleted file mode 100644
index 7600e31..0000000
--- a/wpimath/src/main/java/edu/wpi/first/math/MatrixUtils.java
+++ /dev/null
@@ -1,80 +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 edu.wpi.first.math.numbers.N1;
-import java.util.Objects;
-import org.ejml.simple.SimpleMatrix;
-
-@Deprecated
-public final class MatrixUtils {
-  private MatrixUtils() {
-    throw new AssertionError("utility class");
-  }
-
-  /**
-   * Creates a new matrix of zeros.
-   *
-   * @param rows The number of rows in the matrix.
-   * @param cols The number of columns in the matrix.
-   * @param <R> The number of rows in the matrix as a generic.
-   * @param <C> The number of columns in the matrix as a generic.
-   * @return An RxC matrix filled with zeros.
-   */
-  @SuppressWarnings("LineLength")
-  public static <R extends Num, C extends Num> Matrix<R, C> zeros(Nat<R> rows, Nat<C> cols) {
-    return new Matrix<>(
-        new SimpleMatrix(
-            Objects.requireNonNull(rows).getNum(), Objects.requireNonNull(cols).getNum()));
-  }
-
-  /**
-   * Creates a new vector of zeros.
-   *
-   * @param nums The size of the desired vector.
-   * @param <N> The size of the desired vector as a generic.
-   * @return A vector of size N filled with zeros.
-   */
-  public static <N extends Num> Matrix<N, N1> zeros(Nat<N> nums) {
-    return new Matrix<>(new SimpleMatrix(Objects.requireNonNull(nums).getNum(), 1));
-  }
-
-  /**
-   * Creates the identity matrix of the given dimension.
-   *
-   * @param dim The dimension of the desired matrix.
-   * @param <D> The dimension of the desired matrix as a generic.
-   * @return The DxD identity matrix.
-   */
-  public static <D extends Num> Matrix<D, D> eye(Nat<D> dim) {
-    return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
-  }
-
-  /**
-   * Entrypoint to the MatBuilder class for creation of custom matrices with the given dimensions
-   * and contents.
-   *
-   * @param rows The number of rows of the desired matrix.
-   * @param cols The number of columns of the desired matrix.
-   * @param <R> The number of rows of the desired matrix as a generic.
-   * @param <C> The number of columns of the desired matrix as a generic.
-   * @return A builder to construct the matrix.
-   */
-  public static <R extends Num, C extends Num> MatBuilder<R, C> mat(Nat<R> rows, Nat<C> cols) {
-    return new MatBuilder<>(rows, cols);
-  }
-
-  /**
-   * Entrypoint to the VecBuilder class for creation of custom vectors with the given size and
-   * contents.
-   *
-   * @param dim The dimension of the vector.
-   * @param <D> The dimension of the vector as a generic.
-   * @return A builder to construct the vector.
-   */
-  public static <D extends Num> VecBuilder<D> vec(Nat<D> dim) {
-    return new VecBuilder<>(dim);
-  }
-}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/Pair.java b/wpimath/src/main/java/edu/wpi/first/math/Pair.java
index d1a68c7..8ddf1ae 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Pair.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Pair.java
@@ -21,7 +21,6 @@
     return m_second;
   }
 
-  @SuppressWarnings("ParameterName")
   public static <A, B> Pair<A, B> of(A a, B b) {
     return new Pair<>(a, b);
   }
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 fc78dd1..e1680eb 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
@@ -21,36 +21,34 @@
    * @param matrix The matrix to compute the exponential of.
    * @return The resultant matrix.
    */
-  @SuppressWarnings({"LocalVariableName", "LineLength"})
   public static SimpleMatrix expm(SimpleMatrix matrix) {
     BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider = SimpleBase::solve;
     SimpleMatrix A = matrix;
     double A_L1 = NormOps_DDRM.inducedP1(matrix.getDDRM());
-    int n_squarings = 0;
+    int nSquarings = 0;
 
     if (A_L1 < 1.495585217958292e-002) {
-      Pair<SimpleMatrix, SimpleMatrix> pair = _pade3(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+      Pair<SimpleMatrix, SimpleMatrix> pair = pade3(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), nSquarings, solveProvider);
     } else if (A_L1 < 2.539398330063230e-001) {
-      Pair<SimpleMatrix, SimpleMatrix> pair = _pade5(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+      Pair<SimpleMatrix, SimpleMatrix> pair = pade5(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), nSquarings, solveProvider);
     } else if (A_L1 < 9.504178996162932e-001) {
-      Pair<SimpleMatrix, SimpleMatrix> pair = _pade7(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+      Pair<SimpleMatrix, SimpleMatrix> pair = pade7(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), nSquarings, solveProvider);
     } else if (A_L1 < 2.097847961257068e+000) {
-      Pair<SimpleMatrix, SimpleMatrix> pair = _pade9(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+      Pair<SimpleMatrix, SimpleMatrix> pair = pade9(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), nSquarings, solveProvider);
     } else {
       double maxNorm = 5.371920351148152;
       double log = Math.log(A_L1 / maxNorm) / Math.log(2); // logb(2, arg)
-      n_squarings = (int) Math.max(0, Math.ceil(log));
-      A = A.divide(Math.pow(2.0, n_squarings));
-      Pair<SimpleMatrix, SimpleMatrix> pair = _pade13(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+      nSquarings = (int) Math.max(0, Math.ceil(log));
+      A = A.divide(Math.pow(2.0, nSquarings));
+      Pair<SimpleMatrix, SimpleMatrix> pair = pade13(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), nSquarings, solveProvider);
     }
   }
 
-  @SuppressWarnings({"LocalVariableName", "ParameterName", "LineLength"})
   private static SimpleMatrix dispatchPade(
       SimpleMatrix U,
       SimpleMatrix V,
@@ -68,8 +66,7 @@
     return R;
   }
 
-  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
-  private static Pair<SimpleMatrix, SimpleMatrix> _pade3(SimpleMatrix A) {
+  private static Pair<SimpleMatrix, SimpleMatrix> pade3(SimpleMatrix A) {
     double[] b = new double[] {120, 60, 12, 1};
     SimpleMatrix ident = eye(A.numRows(), A.numCols());
 
@@ -79,8 +76,7 @@
     return new Pair<>(U, V);
   }
 
-  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
-  private static Pair<SimpleMatrix, SimpleMatrix> _pade5(SimpleMatrix A) {
+  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 A2 = A.mult(A);
@@ -92,8 +88,7 @@
     return new Pair<>(U, V);
   }
 
-  @SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
-  private static Pair<SimpleMatrix, SimpleMatrix> _pade7(SimpleMatrix A) {
+  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 A2 = A.mult(A);
@@ -108,8 +103,7 @@
     return new Pair<>(U, V);
   }
 
-  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName", "LineLength"})
-  private static Pair<SimpleMatrix, SimpleMatrix> _pade9(SimpleMatrix A) {
+  private static Pair<SimpleMatrix, SimpleMatrix> pade9(SimpleMatrix A) {
     double[] b =
         new double[] {
           17643225600.0, 8821612800.0, 2075673600, 302702400, 30270240, 2162160, 110880, 3960, 90, 1
@@ -137,8 +131,7 @@
     return new Pair<>(U, V);
   }
 
-  @SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
-  private static Pair<SimpleMatrix, SimpleMatrix> _pade13(SimpleMatrix A) {
+  private static Pair<SimpleMatrix, SimpleMatrix> pade13(SimpleMatrix A) {
     double[] b =
         new double[] {
           64764752532480000.0,
@@ -199,7 +192,7 @@
    *
    * @param src The matrix to decompose.
    * @return The decomposed matrix.
-   * @throws RuntimeException if the matrix could not be decomposed (ie. is not positive
+   * @throws RuntimeException if the matrix could not be decomposed (i.e. is not positive
    *     semidefinite).
    */
   public static SimpleMatrix lltDecompose(SimpleMatrix src) {
@@ -213,7 +206,7 @@
    * @param src The matrix to decompose.
    * @param lowerTriangular if we want to decompose to the lower triangular Cholesky matrix.
    * @return The decomposed matrix.
-   * @throws RuntimeException if the matrix could not be decomposed (ie. is not positive
+   * @throws RuntimeException if the matrix could not be decomposed (i.e. is not positive
    *     semidefinite).
    */
   public static SimpleMatrix lltDecompose(SimpleMatrix src, boolean lowerTriangular) {
@@ -245,7 +238,6 @@
    * @param A the matrix to exponentiate.
    * @return the exponential of A.
    */
-  @SuppressWarnings("ParameterName")
   public static SimpleMatrix exp(SimpleMatrix A) {
     SimpleMatrix toReturn = new SimpleMatrix(A.numRows(), A.numRows());
     WPIMathJNI.exp(A.getDDRM().getData(), A.numRows(), toReturn.getDDRM().getData());
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 69430eb..a041845 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
@@ -11,7 +11,6 @@
 import java.util.Random;
 import org.ejml.simple.SimpleMatrix;
 
-@SuppressWarnings("ParameterName")
 public final class StateSpaceUtil {
   private static Random rand = new Random();
 
@@ -31,7 +30,6 @@
    *     output measurement.
    * @return Process noise or measurement noise covariance matrix.
    */
-  @SuppressWarnings("MethodTypeParameterName")
   public static <States extends Num> Matrix<States, States> makeCovarianceMatrix(
       Nat<States> states, Matrix<States, N1> stdDevs) {
     var result = new Matrix<>(states, states);
@@ -61,24 +59,28 @@
   /**
    * Creates a cost matrix from the given vector for use with LQR.
    *
-   * <p>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.
+   * <p>The cost matrix is constructed using Bryson's rule. The inverse square of each tolerance is
+   * placed on the cost matrix diagonal. If a tolerance is infinity, its cost matrix entry is set to
+   * zero.
    *
-   * @param <States> Nat representing the states of the system.
-   * @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, its elements are the maximum allowed excursions
-   *     of the control inputs from no actuation.
+   * @param <Elements> Nat representing the number of system states or inputs.
+   * @param tolerances An array. For a Q matrix, its elements are the maximum allowed excursions of
+   *     the states from the reference. For an R matrix, its elements are the maximum allowed
+   *     excursions of the control inputs from no actuation.
    * @return State excursion or control effort cost matrix.
    */
-  @SuppressWarnings("MethodTypeParameterName")
-  public static <States extends Num> Matrix<States, States> makeCostMatrix(
-      Matrix<States, N1> costs) {
-    Matrix<States, States> result =
-        new Matrix<>(new SimpleMatrix(costs.getNumRows(), costs.getNumRows()));
+  public static <Elements extends Num> Matrix<Elements, Elements> makeCostMatrix(
+      Matrix<Elements, N1> tolerances) {
+    Matrix<Elements, Elements> result =
+        new Matrix<>(new SimpleMatrix(tolerances.getNumRows(), tolerances.getNumRows()));
     result.fill(0.0);
 
-    for (int i = 0; i < costs.getNumRows(); i++) {
-      result.set(i, i, 1.0 / (Math.pow(costs.get(i, 0), 2)));
+    for (int i = 0; i < tolerances.getNumRows(); i++) {
+      if (tolerances.get(i, 0) == Double.POSITIVE_INFINITY) {
+        result.set(i, i, 0.0);
+      } else {
+        result.set(i, i, 1.0 / (Math.pow(tolerances.get(i, 0), 2)));
+      }
     }
 
     return result;
@@ -97,7 +99,6 @@
    * @param B Input matrix.
    * @return If the system is stabilizable.
    */
-  @SuppressWarnings("MethodTypeParameterName")
   public static <States extends Num, Inputs extends Num> boolean isStabilizable(
       Matrix<States, States> A, Matrix<States, Inputs> B) {
     return WPIMathJNI.isStabilizable(A.getNumRows(), B.getNumCols(), A.getData(), B.getData());
@@ -116,7 +117,6 @@
    * @param C Output matrix.
    * @return If the system is detectable.
    */
-  @SuppressWarnings("MethodTypeParameterName")
   public static <States extends Num, Outputs extends Num> boolean isDetectable(
       Matrix<States, States> A, Matrix<Outputs, States> C) {
     return WPIMathJNI.isStabilizable(
@@ -142,7 +142,6 @@
    * @param <I> The number of inputs.
    * @return The clamped input.
    */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
   public static <I extends Num> Matrix<I, N1> clampInputMaxMagnitude(
       Matrix<I, N1> u, Matrix<I, N1> umin, Matrix<I, N1> umax) {
     var result = new Matrix<I, N1>(new SimpleMatrix(u.getNumRows(), 1));
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 9b06e71..1aebdde 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Vector.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Vector.java
@@ -61,4 +61,35 @@
   public Vector<R> div(double value) {
     return new Vector<>(this.m_storage.divide(value));
   }
+
+  /**
+   * Returns the dot product of this vector with another.
+   *
+   * @param other The other vector.
+   * @return The dot product.
+   */
+  public double dot(Vector<R> other) {
+    double dot = 0.0;
+
+    for (int i = 0; i < getNumRows(); ++i) {
+      dot += get(i, 0) * other.get(i, 0);
+    }
+
+    return dot;
+  }
+
+  /**
+   * Returns the norm of this vector.
+   *
+   * @return The norm.
+   */
+  public double norm() {
+    double squaredNorm = 0.0;
+
+    for (int i = 0; i < getNumRows(); ++i) {
+      squaredNorm += get(i, 0) * get(i, 0);
+    }
+
+    return Math.sqrt(squaredNorm);
+  }
 }
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 54445d3..40a5c63 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
@@ -125,6 +125,19 @@
    */
   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);
 
@@ -136,4 +149,18 @@
       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/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
index 2991511..2e9a8d8 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
@@ -8,10 +8,9 @@
  * A helper class that computes feedforward outputs for a simple arm (modeled as a motor acting
  * against the force of gravity on a beam suspended at an angle).
  */
-@SuppressWarnings("MemberName")
 public class ArmFeedforward {
   public final double ks;
-  public final double kcos;
+  public final double kg;
   public final double kv;
   public final double ka;
 
@@ -20,13 +19,13 @@
    * units of the computed feedforward.
    *
    * @param ks The static gain.
-   * @param kcos The gravity gain.
+   * @param kg The gravity gain.
    * @param kv The velocity gain.
    * @param ka The acceleration gain.
    */
-  public ArmFeedforward(double ks, double kcos, double kv, double ka) {
+  public ArmFeedforward(double ks, double kg, double kv, double ka) {
     this.ks = ks;
-    this.kcos = kcos;
+    this.kg = kg;
     this.kv = kv;
     this.ka = ka;
   }
@@ -36,17 +35,19 @@
    * Units of the gain values will dictate units of the computed feedforward.
    *
    * @param ks The static gain.
-   * @param kcos The gravity gain.
+   * @param kg The gravity gain.
    * @param kv The velocity gain.
    */
-  public ArmFeedforward(double ks, double kcos, double kv) {
-    this(ks, kcos, kv, 0);
+  public ArmFeedforward(double ks, double kg, double kv) {
+    this(ks, kg, kv, 0);
   }
 
   /**
    * Calculates the feedforward from the gains and setpoints.
    *
-   * @param positionRadians The position (angle) setpoint.
+   * @param positionRadians The position (angle) setpoint. This angle should be measured from the
+   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
+   *     your encoder does not follow this convention, an offset should be added.
    * @param velocityRadPerSec The velocity setpoint.
    * @param accelRadPerSecSquared The acceleration setpoint.
    * @return The computed feedforward.
@@ -54,7 +55,7 @@
   public double calculate(
       double positionRadians, double velocityRadPerSec, double accelRadPerSecSquared) {
     return ks * Math.signum(velocityRadPerSec)
-        + kcos * Math.cos(positionRadians)
+        + kg * Math.cos(positionRadians)
         + kv * velocityRadPerSec
         + ka * accelRadPerSecSquared;
   }
@@ -63,7 +64,9 @@
    * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
    * zero).
    *
-   * @param positionRadians The position (angle) setpoint.
+   * @param positionRadians The position (angle) setpoint. This angle should be measured from the
+   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
+   *     your encoder does not follow this convention, an offset should be added.
    * @param velocity The velocity setpoint.
    * @return The computed feedforward.
    */
@@ -81,13 +84,15 @@
    * you a simultaneously-achievable velocity constraint.
    *
    * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm.
+   * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if
+   *     the provided angle is 0, the arm should be parallel with the floor). If your encoder does
+   *     not follow this convention, an offset should be added.
    * @param acceleration The acceleration of the arm.
    * @return The maximum possible velocity at the given acceleration and angle.
    */
   public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) {
     // Assume max velocity is positive
-    return (maxVoltage - ks - Math.cos(angle) * kcos - acceleration * ka) / kv;
+    return (maxVoltage - ks - Math.cos(angle) * kg - acceleration * ka) / kv;
   }
 
   /**
@@ -97,13 +102,15 @@
    * you a simultaneously-achievable velocity constraint.
    *
    * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm.
+   * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if
+   *     the provided angle is 0, the arm should be parallel with the floor). If your encoder does
+   *     not follow this convention, an offset should be added.
    * @param acceleration The acceleration of the arm.
    * @return The minimum possible velocity at the given acceleration and angle.
    */
   public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) {
     // Assume min velocity is negative, ks flips sign
-    return (-maxVoltage + ks - Math.cos(angle) * kcos - acceleration * ka) / kv;
+    return (-maxVoltage + ks - Math.cos(angle) * kg - acceleration * ka) / kv;
   }
 
   /**
@@ -113,12 +120,14 @@
    * simultaneously-achievable acceleration constraint.
    *
    * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm.
+   * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if
+   *     the provided angle is 0, the arm should be parallel with the floor). If your encoder does
+   *     not follow this convention, an offset should be added.
    * @param velocity The velocity of the arm.
    * @return The maximum possible acceleration at the given velocity.
    */
   public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) {
-    return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kcos - velocity * kv) / ka;
+    return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kg - velocity * kv) / ka;
   }
 
   /**
@@ -128,7 +137,9 @@
    * simultaneously-achievable acceleration constraint.
    *
    * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm.
+   * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if
+   *     the provided angle is 0, the arm should be parallel with the floor). If your encoder does
+   *     not follow this convention, an offset should be added.
    * @param velocity The velocity of the arm.
    * @return The minimum possible acceleration at the given velocity.
    */
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 d4beabd..385c5c0 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
@@ -29,16 +29,13 @@
  * <p>For more on the underlying math, read
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
  */
-@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
 public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs extends Num> {
   /** The current reference state. */
-  @SuppressWarnings("MemberName")
   private Matrix<States, N1> m_r;
 
   /** The computed feedforward. */
   private Matrix<Inputs, N1> m_uff;
 
-  @SuppressWarnings("MemberName")
   private final Matrix<States, Inputs> m_B;
 
   private final Nat<Inputs> m_inputs;
@@ -181,7 +178,6 @@
    * @param nextR The reference state of the future timestep (k + dt).
    * @return The calculated feedforward.
    */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
   public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
     var rDot = (nextR.minus(r)).div(m_dt);
 
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java
new file mode 100644
index 0000000..8c47765
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java
@@ -0,0 +1,130 @@
+// 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 edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+
+/**
+ * Filters the provided voltages to limit a differential drive's linear and angular acceleration.
+ *
+ * <p>The differential drive model can be created via the functions in {@link
+ * edu.wpi.first.math.system.plant.LinearSystemId}.
+ */
+public class DifferentialDriveAccelerationLimiter {
+  private final LinearSystem<N2, N2, N2> m_system;
+  private final double m_trackwidth;
+  private final double m_minLinearAccel;
+  private final double m_maxLinearAccel;
+  private final double m_maxAngularAccel;
+
+  /**
+   * Constructs a DifferentialDriveAccelerationLimiter.
+   *
+   * @param system The differential drive dynamics.
+   * @param trackwidth The distance between the differential drive's left and right wheels in
+   *     meters.
+   * @param maxLinearAccel The maximum linear acceleration in meters per second squared.
+   * @param maxAngularAccel The maximum angular acceleration in radians per second squared.
+   */
+  public DifferentialDriveAccelerationLimiter(
+      LinearSystem<N2, N2, N2> system,
+      double trackwidth,
+      double maxLinearAccel,
+      double maxAngularAccel) {
+    this(system, trackwidth, -maxLinearAccel, maxLinearAccel, maxAngularAccel);
+  }
+
+  /**
+   * Constructs a DifferentialDriveAccelerationLimiter.
+   *
+   * @param system The differential drive dynamics.
+   * @param trackwidth The distance between the differential drive's left and right wheels in
+   *     meters.
+   * @param minLinearAccel The minimum (most negative) linear acceleration in meters per second
+   *     squared.
+   * @param maxLinearAccel The maximum (most positive) linear acceleration in meters per second
+   *     squared.
+   * @param maxAngularAccel The maximum angular acceleration in radians per second squared.
+   * @throws IllegalArgumentException if minimum linear acceleration is greater than maximum linear
+   *     acceleration
+   */
+  public DifferentialDriveAccelerationLimiter(
+      LinearSystem<N2, N2, N2> system,
+      double trackwidth,
+      double minLinearAccel,
+      double maxLinearAccel,
+      double maxAngularAccel) {
+    if (minLinearAccel > maxLinearAccel) {
+      throw new IllegalArgumentException("maxLinearAccel must be greater than minLinearAccel");
+    }
+    m_system = system;
+    m_trackwidth = trackwidth;
+    m_minLinearAccel = minLinearAccel;
+    m_maxLinearAccel = maxLinearAccel;
+    m_maxAngularAccel = maxAngularAccel;
+  }
+
+  /**
+   * Returns the next voltage pair subject to acceleration constraints.
+   *
+   * @param leftVelocity The left wheel velocity in meters per second.
+   * @param rightVelocity The right wheel velocity in meters per second.
+   * @param leftVoltage The unconstrained left motor voltage.
+   * @param rightVoltage The unconstrained right motor voltage.
+   * @return The constrained wheel voltages.
+   */
+  public DifferentialDriveWheelVoltages calculate(
+      double leftVelocity, double rightVelocity, double leftVoltage, double rightVoltage) {
+    var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVoltage, rightVoltage);
+
+    // Find unconstrained wheel accelerations
+    var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVelocity, rightVelocity);
+    var dxdt = m_system.getA().times(x).plus(m_system.getB().times(u));
+
+    // Convert from wheel accelerations to linear and angular accelerations
+    //
+    // a = (dxdt(0) + dx/dt(1)) / 2
+    //   = 0.5 dxdt(0) + 0.5 dxdt(1)
+    //
+    // α = (dxdt(1) - dxdt(0)) / trackwidth
+    //   = -1/trackwidth dxdt(0) + 1/trackwidth dxdt(1)
+    //
+    // [a] = [          0.5           0.5][dxdt(0)]
+    // [α]   [-1/trackwidth  1/trackwidth][dxdt(1)]
+    //
+    // accels = M dxdt where M = [0.5, 0.5; -1/trackwidth, 1/trackwidth]
+    var M =
+        new MatBuilder<>(Nat.N2(), Nat.N2())
+            .fill(0.5, 0.5, -1.0 / m_trackwidth, 1.0 / m_trackwidth);
+    var accels = M.times(dxdt);
+
+    // Constrain the linear and angular accelerations
+    if (accels.get(0, 0) > m_maxLinearAccel) {
+      accels.set(0, 0, m_maxLinearAccel);
+    } else if (accels.get(0, 0) < m_minLinearAccel) {
+      accels.set(0, 0, m_minLinearAccel);
+    }
+    if (accels.get(1, 0) > m_maxAngularAccel) {
+      accels.set(1, 0, m_maxAngularAccel);
+    } else if (accels.get(1, 0) < -m_maxAngularAccel) {
+      accels.set(1, 0, -m_maxAngularAccel);
+    }
+
+    // Convert the constrained linear and angular accelerations back to wheel
+    // accelerations
+    dxdt = M.solve(accels);
+
+    // Find voltages for the given wheel accelerations
+    //
+    // dx/dt = Ax + Bu
+    // u = B⁻¹(dx/dt - Ax)
+    u = m_system.getB().solve(dxdt.minus(m_system.getA().times(x)));
+
+    return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0));
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java
new file mode 100644
index 0000000..4d0c94f
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java
@@ -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.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+
+/** A helper class which computes the feedforward outputs for a differential drive drivetrain. */
+public class DifferentialDriveFeedforward {
+  private final LinearSystem<N2, N2, N2> m_plant;
+
+  /**
+   * Creates a new DifferentialDriveFeedforward with the specified parameters.
+   *
+   * @param kVLinear The linear velocity gain in volts per (meters per second).
+   * @param kALinear The linear acceleration gain in volts per (meters per second squared).
+   * @param kVAngular The angular velocity gain in volts per (radians per second).
+   * @param kAAngular The angular acceleration gain in volts per (radians per second squared).
+   * @param trackwidth The distance between the differential drive's left and right wheels, in
+   *     meters.
+   */
+  public DifferentialDriveFeedforward(
+      double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
+    m_plant =
+        LinearSystemId.identifyDrivetrainSystem(
+            kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
+  }
+
+  /**
+   * Creates a new DifferentialDriveFeedforward with the specified parameters.
+   *
+   * @param kVLinear The linear velocity gain in volts per (meters per second).
+   * @param kALinear The linear acceleration gain in volts per (meters per second squared).
+   * @param kVAngular The angular velocity gain in volts per (meters per second).
+   * @param kAAngular The angular acceleration gain in volts per (meters per second squared).
+   */
+  public DifferentialDriveFeedforward(
+      double kVLinear, double kALinear, double kVAngular, double kAAngular) {
+    m_plant = LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular);
+  }
+
+  /**
+   * Calculates the differential drive feedforward inputs given velocity setpoints.
+   *
+   * @param currentLeftVelocity The current left velocity of the differential drive in
+   *     meters/second.
+   * @param nextLeftVelocity The next left velocity of the differential drive in meters/second.
+   * @param currentRightVelocity The current right velocity of the differential drive in
+   *     meters/second.
+   * @param nextRightVelocity The next right velocity of the differential drive in meters/second.
+   * @param dtSeconds Discretization timestep.
+   * @return A DifferentialDriveWheelVoltages object containing the computed feedforward voltages.
+   */
+  public DifferentialDriveWheelVoltages calculate(
+      double currentLeftVelocity,
+      double nextLeftVelocity,
+      double currentRightVelocity,
+      double nextRightVelocity,
+      double dtSeconds) {
+    var feedforward = new LinearPlantInversionFeedforward<>(m_plant, dtSeconds);
+    var r = VecBuilder.fill(currentLeftVelocity, currentRightVelocity);
+    var nextR = VecBuilder.fill(nextLeftVelocity, nextRightVelocity);
+    var u = feedforward.calculate(r, nextR);
+    return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0));
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java
new file mode 100644
index 0000000..2fbd0aa
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java
@@ -0,0 +1,18 @@
+// 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;
+
+/** Motor voltages for a differential drive. */
+public class DifferentialDriveWheelVoltages {
+  public double left;
+  public double right;
+
+  public DifferentialDriveWheelVoltages() {}
+
+  public DifferentialDriveWheelVoltages(double left, double right) {
+    this.left = left;
+    this.right = right;
+  }
+}
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 248015f..9f4dd86 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
@@ -8,7 +8,6 @@
  * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting
  * against the force of gravity).
  */
-@SuppressWarnings("MemberName")
 public class ElevatorFeedforward {
   public final double ks;
   public final double kg;
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 be813cc..e43c6ff 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
@@ -8,6 +8,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.util.Units;
 
 /**
  * This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain
@@ -20,7 +21,6 @@
  * are decoupled from translations, users can specify a custom heading that the drivetrain should
  * point toward. This heading reference is profiled for smoothness.
  */
-@SuppressWarnings("MemberName")
 public class HolonomicDriveController {
   private Pose2d m_poseError = new Pose2d();
   private Rotation2d m_rotationError = new Rotation2d();
@@ -40,12 +40,12 @@
    * @param yController A PID Controller to respond to error in the field-relative y direction.
    * @param thetaController A profiled PID controller to respond to error in angle.
    */
-  @SuppressWarnings("ParameterName")
   public HolonomicDriveController(
       PIDController xController, PIDController yController, ProfiledPIDController thetaController) {
     m_xController = xController;
     m_yController = yController;
     m_thetaController = thetaController;
+    m_thetaController.enableContinuousInput(0, Units.degreesToRadians(360.0));
   }
 
   /**
@@ -75,15 +75,17 @@
   /**
    * Returns the next output of the holonomic drive controller.
    *
-   * @param currentPose The current pose.
-   * @param poseRef The desired pose.
-   * @param linearVelocityRefMeters The linear velocity reference.
-   * @param angleRef The angular reference.
+   * @param currentPose The current pose, as measured by odometry or pose estimator.
+   * @param trajectoryPose The desired trajectory pose, as sampled for the current timestep.
+   * @param desiredLinearVelocityMetersPerSecond The desired linear velocity.
+   * @param desiredHeading The desired heading.
    * @return The next output of the holonomic drive controller.
    */
-  @SuppressWarnings("LocalVariableName")
   public ChassisSpeeds calculate(
-      Pose2d currentPose, Pose2d poseRef, double linearVelocityRefMeters, Rotation2d angleRef) {
+      Pose2d currentPose,
+      Pose2d trajectoryPose,
+      double desiredLinearVelocityMetersPerSecond,
+      Rotation2d desiredHeading) {
     // If this is the first run, then we need to reset the theta controller to the current pose's
     // heading.
     if (m_firstRun) {
@@ -92,21 +94,22 @@
     }
 
     // Calculate feedforward velocities (field-relative).
-    double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos();
-    double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin();
+    double xFF = desiredLinearVelocityMetersPerSecond * trajectoryPose.getRotation().getCos();
+    double yFF = desiredLinearVelocityMetersPerSecond * trajectoryPose.getRotation().getSin();
     double thetaFF =
-        m_thetaController.calculate(currentPose.getRotation().getRadians(), angleRef.getRadians());
+        m_thetaController.calculate(
+            currentPose.getRotation().getRadians(), desiredHeading.getRadians());
 
-    m_poseError = poseRef.relativeTo(currentPose);
-    m_rotationError = angleRef.minus(currentPose.getRotation());
+    m_poseError = trajectoryPose.relativeTo(currentPose);
+    m_rotationError = desiredHeading.minus(currentPose.getRotation());
 
     if (!m_enabled) {
       return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation());
     }
 
     // Calculate feedback velocities (based on position error).
-    double xFeedback = m_xController.calculate(currentPose.getX(), poseRef.getX());
-    double yFeedback = m_yController.calculate(currentPose.getY(), poseRef.getY());
+    double xFeedback = m_xController.calculate(currentPose.getX(), trajectoryPose.getX());
+    double yFeedback = m_yController.calculate(currentPose.getY(), trajectoryPose.getY());
 
     // Return next output.
     return ChassisSpeeds.fromFieldRelativeSpeeds(
@@ -116,15 +119,15 @@
   /**
    * Returns the next output of the holonomic drive controller.
    *
-   * @param currentPose The current pose.
-   * @param desiredState The desired trajectory state.
-   * @param angleRef The desired end-angle.
+   * @param currentPose The current pose, as measured by odometry or pose estimator.
+   * @param desiredState The desired trajectory pose, as sampled for the current timestep.
+   * @param desiredHeading The desired heading.
    * @return The next output of the holonomic drive controller.
    */
   public ChassisSpeeds calculate(
-      Pose2d currentPose, Trajectory.State desiredState, Rotation2d angleRef) {
+      Pose2d currentPose, Trajectory.State desiredState, Rotation2d desiredHeading) {
     return calculate(
-        currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond, angleRef);
+        currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond, desiredHeading);
   }
 
   /**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java
new file mode 100644
index 0000000..7aa4cfa
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ImplicitModelFollower.java
@@ -0,0 +1,117 @@
+// 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 edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.LinearSystem;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * Contains the controller coefficients and logic for an implicit model follower.
+ *
+ * <p>Implicit model following lets us design a feedback controller that erases the dynamics of our
+ * system and makes it behave like some other system. This can be used to make a drivetrain more
+ * controllable during teleop driving by making it behave like a slower or more benign drivetrain.
+ *
+ * <p>For more on the underlying math, read appendix B.3 in
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+public class ImplicitModelFollower<States extends Num, Inputs extends Num, Outputs extends Num> {
+  // Computed controller output
+  private Matrix<Inputs, N1> m_u;
+
+  // State space conversion gain
+  private Matrix<Inputs, States> m_A;
+
+  // Input space conversion gain
+  private Matrix<Inputs, Inputs> m_B;
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param plant The plant being controlled.
+   * @param plantRef The plant whose dynamics should be followed.
+   */
+  public ImplicitModelFollower(
+      LinearSystem<States, Inputs, Outputs> plant, LinearSystem<States, Inputs, Outputs> plantRef) {
+    this(plant.getA(), plant.getB(), plantRef.getA(), plantRef.getB());
+  }
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A Continuous system matrix of the plant being controlled.
+   * @param B Continuous input matrix of the plant being controlled.
+   * @param Aref Continuous system matrix whose dynamics should be followed.
+   * @param Bref Continuous input matrix whose dynamics should be followed.
+   */
+  public ImplicitModelFollower(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Aref,
+      Matrix<States, Inputs> Bref) {
+    m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
+
+    // Find u_imf that makes real model match reference model.
+    //
+    // dx/dt = Ax + Bu_imf
+    // dz/dt = A_ref z + B_ref u
+    //
+    // Let x = z.
+    //
+    // dx/dt = dz/dt
+    // Ax + Bu_imf = Aref x + B_ref u
+    // Bu_imf = A_ref x - Ax + B_ref u
+    // Bu_imf = (A_ref - A)x + B_ref u
+    // u_imf = B⁻¹((A_ref - A)x + Bref u)
+    // u_imf = -B⁻¹(A - A_ref)x + B⁻¹B_ref u
+
+    // The first term makes the open-loop poles that of the reference
+    // system, and the second term makes the input behave like that of the
+    // reference system.
+    m_A = B.solve(A.minus(Aref)).times(-1.0);
+    m_B = B.solve(Bref);
+
+    reset();
+  }
+
+  /**
+   * Returns the control input vector u.
+   *
+   * @return The control input.
+   */
+  public Matrix<Inputs, N1> getU() {
+    return m_u;
+  }
+
+  /**
+   * Returns an element of the control input vector u.
+   *
+   * @param i Row of u.
+   * @return The row of the control input vector.
+   */
+  public double getU(int i) {
+    return m_u.get(i, 0);
+  }
+
+  /** Resets the controller. */
+  public void reset() {
+    m_u.fill(0.0);
+  }
+
+  /**
+   * Returns the next output of the controller.
+   *
+   * @param x The current state x.
+   * @param u The current input for the original model.
+   * @return The next controller output.
+   */
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> x, Matrix<Inputs, N1> u) {
+    m_u = m_A.times(x).plus(m_B.times(u));
+    return m_u;
+  }
+}
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
new file mode 100644
index 0000000..46254a3
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
@@ -0,0 +1,270 @@
+// 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 edu.wpi.first.math.InterpolatingMatrixTreeMap;
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.Pose2d;
+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.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.
+ *
+ * <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.
+ */
+public class LTVDifferentialDriveController {
+  private final double m_trackwidth;
+
+  // LUT from drivetrain linear velocity to LQR gain
+  private final InterpolatingMatrixTreeMap<Double, N2, N5> m_table =
+      new InterpolatingMatrixTreeMap<>();
+
+  private Matrix<N5, N1> m_error = new Matrix<>(Nat.N5(), Nat.N1());
+  private Matrix<N5, N1> m_tolerance = new Matrix<>(Nat.N5(), Nat.N1());
+
+  /** States of the drivetrain system. */
+  private enum State {
+    kX(0),
+    kY(1),
+    kHeading(2),
+    kLeftVelocity(3),
+    kRightVelocity(4);
+
+    public final int value;
+
+    State(int i) {
+      this.value = i;
+    }
+  }
+
+  /**
+   * Constructs a linear time-varying differential drive controller.
+   *
+   * @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.
+   */
+  public LTVDifferentialDriveController(
+      LinearSystem<N2, N2, N2> plant,
+      double trackwidth,
+      Vector<N5> qelems,
+      Vector<N2> relems,
+      double dt) {
+    m_trackwidth = trackwidth;
+
+    // Control law derivation is in section 8.7 of
+    // https://file.tavsys.net/control/controls-engineering-in-frc.pdf
+    var A =
+        new MatBuilder<>(Nat.N5(), Nat.N5())
+            .fill(
+                0.0,
+                0.0,
+                0.0,
+                0.5,
+                0.5,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                -1.0 / m_trackwidth,
+                1.0 / m_trackwidth,
+                0.0,
+                0.0,
+                0.0,
+                plant.getA(0, 0),
+                plant.getA(0, 1),
+                0.0,
+                0.0,
+                0.0,
+                plant.getA(1, 0),
+                plant.getA(1, 1));
+    var B =
+        new MatBuilder<>(Nat.N5(), Nat.N2())
+            .fill(
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                plant.getB(0, 0),
+                plant.getB(0, 1),
+                plant.getB(1, 0),
+                plant.getB(1, 1));
+    var Q = StateSpaceUtil.makeCostMatrix(qelems);
+    var R = StateSpaceUtil.makeCostMatrix(relems);
+
+    // dx/dt = Ax + Bu
+    // 0 = Ax + Bu
+    // Ax = -Bu
+    // x = -A⁻¹Bu
+    double maxV =
+        plant
+            .getA()
+            .solve(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0)))
+            .times(-1.0)
+            .get(0, 0);
+
+    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()));
+      } 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());
+      }
+    }
+  }
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   *
+   * @return True if the pose error is within tolerance of the reference.
+   */
+  public boolean atReference() {
+    return Math.abs(m_error.get(0, 0)) < m_tolerance.get(0, 0)
+        && Math.abs(m_error.get(1, 0)) < m_tolerance.get(1, 0)
+        && Math.abs(m_error.get(2, 0)) < m_tolerance.get(2, 0)
+        && Math.abs(m_error.get(3, 0)) < m_tolerance.get(3, 0)
+        && Math.abs(m_error.get(4, 0)) < m_tolerance.get(4, 0);
+  }
+
+  /**
+   * Sets the pose error which is considered tolerable for use with atReference().
+   *
+   * @param poseTolerance Pose error which is tolerable.
+   * @param leftVelocityTolerance Left velocity error which is tolerable in meters per second.
+   * @param rightVelocityTolerance Right velocity error which is tolerable in meters per second.
+   */
+  public void setTolerance(
+      Pose2d poseTolerance, double leftVelocityTolerance, double rightVelocityTolerance) {
+    m_tolerance =
+        new MatBuilder<>(Nat.N5(), Nat.N1())
+            .fill(
+                poseTolerance.getX(),
+                poseTolerance.getY(),
+                poseTolerance.getRotation().getRadians(),
+                leftVelocityTolerance,
+                rightVelocityTolerance);
+  }
+
+  /**
+   * Returns the left and right output voltages of the LTV controller.
+   *
+   * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
+   * trajectory.
+   *
+   * @param currentPose The current pose.
+   * @param leftVelocity The current left velocity in meters per second.
+   * @param rightVelocity The current right velocity in meters per second.
+   * @param poseRef The desired pose.
+   * @param leftVelocityRef The desired left velocity in meters per second.
+   * @param rightVelocityRef The desired right velocity in meters per second.
+   * @return Left and right output voltages of the LTV controller.
+   */
+  public DifferentialDriveWheelVoltages calculate(
+      Pose2d currentPose,
+      double leftVelocity,
+      double rightVelocity,
+      Pose2d poseRef,
+      double leftVelocityRef,
+      double rightVelocityRef) {
+    // This implements the linear time-varying differential drive controller in
+    // theorem 9.6.3 of https://tavsys.net/controls-in-frc.
+    var x =
+        new MatBuilder<>(Nat.N5(), Nat.N1())
+            .fill(
+                currentPose.getX(),
+                currentPose.getY(),
+                currentPose.getRotation().getRadians(),
+                leftVelocity,
+                rightVelocity);
+
+    var inRobotFrame = Matrix.eye(Nat.N5());
+    inRobotFrame.set(0, 0, Math.cos(x.get(State.kHeading.value, 0)));
+    inRobotFrame.set(0, 1, Math.sin(x.get(State.kHeading.value, 0)));
+    inRobotFrame.set(1, 0, -Math.sin(x.get(State.kHeading.value, 0)));
+    inRobotFrame.set(1, 1, Math.cos(x.get(State.kHeading.value, 0)));
+
+    var r =
+        new MatBuilder<>(Nat.N5(), Nat.N1())
+            .fill(
+                poseRef.getX(),
+                poseRef.getY(),
+                poseRef.getRotation().getRadians(),
+                leftVelocityRef,
+                rightVelocityRef);
+    m_error = r.minus(x);
+    m_error.set(
+        State.kHeading.value, 0, MathUtil.angleModulus(m_error.get(State.kHeading.value, 0)));
+
+    double velocity = (leftVelocity + rightVelocity) / 2.0;
+    var K = m_table.get(velocity);
+
+    var u = K.times(inRobotFrame).times(m_error);
+
+    return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0));
+  }
+
+  /**
+   * Returns the left and right output voltages of the LTV controller.
+   *
+   * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
+   * trajectory.
+   *
+   * @param currentPose The current pose.
+   * @param leftVelocity The left velocity in meters per second.
+   * @param rightVelocity The right velocity in meters per second.
+   * @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory.
+   * @return The left and right output voltages of the LTV controller.
+   */
+  public DifferentialDriveWheelVoltages calculate(
+      Pose2d currentPose,
+      double leftVelocity,
+      double rightVelocity,
+      Trajectory.State desiredState) {
+    // v = (v_r + v_l) / 2     (1)
+    // w = (v_r - v_l) / (2r)  (2)
+    // k = w / v               (3)
+    //
+    // v_l = v - wr
+    // v_l = v - (vk)r
+    // v_l = v(1 - kr)
+    //
+    // v_r = v + wr
+    // v_r = v + (vk)r
+    // v_r = v(1 + kr)
+    return calculate(
+        currentPose,
+        leftVelocity,
+        rightVelocity,
+        desiredState.poseMeters,
+        desiredState.velocityMetersPerSecond
+            * (1 - (desiredState.curvatureRadPerMeter * m_trackwidth / 2.0)),
+        desiredState.velocityMetersPerSecond
+            * (1 + (desiredState.curvatureRadPerMeter * m_trackwidth / 2.0)));
+  }
+}
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
new file mode 100644
index 0000000..701f21b
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
@@ -0,0 +1,226 @@
+// 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 edu.wpi.first.math.InterpolatingMatrixTreeMap;
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.Pose2d;
+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.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.
+ *
+ * <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.
+ */
+public class LTVUnicycleController {
+  // LUT from drivetrain linear velocity to LQR gain
+  private final InterpolatingMatrixTreeMap<Double, N2, N3> m_table =
+      new InterpolatingMatrixTreeMap<>();
+
+  private Pose2d m_poseError;
+  private Pose2d m_poseTolerance;
+  private boolean m_enabled = true;
+
+  /** States of the drivetrain system. */
+  private enum State {
+    kX(0),
+    kY(1),
+    kHeading(2);
+
+    public final int value;
+
+    State(int i) {
+      this.value = i;
+    }
+  }
+
+  /**
+   * Constructs a linear time-varying unicycle controller with default maximum desired error
+   * tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum desired control effort of (1 m/s,
+   * 2 rad/s).
+   *
+   * @param dt Discretization timestep in seconds.
+   */
+  public LTVUnicycleController(double dt) {
+    this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, 9.0);
+  }
+
+  /**
+   * Constructs a linear time-varying unicycle controller with default maximum desired error
+   * tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum desired control effort of (1 m/s,
+   * 2 rad/s).
+   *
+   * @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.
+   */
+  public LTVUnicycleController(double dt, double maxVelocity) {
+    this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, maxVelocity);
+  }
+
+  /**
+   * Constructs a linear time-varying unicycle controller.
+   *
+   * @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.
+   */
+  public LTVUnicycleController(Vector<N3> qelems, Vector<N2> relems, double dt) {
+    this(qelems, relems, dt, 9.0);
+  }
+
+  /**
+   * Constructs a linear time-varying unicycle controller.
+   *
+   * @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.
+   */
+  public LTVUnicycleController(
+      Vector<N3> qelems, Vector<N2> relems, double dt, double maxVelocity) {
+    // The change in global pose for a unicycle is defined by the following
+    // three equations.
+    //
+    // ẋ = v cosθ
+    // ẏ = v sinθ
+    // θ̇ = ω
+    //
+    // Here's the model as a vector function where x = [x  y  θ]ᵀ and
+    // u = [v  ω]ᵀ.
+    //
+    //           [v cosθ]
+    // f(x, u) = [v sinθ]
+    //           [  ω   ]
+    //
+    // To create an LQR, we need to linearize this.
+    //
+    //               [0  0  −v sinθ]                  [cosθ  0]
+    // ∂f(x, u)/∂x = [0  0   v cosθ]    ∂f(x, u)/∂u = [sinθ  0]
+    //               [0  0     0   ]                  [ 0    1]
+    //
+    // We're going to make a cross-track error controller, so we'll apply a
+    // clockwise rotation matrix to the global tracking error to transform it
+    // into the robot's coordinate frame. Since the cross-track error is always
+    // measured from the robot's coordinate frame, the model used to compute the
+    // LQR should be linearized around θ = 0 at all times.
+    //
+    //     [0  0  −v sin0]        [cos0  0]
+    // A = [0  0   v cos0]    B = [sin0  0]
+    //     [0  0     0   ]        [ 0    1]
+    //
+    //     [0  0  0]              [1  0]
+    // A = [0  0  v]          B = [0  0]
+    //     [0  0  0]              [0  1]
+    var A = new Matrix<>(Nat.N3(), Nat.N3());
+    var B = new MatBuilder<>(Nat.N3(), Nat.N2()).fill(1.0, 0.0, 0.0, 0.0, 0.0, 1.0);
+    var Q = StateSpaceUtil.makeCostMatrix(qelems);
+    var R = StateSpaceUtil.makeCostMatrix(relems);
+
+    for (double velocity = -maxVelocity; velocity < maxVelocity; 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.N3()));
+      } 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());
+      }
+    }
+  }
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   *
+   * @return True if the pose error is within tolerance of the reference.
+   */
+  public boolean atReference() {
+    final var eTranslate = m_poseError.getTranslation();
+    final var eRotate = m_poseError.getRotation();
+    final var tolTranslate = m_poseTolerance.getTranslation();
+    final var tolRotate = m_poseTolerance.getRotation();
+    return Math.abs(eTranslate.getX()) < tolTranslate.getX()
+        && Math.abs(eTranslate.getY()) < tolTranslate.getY()
+        && Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
+  }
+
+  /**
+   * Sets the pose error which is considered tolerable for use with atReference().
+   *
+   * @param poseTolerance Pose error which is tolerable.
+   */
+  public void setTolerance(Pose2d poseTolerance) {
+    m_poseTolerance = poseTolerance;
+  }
+
+  /**
+   * Returns the linear and angular velocity outputs of the LTV controller.
+   *
+   * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
+   * trajectory.
+   *
+   * @param currentPose The current pose.
+   * @param poseRef The desired pose.
+   * @param linearVelocityRef The desired linear velocity in meters per second.
+   * @param angularVelocityRef The desired angular velocity in radians per second.
+   * @return The linear and angular velocity outputs of the LTV controller.
+   */
+  public ChassisSpeeds calculate(
+      Pose2d currentPose, Pose2d poseRef, double linearVelocityRef, double angularVelocityRef) {
+    if (!m_enabled) {
+      return new ChassisSpeeds(linearVelocityRef, 0.0, angularVelocityRef);
+    }
+
+    m_poseError = poseRef.relativeTo(currentPose);
+
+    var K = m_table.get(linearVelocityRef);
+    var e =
+        new MatBuilder<>(Nat.N3(), Nat.N1())
+            .fill(m_poseError.getX(), m_poseError.getY(), m_poseError.getRotation().getRadians());
+    var u = K.times(e);
+
+    return new ChassisSpeeds(
+        linearVelocityRef + u.get(0, 0), 0.0, angularVelocityRef + u.get(1, 0));
+  }
+
+  /**
+   * Returns the next output of the LTV controller.
+   *
+   * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
+   * trajectory.
+   *
+   * @param currentPose The current pose.
+   * @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory.
+   * @return The linear and angular velocity outputs of the LTV controller.
+   */
+  public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
+    return calculate(
+        currentPose,
+        desiredState.poseMeters,
+        desiredState.velocityMetersPerSecond,
+        desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter);
+  }
+
+  /**
+   * Enables and disables the controller for troubleshooting purposes.
+   *
+   * @param enabled If the controller is enabled or not.
+   */
+  public void setEnabled(boolean enabled) {
+    m_enabled = enabled;
+  }
+}
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 627c272..a762851 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
@@ -20,20 +20,16 @@
  * <p>For more on the underlying math, read
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
  */
-@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
 public class LinearPlantInversionFeedforward<
     States extends Num, Inputs extends Num, Outputs extends Num> {
   /** The current reference state. */
-  @SuppressWarnings("MemberName")
   private Matrix<States, N1> m_r;
 
   /** The computed feedforward. */
   private Matrix<Inputs, N1> m_uff;
 
-  @SuppressWarnings("MemberName")
   private final Matrix<States, Inputs> m_B;
 
-  @SuppressWarnings("MemberName")
   private final Matrix<States, States> m_A;
 
   /**
@@ -54,7 +50,6 @@
    * @param B Continuous input matrix of the plant being controlled.
    * @param dtSeconds Discretization timestep.
    */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
   public LinearPlantInversionFeedforward(
       Matrix<States, States> A, Matrix<States, Inputs> B, double dtSeconds) {
     var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
@@ -143,7 +138,6 @@
    * @param nextR The reference state of the future timestep (k + dt).
    * @return The calculated feedforward.
    */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
   public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
     m_uff = new Matrix<>(m_B.solve(nextR.minus(m_A.times(r))));
 
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 e244c5f..dce1748 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
@@ -7,7 +7,6 @@
 import edu.wpi.first.math.Drake;
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.StateSpaceUtil;
 import edu.wpi.first.math.Vector;
@@ -23,18 +22,14 @@
  * <p>For more on the underlying math, read
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
  */
-@SuppressWarnings("ClassTypeParameterName")
 public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Outputs extends Num> {
   /** The current reference state. */
-  @SuppressWarnings("MemberName")
   private Matrix<States, N1> m_r;
 
   /** The computed and capped controller output. */
-  @SuppressWarnings("MemberName")
   private Matrix<Inputs, N1> m_u;
 
   // Controller gain.
-  @SuppressWarnings("MemberName")
   private Matrix<Inputs, States> m_K;
 
   /**
@@ -44,6 +39,7 @@
    * @param qelms The maximum desired error tolerance for each state.
    * @param relms The maximum desired control effort for each input.
    * @param dtSeconds Discretization timestep.
+   * @throws IllegalArgumentException If the system is uncontrollable.
    */
   public LinearQuadraticRegulator(
       LinearSystem<States, Inputs, Outputs> plant,
@@ -66,8 +62,8 @@
    * @param qelms The maximum desired error tolerance for each state.
    * @param relms The maximum desired control effort for each input.
    * @param dtSeconds Discretization timestep.
+   * @throws IllegalArgumentException If the system is uncontrollable.
    */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
   public LinearQuadraticRegulator(
       Matrix<States, States> A,
       Matrix<States, Inputs> B,
@@ -90,8 +86,8 @@
    * @param Q The state cost matrix.
    * @param R The input cost matrix.
    * @param dtSeconds Discretization timestep.
+   * @throws IllegalArgumentException If the system is uncontrollable.
    */
-  @SuppressWarnings({"LocalVariableName", "ParameterName"})
   public LinearQuadraticRegulator(
       Matrix<States, States> A,
       Matrix<States, Inputs> B,
@@ -141,8 +137,8 @@
    * @param R The input cost matrix.
    * @param N The state-input cross-term cost matrix.
    * @param dtSeconds Discretization timestep.
+   * @throws IllegalArgumentException If the system is uncontrollable.
    */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
   public LinearQuadraticRegulator(
       Matrix<States, States> A,
       Matrix<States, Inputs> B,
@@ -172,24 +168,6 @@
   }
 
   /**
-   * Constructs a controller with the given coefficients and plant.
-   *
-   * @param states The number of states.
-   * @param inputs The number of inputs.
-   * @param k The gain matrix.
-   */
-  @SuppressWarnings("ParameterName")
-  public LinearQuadraticRegulator(
-      Nat<States> states, Nat<Inputs> inputs, Matrix<Inputs, States> k) {
-    m_K = k;
-
-    m_r = new Matrix<>(states, Nat.N1());
-    m_u = new Matrix<>(inputs, Nat.N1());
-
-    reset();
-  }
-
-  /**
    * Returns the control input vector u.
    *
    * @return The control input.
@@ -248,7 +226,6 @@
    * @param x The current state x.
    * @return The next controller output.
    */
-  @SuppressWarnings("ParameterName")
   public Matrix<Inputs, N1> calculate(Matrix<States, N1> x) {
     m_u = m_K.times(m_r.minus(x));
     return m_u;
@@ -261,7 +238,6 @@
    * @param nextR the next reference vector r.
    * @return The next controller output.
    */
-  @SuppressWarnings("ParameterName")
   public Matrix<Inputs, N1> calculate(Matrix<States, N1> x, Matrix<States, N1> nextR) {
     m_r = nextR;
     return calculate(x);
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 12c4175..5e82909 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
@@ -35,7 +35,7 @@
 
   private double m_minimumInput;
 
-  // Do the endpoints wrap around? eg. Absolute encoder
+  // Do the endpoints wrap around? e.g. Absolute encoder
   private boolean m_continuous;
 
   // The error at the time of the most recent call to calculate()
@@ -175,12 +175,39 @@
   }
 
   /**
+   * Returns the position tolerance of this controller.
+   *
+   * @return the position tolerance of the controller.
+   */
+  public double getPositionTolerance() {
+    return m_positionTolerance;
+  }
+
+  /**
+   * Returns the velocity tolerance of this controller.
+   *
+   * @return the velocity tolerance of the controller.
+   */
+  public double getVelocityTolerance() {
+    return m_velocityTolerance;
+  }
+
+  /**
    * Sets the setpoint for the PIDController.
    *
    * @param setpoint The desired setpoint.
    */
   public void setSetpoint(double setpoint) {
     m_setpoint = setpoint;
+
+    if (m_continuous) {
+      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+      m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+    } else {
+      m_positionError = m_setpoint - m_measurement;
+    }
+
+    m_velocityError = (m_positionError - m_prevError) / m_period;
   }
 
   /**
@@ -200,18 +227,8 @@
    * @return Whether the error is within the acceptable bounds.
    */
   public boolean atSetpoint() {
-    double positionError;
-    if (m_continuous) {
-      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
-      positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
-    } else {
-      positionError = m_setpoint - m_measurement;
-    }
-
-    double velocityError = (positionError - m_prevError) / m_period;
-
-    return Math.abs(positionError) < m_positionTolerance
-        && Math.abs(velocityError) < m_velocityTolerance;
+    return Math.abs(m_positionError) < m_positionTolerance
+        && Math.abs(m_velocityError) < m_velocityTolerance;
   }
 
   /**
@@ -303,8 +320,7 @@
    * @return The next controller output.
    */
   public double calculate(double measurement, double setpoint) {
-    // Set setpoint to provided value
-    setSetpoint(setpoint);
+    m_setpoint = setpoint;
     return calculate(measurement);
   }
 
@@ -322,7 +338,7 @@
       double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
       m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
     } else {
-      m_positionError = m_setpoint - measurement;
+      m_positionError = m_setpoint - m_measurement;
     }
 
     m_velocityError = (m_positionError - m_prevError) / m_period;
@@ -340,8 +356,10 @@
 
   /** Resets the previous error and the integral term. */
   public void reset() {
+    m_positionError = 0;
     m_prevError = 0;
     m_totalError = 0;
+    m_velocityError = 0;
   }
 
   @Override
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 3ebcbd8..02cc17d 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
@@ -10,6 +10,7 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
 /**
  * Implements a PID control loop whose setpoint is constrained by a trapezoid profile. Users should
@@ -33,7 +34,6 @@
    * @param Kd The derivative coefficient.
    * @param constraints Velocity and acceleration constraints for goal.
    */
-  @SuppressWarnings("ParameterName")
   public ProfiledPIDController(
       double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints) {
     this(Kp, Ki, Kd, constraints, 0.02);
@@ -48,12 +48,13 @@
    * @param constraints Velocity and acceleration constraints for goal.
    * @param period The period between controller updates in seconds. The default is 0.02 seconds.
    */
-  @SuppressWarnings("ParameterName")
   public ProfiledPIDController(
       double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints, double period) {
     m_controller = new PIDController(Kp, Ki, Kd, period);
     m_constraints = constraints;
     instances++;
+
+    SendableRegistry.add(this, "ProfiledPIDController", instances);
     MathSharedStore.reportUsage(MathUsageId.kController_ProfiledPIDController, instances);
   }
 
@@ -66,7 +67,6 @@
    * @param Ki Integral coefficient
    * @param Kd Differential coefficient
    */
-  @SuppressWarnings("ParameterName")
   public void setPID(double Kp, double Ki, double Kd) {
     m_controller.setPID(Kp, Ki, Kd);
   }
@@ -76,7 +76,6 @@
    *
    * @param Kp proportional coefficient
    */
-  @SuppressWarnings("ParameterName")
   public void setP(double Kp) {
     m_controller.setP(Kp);
   }
@@ -86,7 +85,6 @@
    *
    * @param Ki integral coefficient
    */
-  @SuppressWarnings("ParameterName")
   public void setI(double Ki) {
     m_controller.setI(Ki);
   }
@@ -96,7 +94,6 @@
    *
    * @param Kd differential coefficient
    */
-  @SuppressWarnings("ParameterName")
   public void setD(double Kd) {
     m_controller.setD(Kd);
   }
@@ -138,6 +135,24 @@
   }
 
   /**
+   * Returns the position tolerance of this controller.
+   *
+   * @return the position tolerance of the controller.
+   */
+  public double getPositionTolerance() {
+    return m_controller.getPositionTolerance();
+  }
+
+  /**
+   * Returns the velocity tolerance of this controller.
+   *
+   * @return the velocity tolerance of the controller.
+   */
+  public double getVelocityTolerance() {
+    return m_controller.getVelocityTolerance();
+  }
+
+  /**
    * Sets the goal for the ProfiledPIDController.
    *
    * @param goal The desired goal state.
@@ -282,7 +297,7 @@
    */
   public double calculate(double measurement) {
     if (m_controller.isContinuousInputEnabled()) {
-      // Get error which is smallest distance between goal and measurement
+      // Get error which is the smallest distance between goal and measurement
       double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
       double goalMinDistance =
           MathUtil.inputModulus(m_goal.position - measurement, -errorBound, errorBound);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java
index a1d6237..5683987 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java
@@ -33,10 +33,8 @@
  * derivation and analysis.
  */
 public class RamseteController {
-  @SuppressWarnings("MemberName")
   private final double m_b;
 
-  @SuppressWarnings("MemberName")
   private final double m_zeta;
 
   private Pose2d m_poseError = new Pose2d();
@@ -51,7 +49,6 @@
    * @param zeta Tuning parameter (0 rad⁻¹ &lt; zeta &lt; 1 rad⁻¹) for which larger values provide
    *     more damping in response.
    */
-  @SuppressWarnings("ParameterName")
   public RamseteController(double b, double zeta) {
     m_b = b;
     m_zeta = zeta;
@@ -101,7 +98,6 @@
    * @param angularVelocityRefRadiansPerSecond The desired angular velocity in radians per second.
    * @return The next controller output.
    */
-  @SuppressWarnings("LocalVariableName")
   public ChassisSpeeds calculate(
       Pose2d currentPose,
       Pose2d poseRef,
@@ -120,8 +116,11 @@
     final double vRef = linearVelocityRefMeters;
     final double omegaRef = angularVelocityRefRadiansPerSecond;
 
+    // k = 2ζ√(ω_ref² + b v_ref²)
     double k = 2.0 * m_zeta * Math.sqrt(Math.pow(omegaRef, 2) + m_b * Math.pow(vRef, 2));
 
+    // v_cmd = v_ref cos(e_θ) + k e_x
+    // ω_cmd = ω_ref + k e_θ + b v_ref sinc(e_θ) e_y
     return new ChassisSpeeds(
         vRef * m_poseError.getRotation().getCos() + k * eX,
         0.0,
@@ -138,7 +137,6 @@
    * @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory.
    * @return The next controller output.
    */
-  @SuppressWarnings("LocalVariableName")
   public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
     return calculate(
         currentPose,
@@ -161,7 +159,6 @@
    *
    * @param x Value of which to take sinc(x).
    */
-  @SuppressWarnings("ParameterName")
   private static double sinc(double x) {
     if (Math.abs(x) < 1e-9) {
       return 1.0 - 1.0 / 6.0 * x * x;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
index a1e5f68..714d705 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
@@ -9,7 +9,6 @@
 import edu.wpi.first.math.system.plant.LinearSystemId;
 
 /** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */
-@SuppressWarnings("MemberName")
 public class SimpleMotorFeedforward {
   public final double ks;
   public final double kv;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java
index a6d8b44..0f91a1d 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java
@@ -87,7 +87,6 @@
    * @param angleStateIdx The row containing the angles.
    * @return Mean of sigma points.
    */
-  @SuppressWarnings("checkstyle:ParameterName")
   public static <S extends Num> Matrix<S, N1> angleMean(
       Matrix<S, ?> sigmas, Matrix<?, N1> Wm, int angleStateIdx) {
     double[] angleSigmas = sigmas.extractRowVector(angleStateIdx).getData();
@@ -115,7 +114,6 @@
    * @param angleStateIdx The row containing the angles.
    * @return Function returning mean of sigma points.
    */
-  @SuppressWarnings("LambdaParameterName")
   public static <S extends Num> BiFunction<Matrix<S, ?>, Matrix<?, N1>, Matrix<S, N1>> angleMean(
       int angleStateIdx) {
     return (sigmas, Wm) -> angleMean(sigmas, Wm, angleStateIdx);
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 b199d4d..b5fe591 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,158 +4,109 @@
 
 package edu.wpi.first.math.estimator;
 
-import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.StateSpaceUtil;
 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.kinematics.DifferentialDriveWheelSpeeds;
+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.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.math.numbers.N5;
 import edu.wpi.first.util.WPIUtilJNI;
-import java.util.function.BiConsumer;
+import java.util.Map;
+import java.util.Objects;
 
 /**
- * This class wraps an {@link edu.wpi.first.math.estimator.UnscentedKalmanFilter Unscented Kalman
- * Filter} to fuse latency-compensated vision measurements with differential drive encoder
- * measurements. It will correct for noisy vision measurements and encoder drift. It is intended to
- * be an easy drop-in for {@link edu.wpi.first.math.kinematics.DifferentialDriveOdometry}; in fact,
- * if you never call {@link DifferentialDrivePoseEstimator#addVisionMeasurement} and only call
- * {@link DifferentialDrivePoseEstimator#update} then this will behave exactly the same as
+ * This class wraps {@link DifferentialDriveOdometry Differential Drive Odometry} to fuse
+ * latency-compensated vision measurements with differential drive encoder measurements. It is
+ * intended to be a drop-in replacement for {@link DifferentialDriveOdometry}; in fact, if you never
+ * call {@link DifferentialDrivePoseEstimator#addVisionMeasurement} and only call {@link
+ * DifferentialDrivePoseEstimator#update} then this will behave exactly the same as
  * DifferentialDriveOdometry.
  *
- * <p>{@link DifferentialDrivePoseEstimator#update} should be called every robot loop (if your robot
- * loops are faster than the default then you should change the {@link
- * DifferentialDrivePoseEstimator#DifferentialDrivePoseEstimator(Rotation2d, Pose2d, Matrix, Matrix,
- * Matrix, double) nominal delta time}.) {@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.
+ * <p>{@link DifferentialDrivePoseEstimator#update} should be called every robot loop.
  *
- * <p>The state-space system used internally has the following states (x), inputs (u), and outputs
- * (y):
- *
- * <p><strong> x = [x, y, theta, dist_l, dist_r]ᵀ </strong> in the field coordinate system
- * containing x position, y position, heading, left encoder distance, and right encoder distance.
- *
- * <p><strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity, and angular rate
- * in the field coordinate system.
- *
- * <p>NB: Using velocities make things considerably easier, because it means that teams don't have
- * to worry about getting an accurate model. Basically, we suspect that it's easier for teams to get
- * good encoder data than it is for them to perform system identification well enough to get a good
- * model.
- *
- * <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
- * heading; or <strong>y = [dist_l, dist_r, theta] </strong> containing left encoder position, right
- * encoder position, and gyro heading.
+ * <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 {
-  final UnscentedKalmanFilter<N5, N3, N3> m_observer; // Package-private to allow for unit testing
-  private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
-  private final KalmanFilterLatencyCompensator<N5, N3, N3> m_latencyCompensator;
+  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 double m_nominalDt; // Seconds
-  private double m_prevTimeSeconds = -1.0;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
-
-  private Matrix<N3, N3> m_visionContR;
+  private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
+      TimeInterpolatableBuffer.createBuffer(1.5);
 
   /**
-   * Constructs a DifferentialDrivePoseEstimator.
+   * Constructs a DifferentialDrivePoseEstimator with default standard deviations for the model and
+   * vision measurements.
    *
+   * <p>The default standard deviations of the model states are 0.02 meters for x, 0.02 meters for
+   * y, and 0.01 radians for heading. The default standard deviations of the vision measurements are
+   * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
    * @param gyroAngle The current gyro angle.
+   * @param leftDistanceMeters The distance traveled by the left encoder.
+   * @param rightDistanceMeters The distance traveled by the right encoder.
    * @param initialPoseMeters The starting pose estimate.
-   * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
-   *     model's state estimates less. This matrix is in the form [x, y, theta, dist_l, dist_r]ᵀ,
-   *     with units in meters and radians.
-   * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
-   *     Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
-   *     is in the form [dist_l, dist_r, theta]ᵀ, with units in meters and radians.
-   * @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 DifferentialDrivePoseEstimator(
+      DifferentialDriveKinematics kinematics,
       Rotation2d gyroAngle,
-      Pose2d initialPoseMeters,
-      Matrix<N5, N1> stateStdDevs,
-      Matrix<N3, N1> localMeasurementStdDevs,
-      Matrix<N3, N1> visionMeasurementStdDevs) {
+      double leftDistanceMeters,
+      double rightDistanceMeters,
+      Pose2d initialPoseMeters) {
     this(
+        kinematics,
         gyroAngle,
+        leftDistanceMeters,
+        rightDistanceMeters,
         initialPoseMeters,
-        stateStdDevs,
-        localMeasurementStdDevs,
-        visionMeasurementStdDevs,
-        0.02);
+        VecBuilder.fill(0.02, 0.02, 0.01),
+        VecBuilder.fill(0.1, 0.1, 0.1));
   }
 
   /**
    * Constructs a DifferentialDrivePoseEstimator.
    *
-   * @param gyroAngle The current gyro angle.
-   * @param initialPoseMeters The starting pose estimate.
-   * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
-   *     model's state estimates less. This matrix is in the form [x, y, theta, dist_l, dist_r]ᵀ,
-   *     with units in meters and radians.
-   * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
-   *     Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
-   *     is in the form [dist_l, dist_r, theta]ᵀ, with units in meters and radians.
-   * @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.
-   * @param nominalDtSeconds The time in seconds between each robot loop.
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param gyroAngle The gyro angle of the robot.
+   * @param leftDistanceMeters The distance traveled by the left encoder.
+   * @param rightDistanceMeters The distance traveled by the right encoder.
+   * @param initialPoseMeters The estimated initial pose.
+   * @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.
    */
-  @SuppressWarnings("ParameterName")
   public DifferentialDrivePoseEstimator(
+      DifferentialDriveKinematics kinematics,
       Rotation2d gyroAngle,
+      double leftDistanceMeters,
+      double rightDistanceMeters,
       Pose2d initialPoseMeters,
-      Matrix<N5, N1> stateStdDevs,
-      Matrix<N3, N1> localMeasurementStdDevs,
-      Matrix<N3, N1> visionMeasurementStdDevs,
-      double nominalDtSeconds) {
-    m_nominalDt = nominalDtSeconds;
+      Matrix<N3, N1> stateStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    m_kinematics = kinematics;
+    m_odometry =
+        new DifferentialDriveOdometry(
+            gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters);
 
-    m_observer =
-        new UnscentedKalmanFilter<>(
-            Nat.N5(),
-            Nat.N3(),
-            this::f,
-            (x, u) -> VecBuilder.fill(x.get(3, 0), x.get(4, 0), x.get(2, 0)),
-            stateStdDevs,
-            localMeasurementStdDevs,
-            AngleStatistics.angleMean(2),
-            AngleStatistics.angleMean(2),
-            AngleStatistics.angleResidual(2),
-            AngleStatistics.angleResidual(2),
-            AngleStatistics.angleAdd(2),
-            m_nominalDt);
-    m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+    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);
-
-    m_visionCorrect =
-        (u, y) ->
-            m_observer.correct(
-                Nat.N3(),
-                u,
-                y,
-                (x, u1) -> new Matrix<>(x.getStorage().extractMatrix(0, 3, 0, 1)),
-                m_visionContR,
-                AngleStatistics.angleMean(2),
-                AngleStatistics.angleResidual(2),
-                AngleStatistics.angleResidual(2),
-                AngleStatistics.angleAdd(2));
-
-    m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPoseMeters.getRotation();
-    m_observer.setXhat(fillStateVector(initialPoseMeters, 0.0, 0.0));
   }
 
   /**
@@ -168,109 +119,122 @@
    *     theta]ᵀ, with units in meters and radians.
    */
   public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
-    m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
-  }
+    var r = new double[3];
+    for (int i = 0; i < 3; ++i) {
+      r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
+    }
 
-  @SuppressWarnings({"ParameterName", "MethodName"})
-  private Matrix<N5, N1> f(Matrix<N5, N1> x, Matrix<N3, N1> u) {
-    // Apply a rotation matrix. Note that we do *not* add x--Runge-Kutta does that for us.
-    var theta = x.get(2, 0);
-    var toFieldRotation =
-        new MatBuilder<>(Nat.N5(), Nat.N5())
-            .fill(
-                Math.cos(theta),
-                -Math.sin(theta),
-                0,
-                0,
-                0,
-                Math.sin(theta),
-                Math.cos(theta),
-                0,
-                0,
-                0,
-                0,
-                0,
-                1,
-                0,
-                0,
-                0,
-                0,
-                0,
-                1,
-                0,
-                0,
-                0,
-                0,
-                0,
-                1);
-    return toFieldRotation.times(
-        VecBuilder.fill(u.get(0, 0), u.get(1, 0), u.get(2, 0), u.get(0, 0), u.get(1, 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>You NEED to reset your encoders (to zero) when calling this method.
-   *
    * <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 poseMeters The position on the field that your robot is at.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftPositionMeters The distance traveled by the left encoder.
+   * @param rightPositionMeters The distance traveled by the right encoder.
+   * @param poseMeters The position on the field that your robot is at.
    */
-  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+  public void resetPosition(
+      Rotation2d gyroAngle,
+      double leftPositionMeters,
+      double rightPositionMeters,
+      Pose2d poseMeters) {
     // Reset state estimate and error covariance
-    m_observer.reset();
-    m_latencyCompensator.reset();
-
-    m_observer.setXhat(fillStateVector(poseMeters, 0.0, 0.0));
-
-    m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
-    m_previousAngle = poseMeters.getRotation();
+    m_odometry.resetPosition(gyroAngle, leftPositionMeters, rightPositionMeters, poseMeters);
+    m_poseBuffer.clear();
   }
 
   /**
-   * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
+   * Gets the estimated robot pose.
    *
    * @return The estimated robot pose in meters.
    */
   public Pose2d getEstimatedPosition() {
-    return new Pose2d(
-        m_observer.getXhat(0), m_observer.getXhat(1), new Rotation2d(m_observer.getXhat(2)));
+    return m_odometry.getPoseMeters();
   }
 
   /**
-   * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
-   * estimate while still accounting for measurement noise.
+   * 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} then you must use a timestamp with an epoch
-   *     since FPGA startup (i.e. the epoch of this timestamp is the same epoch as
-   *     Timer.getFPGATimestamp.) This means that you should use Timer.getFPGATimestamp as your time
-   *     source in this case.
+   *     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) {
-    m_latencyCompensator.applyPastGlobalMeasurement(
-        Nat.N3(),
-        m_observer,
-        m_nominalDt,
-        StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
-        m_visionCorrect,
-        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);
+    }
   }
 
   /**
-   * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
-   * estimate while still accounting for measurement noise.
+   * 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.
@@ -278,13 +242,14 @@
    * @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} then you must use a timestamp with an epoch
-   *     since FPGA startup (i.e. the epoch of this timestamp is the same epoch as
-   *     Timer.getFPGATimestamp.) This means that you should use Timer.getFPGATimestamp as your time
-   *     source in this case.
-   * @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.
+   *     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,
@@ -295,78 +260,127 @@
   }
 
   /**
-   * Updates the the Unscented Kalman Filter using only wheel encoder information. Note that this
-   * should be called every loop.
+   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+   * loop.
    *
    * @param gyroAngle The current gyro angle.
-   * @param wheelVelocitiesMetersPerSecond The velocities of the wheels in meters per second.
-   * @param distanceLeftMeters The total distance travelled by the left wheel in meters since the
-   *     last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
-   * @param distanceRightMeters The total distance travelled by the right wheel in meters since the
-   *     last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
+   * @param distanceLeftMeters The total distance travelled by the left wheel in meters.
+   * @param distanceRightMeters The total distance travelled by the right wheel in meters.
    * @return The estimated pose of the robot in meters.
    */
   public Pose2d update(
-      Rotation2d gyroAngle,
-      DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond,
-      double distanceLeftMeters,
-      double distanceRightMeters) {
+      Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters) {
     return updateWithTime(
-        WPIUtilJNI.now() * 1.0e-6,
-        gyroAngle,
-        wheelVelocitiesMetersPerSecond,
-        distanceLeftMeters,
-        distanceRightMeters);
+        WPIUtilJNI.now() * 1.0e-6, gyroAngle, distanceLeftMeters, distanceRightMeters);
   }
 
   /**
-   * Updates the the Unscented Kalman Filter using only wheel encoder information. Note that this
-   * should be called every loop.
+   * 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 wheelVelocitiesMetersPerSecond The velocities of the wheels in meters per second.
-   * @param distanceLeftMeters The total distance travelled by the left wheel in meters since the
-   *     last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
-   * @param distanceRightMeters The total distance travelled by the right wheel in meters since the
-   *     last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
+   * @param distanceLeftMeters The total distance travelled by the left wheel in meters.
+   * @param distanceRightMeters The total distance travelled by the right wheel in meters.
    * @return The estimated pose of the robot in meters.
    */
-  @SuppressWarnings({"LocalVariableName", "ParameterName"})
   public Pose2d updateWithTime(
       double currentTimeSeconds,
       Rotation2d gyroAngle,
-      DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond,
       double distanceLeftMeters,
       double distanceRightMeters) {
-    double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
-    m_prevTimeSeconds = currentTimeSeconds;
-
-    var angle = gyroAngle.plus(m_gyroOffset);
-    // Diff drive forward kinematics:
-    // v_c = (v_l + v_r) / 2
-    var wheelVels = wheelVelocitiesMetersPerSecond;
-    var u =
-        VecBuilder.fill(
-            (wheelVels.leftMetersPerSecond + wheelVels.rightMetersPerSecond) / 2,
-            0,
-            angle.minus(m_previousAngle).getRadians() / dt);
-    m_previousAngle = angle;
-
-    var localY = VecBuilder.fill(distanceLeftMeters, distanceRightMeters, angle.getRadians());
-    m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
-    m_observer.predict(u, dt);
-    m_observer.correct(u, localY);
+    m_odometry.update(gyroAngle, distanceLeftMeters, distanceRightMeters);
+    m_poseBuffer.addSample(
+        currentTimeSeconds,
+        new InterpolationRecord(
+            getEstimatedPosition(), gyroAngle, distanceLeftMeters, distanceRightMeters));
 
     return getEstimatedPosition();
   }
 
-  private static Matrix<N5, N1> fillStateVector(Pose2d pose, double leftDist, double rightDist) {
-    return VecBuilder.fill(
-        pose.getTranslation().getX(),
-        pose.getTranslation().getY(),
-        pose.getRotation().getRadians(),
-        leftDist,
-        rightDist);
+  /**
+   * 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);
+    }
   }
 }
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 d4f9e56..5f9e52e 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
@@ -34,16 +34,13 @@
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
  * theory".
  */
-@SuppressWarnings("ClassTypeParameterName")
 public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
     implements KalmanTypeFilter<States, Inputs, Outputs> {
   private final Nat<States> m_states;
   private final Nat<Outputs> m_outputs;
 
-  @SuppressWarnings("MemberName")
   private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
 
-  @SuppressWarnings("MemberName")
   private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> m_h;
 
   private BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> m_residualFuncY;
@@ -53,10 +50,8 @@
   private final Matrix<States, States> m_initP;
   private final Matrix<Outputs, Outputs> m_contR;
 
-  @SuppressWarnings("MemberName")
   private Matrix<States, N1> m_xHat;
 
-  @SuppressWarnings("MemberName")
   private Matrix<States, States> m_P;
 
   private double m_dtSeconds;
@@ -73,7 +68,6 @@
    * @param measurementStdDevs Standard deviations of measurements.
    * @param dtSeconds Nominal discretization timestep.
    */
-  @SuppressWarnings("ParameterName")
   public ExtendedKalmanFilter(
       Nat<States> states,
       Nat<Inputs> inputs,
@@ -111,7 +105,6 @@
    * @param addFuncX A function that adds two state vectors.
    * @param dtSeconds Nominal discretization timestep.
    */
-  @SuppressWarnings("ParameterName")
   public ExtendedKalmanFilter(
       Nat<States> states,
       Nat<Inputs> inputs,
@@ -133,7 +126,7 @@
     m_addFuncX = addFuncX;
 
     m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
-    this.m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+    m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
     m_dtSeconds = dtSeconds;
 
     reset();
@@ -207,7 +200,7 @@
    * Returns an element of the state estimate x-hat.
    *
    * @param row Row of x-hat.
-   * @return the value of the state estimate x-hat at i.
+   * @return the value of the state estimate x-hat at that row.
    */
   @Override
   public double getXhat(int row) {
@@ -219,7 +212,6 @@
    *
    * @param xHat The state estimate x-hat.
    */
-  @SuppressWarnings("ParameterName")
   @Override
   public void setXhat(Matrix<States, N1> xHat) {
     m_xHat = xHat;
@@ -248,7 +240,6 @@
    * @param u New control input from controller.
    * @param dtSeconds Timestep for prediction.
    */
-  @SuppressWarnings("ParameterName")
   @Override
   public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
     predict(u, m_f, dtSeconds);
@@ -258,10 +249,9 @@
    * Project the model into the future with a new control input u.
    *
    * @param u New control input from controller.
-   * @param f The function used to linearlize the model.
+   * @param f The function used to linearize the model.
    * @param dtSeconds Timestep for prediction.
    */
-  @SuppressWarnings("ParameterName")
   public void predict(
       Matrix<Inputs, N1> u,
       BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
@@ -288,7 +278,6 @@
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    */
-  @SuppressWarnings("ParameterName")
   @Override
   public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
     correct(m_outputs, u, y, m_h, m_contR, m_residualFuncY, m_addFuncX);
@@ -306,16 +295,15 @@
    * @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 Discrete measurement noise covariance matrix.
+   * @param contR Continuous measurement noise covariance matrix.
    */
-  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
   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);
+      Matrix<Rows, Rows> contR) {
+    correct(rows, u, y, h, contR, Matrix::minus, Matrix::plus);
   }
 
   /**
@@ -330,22 +318,21 @@
    * @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 Discrete measurement noise covariance matrix.
+   * @param contR 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.
    */
-  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
   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,
+      Matrix<Rows, Rows> contR,
       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(R, m_dtSeconds);
+    final var discR = Discretization.discretizeR(contR, m_dtSeconds);
 
     final var S = C.times(m_P).times(C.transpose()).plus(discR);
 
@@ -366,7 +353,13 @@
     // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
     m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, h.apply(m_xHat, u))));
 
-    // Pₖ₊₁⁺ = (I − KC)Pₖ₊₁⁻
-    m_P = Matrix.eye(m_states).minus(K.times(C)).times(m_P);
+    // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
+    // Use Joseph form for numerical stability
+    m_P =
+        Matrix.eye(m_states)
+            .minus(K.times(C))
+            .times(m_P)
+            .times(Matrix.eye(m_states).minus(K.times(C)).transpose())
+            .plus(K.times(discR).times(K.transpose()));
   }
 }
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 c992d2f..24d6d91 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
@@ -29,18 +29,15 @@
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
  * theory".
  */
-@SuppressWarnings("ClassTypeParameterName")
 public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
   private final Nat<States> m_states;
 
   private final LinearSystem<States, Inputs, Outputs> m_plant;
 
   /** The steady-state Kalman gain matrix. */
-  @SuppressWarnings("MemberName")
   private final Matrix<States, Outputs> m_K;
 
   /** The state estimate. */
-  @SuppressWarnings("MemberName")
   private Matrix<States, N1> m_xHat;
 
   /**
@@ -52,8 +49,8 @@
    * @param stateStdDevs Standard deviations of model states.
    * @param measurementStdDevs Standard deviations of measurements.
    * @param dtSeconds Nominal discretization timestep.
+   * @throws IllegalArgumentException If the system is unobservable.
    */
-  @SuppressWarnings("LocalVariableName")
   public KalmanFilter(
       Nat<States> states,
       Nat<Outputs> outputs,
@@ -172,7 +169,7 @@
    * Returns an element of the state estimate x-hat.
    *
    * @param row Row of x-hat.
-   * @return the state estimate x-hat at i.
+   * @return the state estimate x-hat at that row.
    */
   public double getXhat(int row) {
     return m_xHat.get(row, 0);
@@ -184,7 +181,6 @@
    * @param u New control input from controller.
    * @param dtSeconds Timestep for prediction.
    */
-  @SuppressWarnings("ParameterName")
   public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
     this.m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds);
   }
@@ -195,7 +191,6 @@
    * @param u Same control input used in the last predict step.
    * @param y Measurement vector.
    */
-  @SuppressWarnings("ParameterName")
   public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
     final var C = m_plant.getC();
     final var D = m_plant.getD();
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
index 89f35e5..15e7bd4 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
@@ -33,9 +33,8 @@
    * @param observer The observer.
    * @param u The input at the timestamp.
    * @param localY The local output at the timestamp
-   * @param timestampSeconds The timesnap of the state.
+   * @param timestampSeconds The timestamp of the state.
    */
-  @SuppressWarnings("ParameterName")
   public void addObserverState(
       KalmanTypeFilter<S, I, O> observer,
       Matrix<I, N1> u,
@@ -60,7 +59,6 @@
    * @param globalMeasurementCorrect The function take calls correct() on the observer.
    * @param timestampSeconds The timestamp of the measurement.
    */
-  @SuppressWarnings("ParameterName")
   public <R extends Num> void applyPastGlobalMeasurement(
       Nat<R> rows,
       KalmanTypeFilter<S, I, O> observer,
@@ -119,7 +117,7 @@
 
       // Index of snapshot taken before the global measurement. Since we already
       // handled the case where the index points to the first snapshot, this
-      // computation is guaranteed to be nonnegative.
+      // computation is guaranteed to be non-negative.
       int prevIdx = nextIdx - 1;
 
       // Find the snapshot closest in time to global measurement
@@ -165,14 +163,12 @@
   }
 
   /** This class contains all the information about our observer at a given time. */
-  @SuppressWarnings("MemberName")
   public class ObserverSnapshot {
     public final Matrix<S, N1> xHat;
     public final Matrix<S, S> errorCovariances;
     public final Matrix<I, N1> inputs;
     public final Matrix<O, N1> localMeasurements;
 
-    @SuppressWarnings("ParameterName")
     private ObserverSnapshot(
         KalmanTypeFilter<S, I, O> observer, Matrix<I, N1> u, Matrix<O, N1> localY) {
       this.xHat = observer.getXhat();
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 3fd3957..7a100f5 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,6 @@
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.numbers.N1;
 
-@SuppressWarnings({"ParameterName", "InterfaceTypeParameterName"})
 interface KalmanTypeFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
   Matrix<States, States> getP();
 
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 d6f1075..448b8d3 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,157 +4,101 @@
 
 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.StateSpaceUtil;
 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.Translation2d;
+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.MecanumDriveWheelSpeeds;
+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.function.BiConsumer;
+import java.util.Map;
+import java.util.Objects;
 
 /**
- * This class wraps an {@link UnscentedKalmanFilter Unscented Kalman Filter} to fuse
- * latency-compensated vision measurements with mecanum drive encoder velocity measurements. It will
- * correct for noisy measurements and encoder drift. It is intended to be an easy but more accurate
- * drop-in for {@link edu.wpi.first.math.kinematics.MecanumDriveOdometry}.
+ * This class wraps {@link MecanumDriveOdometry Mecanum Drive Odometry} to fuse latency-compensated
+ * vision measurements with mecanum drive encoder distance measurements. It will correct for noisy
+ * measurements and encoder drift. It is intended to be a drop-in replacement for {@link
+ * edu.wpi.first.math.kinematics.MecanumDriveOdometry}.
  *
- * <p>{@link MecanumDrivePoseEstimator#update} should be called every robot loop. If your loops are
- * faster or slower than the default of 0.02s, then you should change the nominal delta time using
- * the secondary constructor: {@link MecanumDrivePoseEstimator#MecanumDrivePoseEstimator(Rotation2d,
- * Pose2d, MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}.
+ * <p>{@link MecanumDrivePoseEstimator#update} should be called every robot loop.
  *
  * <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.
- *
- * <p>The state-space system used internally has the following states (x), inputs (u), and outputs
- * (y):
- *
- * <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
- * position, and heading.
- *
- * <p><strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity, and angular rate
- * in the field coordinate system.
- *
- * <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
- * heading; or <strong> y = [theta]ᵀ </strong> containing gyro heading.
  */
 public class MecanumDrivePoseEstimator {
-  private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
   private final MecanumDriveKinematics m_kinematics;
-  private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
-  private final KalmanFilterLatencyCompensator<N3, N3, N1> m_latencyCompensator;
+  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 double m_nominalDt; // Seconds
-  private double m_prevTimeSeconds = -1.0;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
-
-  private Matrix<N3, N3> m_visionContR;
+  private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
+      TimeInterpolatableBuffer.createBuffer(1.5);
 
   /**
-   * Constructs a MecanumDrivePoseEstimator.
+   * Constructs a MecanumDrivePoseEstimator with default standard deviations for the model and
+   * vision measurements.
    *
-   * @param gyroAngle The current gyro angle.
-   * @param initialPoseMeters The starting pose estimate.
+   * <p>The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
+   * and 0.1 radians for heading. The default standard deviations of the vision measurements are
+   * 0.45 meters for x, 0.45 meters for y, and 0.45 radians for heading.
+   *
    * @param kinematics A correctly-configured kinematics object for your drivetrain.
-   * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
-   *     model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
-   *     meters and radians.
-   * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
-   *     Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
-   *     is in the form [theta], with units in radians.
-   * @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.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distances driven by each wheel.
+   * @param initialPoseMeters The starting pose estimate.
    */
   public MecanumDrivePoseEstimator(
-      Rotation2d gyroAngle,
-      Pose2d initialPoseMeters,
       MecanumDriveKinematics kinematics,
-      Matrix<N3, N1> stateStdDevs,
-      Matrix<N1, N1> localMeasurementStdDevs,
-      Matrix<N3, N1> visionMeasurementStdDevs) {
+      Rotation2d gyroAngle,
+      MecanumDriveWheelPositions wheelPositions,
+      Pose2d initialPoseMeters) {
     this(
-        gyroAngle,
-        initialPoseMeters,
         kinematics,
-        stateStdDevs,
-        localMeasurementStdDevs,
-        visionMeasurementStdDevs,
-        0.02);
+        gyroAngle,
+        wheelPositions,
+        initialPoseMeters,
+        VecBuilder.fill(0.1, 0.1, 0.1),
+        VecBuilder.fill(0.45, 0.45, 0.45));
   }
 
   /**
    * Constructs a MecanumDrivePoseEstimator.
    *
-   * @param gyroAngle The current gyro angle.
-   * @param initialPoseMeters The starting pose estimate.
    * @param kinematics A correctly-configured kinematics object for your drivetrain.
-   * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
-   *     model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
-   *     meters and radians.
-   * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
-   *     Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
-   *     is in the form [theta], with units in radians.
-   * @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.
-   * @param nominalDtSeconds The time in seconds between each robot loop.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distance measured by each wheel.
+   * @param initialPoseMeters The starting pose estimate.
+   * @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.
    */
-  @SuppressWarnings("ParameterName")
   public MecanumDrivePoseEstimator(
-      Rotation2d gyroAngle,
-      Pose2d initialPoseMeters,
       MecanumDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      MecanumDriveWheelPositions wheelPositions,
+      Pose2d initialPoseMeters,
       Matrix<N3, N1> stateStdDevs,
-      Matrix<N1, N1> localMeasurementStdDevs,
-      Matrix<N3, N1> visionMeasurementStdDevs,
-      double nominalDtSeconds) {
-    m_nominalDt = nominalDtSeconds;
-
-    m_observer =
-        new UnscentedKalmanFilter<>(
-            Nat.N3(),
-            Nat.N1(),
-            (x, u) -> u,
-            (x, u) -> x.extractRowVector(2),
-            stateStdDevs,
-            localMeasurementStdDevs,
-            AngleStatistics.angleMean(2),
-            AngleStatistics.angleMean(0),
-            AngleStatistics.angleResidual(2),
-            AngleStatistics.angleResidual(0),
-            AngleStatistics.angleAdd(2),
-            m_nominalDt);
+      Matrix<N3, N1> visionMeasurementStdDevs) {
     m_kinematics = kinematics;
-    m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+    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);
-
-    m_visionCorrect =
-        (u, y) ->
-            m_observer.correct(
-                Nat.N3(),
-                u,
-                y,
-                (x, u1) -> x,
-                m_visionContR,
-                AngleStatistics.angleMean(2),
-                AngleStatistics.angleResidual(2),
-                AngleStatistics.angleResidual(2),
-                AngleStatistics.angleAdd(2));
-
-    m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPoseMeters.getRotation();
-    m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters));
   }
 
   /**
@@ -167,85 +111,128 @@
    *     theta]ᵀ, with units in meters and radians.
    */
   public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
-    m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), 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>You NEED to reset your encoders (to zero) when calling this method.
-   *
    * <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 poseMeters The position on the field that your robot is at.
    * @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(Pose2d poseMeters, Rotation2d gyroAngle) {
+  public void resetPosition(
+      Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
     // Reset state estimate and error covariance
-    m_observer.reset();
-    m_latencyCompensator.reset();
-
-    m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
-
-    m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
-    m_previousAngle = poseMeters.getRotation();
+    m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters);
+    m_poseBuffer.clear();
   }
 
   /**
-   * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
+   * Gets the estimated robot pose.
    *
    * @return The estimated robot pose in meters.
    */
   public Pose2d getEstimatedPosition() {
-    return new Pose2d(
-        m_observer.getXhat(0), m_observer.getXhat(1), new Rotation2d(m_observer.getXhat(2)));
+    return m_odometry.getPoseMeters();
   }
 
   /**
-   * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
-   * estimate while still accounting for measurement noise.
+   * 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}
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this
-   *     timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use
-   *     Timer.getFPGATimestamp as your time source or sync the epochs.
+   *     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) {
-    m_latencyCompensator.applyPastGlobalMeasurement(
-        Nat.N3(),
-        m_observer,
-        m_nominalDt,
-        StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
-        m_visionCorrect,
-        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);
+    }
   }
 
   /**
-   * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
-   * estimate while still accounting for measurement noise.
+   * 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}
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this
-   *     timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use
-   *     Timer.getFPGATimestamp as your time source in this case.
-   * @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.
+   *     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,
@@ -256,50 +243,141 @@
   }
 
   /**
-   * Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
-   * called every loop, and the correct loop period must be passed into the constructor of this
-   * class.
+   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+   * loop.
    *
    * @param gyroAngle The current gyro angle.
-   * @param wheelSpeeds The current speeds of the mecanum drive wheels.
+   * @param wheelPositions The distances driven by each wheel.
    * @return The estimated pose of the robot in meters.
    */
-  public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
-    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds);
+  public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
+    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelPositions);
   }
 
   /**
-   * Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
-   * called every loop, and the correct loop period must be passed into the constructor of this
-   * class.
+   * 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 wheelSpeeds The current speeds of the mecanum drive wheels.
+   * @param wheelPositions The distances driven by each wheel.
    * @return The estimated pose of the robot in meters.
    */
-  @SuppressWarnings("LocalVariableName")
   public Pose2d updateWithTime(
-      double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
-    double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
-    m_prevTimeSeconds = currentTimeSeconds;
+      double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
+    m_odometry.update(gyroAngle, wheelPositions);
 
-    var angle = gyroAngle.plus(m_gyroOffset);
-    var omega = angle.minus(m_previousAngle).getRadians() / dt;
-
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
-    var fieldRelativeVelocities =
-        new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond)
-            .rotateBy(angle);
-
-    var u = VecBuilder.fill(fieldRelativeVelocities.getX(), fieldRelativeVelocities.getY(), omega);
-    m_previousAngle = angle;
-
-    var localY = VecBuilder.fill(angle.getRadians());
-    m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
-    m_observer.predict(u, dt);
-    m_observer.correct(u, localY);
+    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);
+    }
+  }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
index fb0628b..24835f6 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
@@ -71,16 +71,15 @@
    * of the filter.
    *
    * @param x An array of the means.
-   * @param P Covariance of the filter.
-   * @return Two dimensional array of sigma points. Each column contains all of the sigmas for one
+   * @param s Square-root covariance of the filter.
+   * @return Two-dimensional array of sigma points. Each column contains all the sigmas for one
    *     dimension in the problem space. Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
    */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
-  public Matrix<S, ?> sigmaPoints(Matrix<S, N1> x, Matrix<S, S> P) {
+  public Matrix<S, ?> squareRootSigmaPoints(Matrix<S, N1> x, Matrix<S, S> s) {
     double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum();
+    double eta = Math.sqrt(lambda + m_states.getNum());
 
-    var intermediate = P.times(lambda + m_states.getNum());
-    var U = intermediate.lltDecompose(true); // Lower triangular
+    Matrix<S, S> U = s.times(eta);
 
     // 2 * states + 1 by states
     Matrix<S, ?> sigmas =
@@ -101,7 +100,6 @@
    *
    * @param beta Incorporates prior knowledge of the distribution of the mean.
    */
-  @SuppressWarnings("LocalVariableName")
   private void computeWeights(double beta) {
     double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum();
     double c = 0.5 / (m_states.getNum() + lambda);
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 8782b7b..57451a0 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,157 +4,103 @@
 
 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.StateSpaceUtil;
 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.Translation2d;
+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.SwerveModuleState;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+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.function.BiConsumer;
+import java.util.Arrays;
+import java.util.Map;
+import java.util.Objects;
 
 /**
- * This class wraps an {@link UnscentedKalmanFilter Unscented Kalman Filter} to fuse
- * latency-compensated vision measurements with swerve drive encoder velocity measurements. It will
- * correct for noisy measurements and encoder drift. It is intended to be an easy but more accurate
- * drop-in for {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry}.
+ * This class wraps {@link SwerveDriveOdometry Swerve Drive Odometry} to fuse latency-compensated
+ * vision measurements with swerve drive encoder distance measurements. It is intended to be a
+ * drop-in replacement for {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry}.
  *
- * <p>{@link SwerveDrivePoseEstimator#update} should be called every robot loop. If your loops are
- * faster or slower than the default of 0.02s, then you should change the nominal delta time using
- * the secondary constructor: {@link SwerveDrivePoseEstimator#SwerveDrivePoseEstimator(Rotation2d,
- * Pose2d, SwerveDriveKinematics, Matrix, Matrix, Matrix, double)}.
+ * <p>{@link SwerveDrivePoseEstimator#update} should be called every robot loop.
  *
  * <p>{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
- * want; if you never call it, then this class will behave mostly like regular encoder odometry.
- *
- * <p>The state-space system used internally has the following states (x), inputs (u), and outputs
- * (y):
- *
- * <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
- * position, and heading.
- *
- * <p><strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity, and angular rate
- * in the field coordinate system.
- *
- * <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
- * heading; or <strong> y = [theta]ᵀ </strong> containing gyro heading.
+ * want; if you never call it, then this class will behave as regular encoder odometry.
  */
 public class SwerveDrivePoseEstimator {
-  private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
   private final SwerveDriveKinematics m_kinematics;
-  private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
-  private final KalmanFilterLatencyCompensator<N3, N3, N1> m_latencyCompensator;
+  private final SwerveDriveOdometry m_odometry;
+  private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
+  private final int m_numModules;
+  private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
 
-  private final double m_nominalDt; // Seconds
-  private double m_prevTimeSeconds = -1.0;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
-
-  private Matrix<N3, N3> m_visionContR;
+  private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
+      TimeInterpolatableBuffer.createBuffer(1.5);
 
   /**
-   * Constructs a SwerveDrivePoseEstimator.
+   * Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision
+   * measurements.
    *
-   * @param gyroAngle The current gyro angle.
-   * @param initialPoseMeters The starting pose estimate.
+   * <p>The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
+   * and 0.1 radians for heading. The default standard deviations of the vision measurements are 0.9
+   * meters for x, 0.9 meters for y, and 0.9 radians for heading.
+   *
    * @param kinematics A correctly-configured kinematics object for your drivetrain.
-   * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
-   *     model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
-   *     meters and radians.
-   * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
-   *     Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
-   *     is in the form [theta], with units in radians.
-   * @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.
+   * @param gyroAngle The current gyro angle.
+   * @param modulePositions The current distance measurements and rotations of the swerve modules.
+   * @param initialPoseMeters The starting pose estimate.
    */
   public SwerveDrivePoseEstimator(
-      Rotation2d gyroAngle,
-      Pose2d initialPoseMeters,
       SwerveDriveKinematics kinematics,
-      Matrix<N3, N1> stateStdDevs,
-      Matrix<N1, N1> localMeasurementStdDevs,
-      Matrix<N3, N1> visionMeasurementStdDevs) {
+      Rotation2d gyroAngle,
+      SwerveModulePosition[] modulePositions,
+      Pose2d initialPoseMeters) {
     this(
-        gyroAngle,
-        initialPoseMeters,
         kinematics,
-        stateStdDevs,
-        localMeasurementStdDevs,
-        visionMeasurementStdDevs,
-        0.02);
+        gyroAngle,
+        modulePositions,
+        initialPoseMeters,
+        VecBuilder.fill(0.1, 0.1, 0.1),
+        VecBuilder.fill(0.9, 0.9, 0.9));
   }
 
   /**
    * Constructs a SwerveDrivePoseEstimator.
    *
-   * @param gyroAngle The current gyro angle.
-   * @param initialPoseMeters The starting pose estimate.
    * @param kinematics A correctly-configured kinematics object for your drivetrain.
-   * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
-   *     model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
-   *     meters and radians.
-   * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
-   *     Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
-   *     is in the form [theta], with units in radians.
-   * @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.
-   * @param nominalDtSeconds The time in seconds between each robot loop.
+   * @param gyroAngle The current gyro angle.
+   * @param modulePositions The current distance and rotation measurements of the swerve modules.
+   * @param initialPoseMeters The starting pose estimate.
+   * @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.
    */
-  @SuppressWarnings("ParameterName")
   public SwerveDrivePoseEstimator(
-      Rotation2d gyroAngle,
-      Pose2d initialPoseMeters,
       SwerveDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      SwerveModulePosition[] modulePositions,
+      Pose2d initialPoseMeters,
       Matrix<N3, N1> stateStdDevs,
-      Matrix<N1, N1> localMeasurementStdDevs,
-      Matrix<N3, N1> visionMeasurementStdDevs,
-      double nominalDtSeconds) {
-    m_nominalDt = nominalDtSeconds;
-
-    m_observer =
-        new UnscentedKalmanFilter<>(
-            Nat.N3(),
-            Nat.N1(),
-            (x, u) -> u,
-            (x, u) -> x.extractRowVector(2),
-            stateStdDevs,
-            localMeasurementStdDevs,
-            AngleStatistics.angleMean(2),
-            AngleStatistics.angleMean(0),
-            AngleStatistics.angleResidual(2),
-            AngleStatistics.angleResidual(0),
-            AngleStatistics.angleAdd(2),
-            m_nominalDt);
+      Matrix<N3, N1> visionMeasurementStdDevs) {
     m_kinematics = kinematics;
-    m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+    m_odometry = new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters);
 
-    // Initialize vision R
+    for (int i = 0; i < 3; ++i) {
+      m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
+    }
+
+    m_numModules = modulePositions.length;
+
     setVisionMeasurementStdDevs(visionMeasurementStdDevs);
-
-    m_visionCorrect =
-        (u, y) ->
-            m_observer.correct(
-                Nat.N3(),
-                u,
-                y,
-                (x, u1) -> x,
-                m_visionContR,
-                AngleStatistics.angleMean(2),
-                AngleStatistics.angleResidual(2),
-                AngleStatistics.angleResidual(2),
-                AngleStatistics.angleAdd(2));
-
-    m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPoseMeters.getRotation();
-    m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters));
   }
 
   /**
@@ -167,85 +113,128 @@
    *     theta]ᵀ, with units in meters and radians.
    */
   public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
-    m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), 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>You NEED to reset your encoders (to zero) when calling this method.
-   *
    * <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 poseMeters The position on the field that your robot is at.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param modulePositions The current distance measurements and rotations of the swerve modules.
+   * @param poseMeters The position on the field that your robot is at.
    */
-  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+  public void resetPosition(
+      Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) {
     // Reset state estimate and error covariance
-    m_observer.reset();
-    m_latencyCompensator.reset();
-
-    m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
-
-    m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
-    m_previousAngle = poseMeters.getRotation();
+    m_odometry.resetPosition(gyroAngle, modulePositions, poseMeters);
+    m_poseBuffer.clear();
   }
 
   /**
-   * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
+   * Gets the estimated robot pose.
    *
    * @return The estimated robot pose in meters.
    */
   public Pose2d getEstimatedPosition() {
-    return new Pose2d(
-        m_observer.getXhat(0), m_observer.getXhat(1), new Rotation2d(m_observer.getXhat(2)));
+    return m_odometry.getPoseMeters();
   }
 
   /**
-   * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
-   * estimate while still accounting for measurement noise.
+   * 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}
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this
-   *     timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use
-   *     Timer.getFPGATimestamp as your time source or sync the epochs.
+   *     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) {
-    m_latencyCompensator.applyPastGlobalMeasurement(
-        Nat.N3(),
-        m_observer,
-        m_nominalDt,
-        StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
-        m_visionCorrect,
-        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);
+    }
   }
 
   /**
-   * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
-   * estimate while still accounting for measurement noise.
+   * 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}
-   *     then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this
-   *     timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use
-   *     Timer.getFPGATimestamp as your time source in this case.
-   * @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.
+   *     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,
@@ -256,50 +245,140 @@
   }
 
   /**
-   * Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
-   * called every loop, and the correct loop period must be passed into the constructor of this
-   * class.
+   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
+   * loop.
    *
    * @param gyroAngle The current gyro angle.
-   * @param moduleStates The current velocities and rotations of the swerve modules.
+   * @param modulePositions The current distance measurements and rotations of the swerve modules.
    * @return The estimated pose of the robot in meters.
    */
-  public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
-    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
+  public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
+    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, modulePositions);
   }
 
   /**
-   * Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
-   * called every loop, and the correct loop period must be passed into the constructor of this
-   * class.
+   * 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 moduleStates The current velocities and rotations of the swerve modules.
+   * @param modulePositions The current distance measurements and rotations of the swerve modules.
    * @return The estimated pose of the robot in meters.
    */
-  @SuppressWarnings("LocalVariableName")
   public Pose2d updateWithTime(
-      double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
-    double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
-    m_prevTimeSeconds = currentTimeSeconds;
+      double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
+    if (modulePositions.length != m_numModules) {
+      throw new IllegalArgumentException(
+          "Number of modules is not consistent with number of wheel locations provided in "
+              + "constructor");
+    }
 
-    var angle = gyroAngle.plus(m_gyroOffset);
-    var omega = angle.minus(m_previousAngle).getRadians() / dt;
+    var internalModulePositions = new SwerveModulePosition[m_numModules];
 
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(moduleStates);
-    var fieldRelativeVelocities =
-        new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond)
-            .rotateBy(angle);
+    for (int i = 0; i < m_numModules; i++) {
+      internalModulePositions[i] =
+          new SwerveModulePosition(modulePositions[i].distanceMeters, modulePositions[i].angle);
+    }
 
-    var u = VecBuilder.fill(fieldRelativeVelocities.getX(), fieldRelativeVelocities.getY(), omega);
-    m_previousAngle = angle;
+    m_odometry.update(gyroAngle, internalModulePositions);
 
-    var localY = VecBuilder.fill(angle.getRadians());
-    m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
-    m_observer.predict(u, dt);
-    m_observer.correct(u, localY);
+    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);
+    }
+  }
 }
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 ffc2c15..d668564 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
@@ -14,6 +14,7 @@
 import edu.wpi.first.math.system.NumericalIntegration;
 import edu.wpi.first.math.system.NumericalJacobian;
 import java.util.function.BiFunction;
+import org.ejml.dense.row.decomposition.qr.QRDecompositionHouseholder_DDRM;
 import org.ejml.simple.SimpleMatrix;
 
 /**
@@ -33,8 +34,10 @@
  * <p>For more on the underlying math, read
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
  * theory".
+ *
+ * <p>This class implements a square-root-form unscented Kalman filter (SR-UKF). For more
+ * information about the SR-UKF, see https://www.researchgate.net/publication/3908304.
  */
-@SuppressWarnings({"MemberName", "ClassTypeParameterName"})
 public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
     implements KalmanTypeFilter<States, Inputs, Outputs> {
   private final Nat<States> m_states;
@@ -50,7 +53,7 @@
   private BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> m_addFuncX;
 
   private Matrix<States, N1> m_xHat;
-  private Matrix<States, States> m_P;
+  private Matrix<States, States> m_S;
   private final Matrix<States, States> m_contQ;
   private final Matrix<Outputs, Outputs> m_contR;
   private Matrix<States, ?> m_sigmasF;
@@ -69,7 +72,6 @@
    * @param measurementStdDevs Standard deviations of measurements.
    * @param nominalDtSeconds Nominal discretization timestep.
    */
-  @SuppressWarnings("LambdaParameterName")
   public UnscentedKalmanFilter(
       Nat<States> states,
       Nat<Outputs> outputs,
@@ -115,7 +117,6 @@
    * @param addFuncX A function that adds two state vectors.
    * @param nominalDtSeconds Nominal discretization timestep.
    */
-  @SuppressWarnings("ParameterName")
   public UnscentedKalmanFilter(
       Nat<States> states,
       Nat<Outputs> outputs,
@@ -151,15 +152,16 @@
     reset();
   }
 
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
-  static <S extends Num, C extends Num> Pair<Matrix<C, N1>, Matrix<C, C>> unscentedTransform(
-      Nat<S> s,
-      Nat<C> dim,
-      Matrix<C, ?> sigmas,
-      Matrix<?, N1> Wm,
-      Matrix<?, N1> Wc,
-      BiFunction<Matrix<C, ?>, Matrix<?, N1>, Matrix<C, N1>> meanFunc,
-      BiFunction<Matrix<C, N1>, Matrix<C, N1>, Matrix<C, N1>> residualFunc) {
+  static <S extends Num, C extends Num>
+      Pair<Matrix<C, N1>, Matrix<C, C>> squareRootUnscentedTransform(
+          Nat<S> s,
+          Nat<C> dim,
+          Matrix<C, ?> sigmas,
+          Matrix<?, N1> Wm,
+          Matrix<?, N1> Wc,
+          BiFunction<Matrix<C, ?>, Matrix<?, N1>, Matrix<C, N1>> meanFunc,
+          BiFunction<Matrix<C, N1>, Matrix<C, N1>, Matrix<C, N1>> residualFunc,
+          Matrix<C, C> squareRootR) {
     if (sigmas.getNumRows() != dim.getNum() || sigmas.getNumCols() != 2 * s.getNum() + 1) {
       throw new IllegalArgumentException(
           "Sigmas must be covDim by 2 * states + 1! Got "
@@ -184,28 +186,64 @@
     //      k=1
     Matrix<C, N1> x = meanFunc.apply(sigmas, Wm);
 
-    // New covariance is the sum of the outer product of the residuals times the
-    // weights
-    Matrix<C, ?> y = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + 1));
-    for (int i = 0; i < 2 * s.getNum() + 1; i++) {
-      // y[:, i] = sigmas[:, i] - x
-      y.setColumn(i, residualFunc.apply(sigmas.extractColumnVector(i), x));
+    Matrix<C, ?> Sbar = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + dim.getNum()));
+    for (int i = 0; i < 2 * s.getNum(); i++) {
+      Sbar.setColumn(
+          i,
+          residualFunc.apply(sigmas.extractColumnVector(1 + i), x).times(Math.sqrt(Wc.get(1, 0))));
     }
-    Matrix<C, C> P =
-        y.times(Matrix.changeBoundsUnchecked(Wc.diag()))
-            .times(Matrix.changeBoundsUnchecked(y.transpose()));
+    Sbar.assignBlock(0, 2 * s.getNum(), squareRootR);
 
-    return new Pair<>(x, P);
+    QRDecompositionHouseholder_DDRM qr = new QRDecompositionHouseholder_DDRM();
+    var qrStorage = Sbar.transpose().getStorage();
+
+    if (!qr.decompose(qrStorage.getDDRM())) {
+      throw new RuntimeException("QR decomposition failed! Input matrix:\n" + qrStorage.toString());
+    }
+
+    Matrix<C, C> newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true)));
+    newS.rankUpdate(residualFunc.apply(sigmas.extractColumnVector(0), x), Wc.get(0, 0), false);
+
+    return new Pair<>(x, newS);
   }
 
   /**
-   * Returns the error covariance matrix P.
+   * Returns the square-root error covariance matrix S.
+   *
+   * @return the square-root error covariance matrix S.
+   */
+  public Matrix<States, States> getS() {
+    return m_S;
+  }
+
+  /**
+   * Returns an element of the square-root error covariance matrix S.
+   *
+   * @param row Row of S.
+   * @param col Column of S.
+   * @return the value of the square-root error covariance matrix S at (i, j).
+   */
+  public double getS(int row, int col) {
+    return m_S.get(row, col);
+  }
+
+  /**
+   * Sets the entire square-root error covariance matrix S.
+   *
+   * @param newS The new value of S to use.
+   */
+  public void setS(Matrix<States, States> newS) {
+    m_S = newS;
+  }
+
+  /**
+   * Returns the reconstructed error covariance matrix P.
    *
    * @return the error covariance matrix P.
    */
   @Override
   public Matrix<States, States> getP() {
-    return m_P;
+    return m_S.transpose().times(m_S);
   }
 
   /**
@@ -214,10 +252,12 @@
    * @param row Row of P.
    * @param col Column of P.
    * @return the value of the error covariance matrix P at (i, j).
+   * @throws UnsupportedOperationException indexing into the reconstructed P matrix is not supported
    */
   @Override
   public double getP(int row, int col) {
-    return m_P.get(row, col);
+    throw new UnsupportedOperationException(
+        "indexing into the reconstructed P matrix is not supported");
   }
 
   /**
@@ -227,7 +267,7 @@
    */
   @Override
   public void setP(Matrix<States, States> newP) {
-    m_P = newP;
+    m_S = newP.lltDecompose(false);
   }
 
   /**
@@ -244,7 +284,7 @@
    * Returns an element of the state estimate x-hat.
    *
    * @param row Row of x-hat.
-   * @return the value of the state estimate x-hat at i.
+   * @return the value of the state estimate x-hat at 'i'.
    */
   @Override
   public double getXhat(int row) {
@@ -256,7 +296,6 @@
    *
    * @param xHat The state estimate x-hat.
    */
-  @SuppressWarnings("ParameterName")
   @Override
   public void setXhat(Matrix<States, N1> xHat) {
     m_xHat = xHat;
@@ -277,7 +316,7 @@
   @Override
   public void reset() {
     m_xHat = new Matrix<>(m_states, Nat.N1());
-    m_P = new Matrix<>(m_states, m_states);
+    m_S = new Matrix<>(m_states, m_states);
     m_sigmasF = new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
   }
 
@@ -287,15 +326,15 @@
    * @param u New control input from controller.
    * @param dtSeconds Timestep for prediction.
    */
-  @SuppressWarnings({"LocalVariableName", "ParameterName"})
   @Override
   public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
     // 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 squareRootDiscQ = discQ.lltDecompose(true);
 
-    var sigmas = m_pts.sigmaPoints(m_xHat, m_P);
+    var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
 
     for (int i = 0; i < m_pts.getNumSigmas(); ++i) {
       Matrix<States, N1> x = sigmas.extractColumnVector(i);
@@ -304,17 +343,18 @@
     }
 
     var ret =
-        unscentedTransform(
+        squareRootUnscentedTransform(
             m_states,
             m_states,
             m_sigmasF,
             m_pts.getWm(),
             m_pts.getWc(),
             m_meanFuncX,
-            m_residualFuncX);
+            m_residualFuncX,
+            squareRootDiscQ);
 
     m_xHat = ret.getFirst();
-    m_P = ret.getSecond().plus(discQ);
+    m_S = ret.getSecond();
     m_dtSeconds = dtSeconds;
   }
 
@@ -324,7 +364,6 @@
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    */
-  @SuppressWarnings("ParameterName")
   @Override
   public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
     correct(
@@ -345,7 +384,6 @@
    * @param h A vector-valued function of x and u that returns the measurement vector.
    * @param R Measurement noise covariance matrix (continuous-time).
    */
-  @SuppressWarnings({"ParameterName", "LambdaParameterName", "LocalVariableName"})
   public <R extends Num> void correct(
       Nat<R> rows,
       Matrix<Inputs, N1> u,
@@ -382,7 +420,6 @@
    *     subtracts them.)
    * @param addFuncX A function that adds two state vectors.
    */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
   public <R extends Num> void correct(
       Nat<R> rows,
       Matrix<Inputs, N1> u,
@@ -394,10 +431,11 @@
       BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
       BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX) {
     final var discR = Discretization.discretizeR(R, m_dtSeconds);
+    final var squareRootDiscR = discR.lltDecompose(true);
 
     // Transform sigma points into measurement space
     Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(rows.getNum(), 2 * m_states.getNum() + 1));
-    var sigmas = m_pts.sigmaPoints(m_xHat, m_P);
+    var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
     for (int i = 0; i < m_pts.getNumSigmas(); i++) {
       Matrix<R, N1> hRet = h.apply(sigmas.extractColumnVector(i), u);
       sigmasH.setColumn(i, hRet);
@@ -405,10 +443,17 @@
 
     // Mean and covariance of prediction passed through unscented transform
     var transRet =
-        unscentedTransform(
-            m_states, rows, sigmasH, m_pts.getWm(), m_pts.getWc(), meanFuncY, residualFuncY);
+        squareRootUnscentedTransform(
+            m_states,
+            rows,
+            sigmasH,
+            m_pts.getWm(),
+            m_pts.getWc(),
+            meanFuncY,
+            residualFuncY,
+            squareRootDiscR);
     var yHat = transRet.getFirst();
-    var Py = transRet.getSecond().plus(discR);
+    var Sy = transRet.getSecond();
 
     // Compute cross covariance of the state and the measurements
     Matrix<States, R> Pxy = new Matrix<>(m_states, rows);
@@ -420,17 +465,20 @@
       Pxy = Pxy.plus(dx.times(dy).times(m_pts.getWc(i)));
     }
 
-    // K = P_{xy} P_y⁻¹
-    // Kᵀ = P_yᵀ⁻¹ P_{xy}ᵀ
-    // P_yᵀKᵀ = P_{xy}ᵀ
-    // Kᵀ = P_yᵀ.solve(P_{xy}ᵀ)
-    // K = (P_yᵀ.solve(P_{xy}ᵀ)ᵀ
-    Matrix<States, R> K = new Matrix<>(Py.transpose().solve(Pxy.transpose()).transpose());
+    // K = (P_{xy} / S_yᵀ) / S_y
+    // K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
+    // K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
+    Matrix<States, R> K =
+        Sy.transpose()
+            .solveFullPivHouseholderQr(Sy.solveFullPivHouseholderQr(Pxy.transpose()))
+            .transpose();
 
     // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
     m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, yHat)));
 
-    // Pₖ₊₁⁺ = Pₖ₊₁⁻ − KP_yKᵀ
-    m_P = m_P.minus(K.times(Py).times(K.transpose()));
+    Matrix<States, R> U = K.times(Sy);
+    for (int i = 0; i < rows.getNum(); i++) {
+      m_S.rankUpdate(U.extractColumnVector(i), -1, false);
+    }
   }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
index 5eac0b3..98dd4e1 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
@@ -128,9 +128,7 @@
     }
 
     double[] ffGains = new double[taps];
-    for (int i = 0; i < ffGains.length; i++) {
-      ffGains[i] = 1.0 / taps;
-    }
+    Arrays.fill(ffGains, 1.0 / taps);
 
     double[] fbGains = new double[0];
 
@@ -144,20 +142,17 @@
    * <p>Stencil points are the indices of the samples to use in the finite difference. 0 is the
    * current sample, -1 is the previous sample, -2 is the sample before that, etc. Don't use
    * positive stencil points (samples from the future) if the LinearFilter will be used for
-   * stream-based online filtering.
+   * stream-based online filtering (e.g., taking derivative of encoder samples in real-time).
    *
    * @param derivative The order of the derivative to compute.
-   * @param samples The number of samples to use to compute the given derivative. This must be one
-   *     more than the order of derivative or higher.
-   * @param stencil List of stencil points.
+   * @param stencil List of stencil points. Its length is the number of samples to use to compute
+   *     the given derivative. This must be one more than the order of the derivative or higher.
    * @param period The period in seconds between samples taken by the user.
    * @return Linear filter.
    * @throws IllegalArgumentException if derivative &lt; 1, samples &lt;= 0, or derivative &gt;=
    *     samples.
    */
-  @SuppressWarnings("LocalVariableName")
-  public static LinearFilter finiteDifference(
-      int derivative, int samples, int[] stencil, double period) {
+  public static LinearFilter finiteDifference(int derivative, int[] stencil, double period) {
     // See
     // https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points
     //
@@ -170,7 +165,7 @@
     // [s₁ⁿ⁻¹ ⋯ sₙⁿ⁻¹][aₙ]      [δₙ₋₁,d]
     //
     // where δᵢ,ⱼ are the Kronecker delta. The FIR gains are the elements of the
-    // vector a in reverse order divided by hᵈ.
+    // vector 'a' in reverse order divided by hᵈ.
     //
     // The order of accuracy of the approximation is of the form O(hⁿ⁻ᵈ).
 
@@ -179,6 +174,8 @@
           "Order of derivative must be greater than or equal to one.");
     }
 
+    int samples = stencil.length;
+
     if (samples <= 0) {
       throw new IllegalArgumentException("Number of samples must be greater than zero.");
     }
@@ -236,7 +233,7 @@
       stencil[i] = -(samples - 1) + i;
     }
 
-    return finiteDifference(derivative, samples, stencil, period);
+    return finiteDifference(derivative, stencil, period);
   }
 
   /** Reset the filter state. */
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 d3aa7d8..668b8b1 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
@@ -14,29 +14,50 @@
  * edu.wpi.first.math.trajectory.TrapezoidProfile} instead.
  */
 public class SlewRateLimiter {
-  private final double m_rateLimit;
+  private final double m_positiveRateLimit;
+  private final double m_negativeRateLimit;
   private double m_prevVal;
   private double m_prevTime;
 
   /**
-   * Creates a new SlewRateLimiter with the given rate limit and initial value.
+   * Creates a new SlewRateLimiter with the given positive and negative rate limits and initial
+   * value.
    *
-   * @param rateLimit The rate-of-change limit, in units per second.
+   * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per
+   *     second. This is expected to be positive.
+   * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per
+   *     second. This is expected to be negative.
    * @param initialValue The initial value of the input.
    */
-  public SlewRateLimiter(double rateLimit, double initialValue) {
-    m_rateLimit = rateLimit;
+  public SlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double initialValue) {
+    m_positiveRateLimit = positiveRateLimit;
+    m_negativeRateLimit = negativeRateLimit;
     m_prevVal = initialValue;
     m_prevTime = WPIUtilJNI.now() * 1e-6;
   }
 
   /**
-   * Creates a new SlewRateLimiter with the given rate limit and an initial value of zero.
+   * 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);
+  }
+
+  /**
+   * Creates a new SlewRateLimiter with the given positive rate limit and negative rate limit of
+   * -rateLimit.
    *
    * @param rateLimit The rate-of-change limit, in units per second.
    */
   public SlewRateLimiter(double rateLimit) {
-    this(rateLimit, 0);
+    this(rateLimit, -rateLimit, 0);
   }
 
   /**
@@ -49,7 +70,10 @@
     double currentTime = WPIUtilJNI.now() * 1e-6;
     double elapsedTime = currentTime - m_prevTime;
     m_prevVal +=
-        MathUtil.clamp(input - m_prevVal, -m_rateLimit * elapsedTime, m_rateLimit * elapsedTime);
+        MathUtil.clamp(
+            input - m_prevVal,
+            m_negativeRateLimit * elapsedTime,
+            m_positiveRateLimit * elapsedTime);
     m_prevTime = currentTime;
     return m_prevVal;
   }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java
new file mode 100644
index 0000000..d6033f8
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java
@@ -0,0 +1,87 @@
+// 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.geometry;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.numbers.N3;
+
+/** A class representing a coordinate system axis within the NWU coordinate system. */
+public class CoordinateAxis {
+  private static final CoordinateAxis m_n = new CoordinateAxis(1.0, 0.0, 0.0);
+  private static final CoordinateAxis m_s = new CoordinateAxis(-1.0, 0.0, 0.0);
+  private static final CoordinateAxis m_e = new CoordinateAxis(0.0, -1.0, 0.0);
+  private static final CoordinateAxis m_w = new CoordinateAxis(0.0, 1.0, 0.0);
+  private static final CoordinateAxis m_u = new CoordinateAxis(0.0, 0.0, 1.0);
+  private static final CoordinateAxis m_d = new CoordinateAxis(0.0, 0.0, -1.0);
+
+  final Vector<N3> m_axis;
+
+  /**
+   * Constructs a coordinate system axis within the NWU coordinate system and normalizes it.
+   *
+   * @param x The x component.
+   * @param y The y component.
+   * @param z The z component.
+   */
+  public CoordinateAxis(double x, double y, double z) {
+    double norm = Math.sqrt(x * x + y * y + z * z);
+    m_axis = VecBuilder.fill(x / norm, y / norm, z / norm);
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to +X in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to +X in the NWU coordinate system.
+   */
+  public static CoordinateAxis N() {
+    return m_n;
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to -X in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to -X in the NWU coordinate system.
+   */
+  public static CoordinateAxis S() {
+    return m_s;
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to -Y in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to -Y in the NWU coordinate system.
+   */
+  public static CoordinateAxis E() {
+    return m_e;
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to +Y in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to +Y in the NWU coordinate system.
+   */
+  public static CoordinateAxis W() {
+    return m_w;
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to +Z in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to +Z in the NWU coordinate system.
+   */
+  public static CoordinateAxis U() {
+    return m_u;
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to -Z in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to -Z in the NWU coordinate system.
+   */
+  public static CoordinateAxis D() {
+    return m_d;
+  }
+}
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
new file mode 100644
index 0000000..5733177
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java
@@ -0,0 +1,130 @@
+// 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.geometry;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+
+/** A helper class that converts Pose3d objects between different standard coordinate frames. */
+public class CoordinateSystem {
+  private static final CoordinateSystem m_nwu =
+      new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.W(), CoordinateAxis.U());
+  private static final CoordinateSystem m_edn =
+      new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.D(), CoordinateAxis.N());
+  private static final CoordinateSystem m_ned =
+      new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.E(), CoordinateAxis.D());
+
+  // Rotation from this coordinate system to NWU coordinate system
+  private final Rotation3d m_rotation;
+
+  /**
+   * Constructs a coordinate system with the given cardinal directions for each axis.
+   *
+   * @param positiveX The cardinal direction of the positive x-axis.
+   * @param positiveY The cardinal direction of the positive y-axis.
+   * @param positiveZ The cardinal direction of the positive z-axis.
+   * @throws IllegalArgumentException if the coordinate system isn't special orthogonal
+   */
+  public CoordinateSystem(
+      CoordinateAxis positiveX, CoordinateAxis positiveY, CoordinateAxis positiveZ) {
+    // 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.
+    var R = new Matrix<>(Nat.N3(), Nat.N3());
+    R.assignBlock(0, 0, positiveX.m_axis);
+    R.assignBlock(0, 1, positiveY.m_axis);
+    R.assignBlock(0, 2, positiveZ.m_axis);
+
+    // The change of basis matrix should be a pure rotation. The Rotation3d
+    // constructor will verify this by checking for special orthogonality.
+    m_rotation = new Rotation3d(R);
+  }
+
+  /**
+   * Returns an instance of the North-West-Up (NWU) coordinate system.
+   *
+   * <p>The +X axis is north, the +Y axis is west, and the +Z axis is up.
+   *
+   * @return An instance of the North-West-Up (NWU) coordinate system.
+   */
+  public static CoordinateSystem NWU() {
+    return m_nwu;
+  }
+
+  /**
+   * Returns an instance of the East-Down-North (EDN) coordinate system.
+   *
+   * <p>The +X axis is east, the +Y axis is down, and the +Z axis is north.
+   *
+   * @return An instance of the East-Down-North (EDN) coordinate system.
+   */
+  public static CoordinateSystem EDN() {
+    return m_edn;
+  }
+
+  /**
+   * Returns an instance of the North-East-Down (NED) coordinate system.
+   *
+   * <p>The +X axis is north, the +Y axis is east, and the +Z axis is down.
+   *
+   * @return An instance of the North-East-Down (NED) coordinate system.
+   */
+  public static CoordinateSystem NED() {
+    return m_ned;
+  }
+
+  /**
+   * Converts the given translation from one coordinate system to another.
+   *
+   * @param translation The translation to convert.
+   * @param from The coordinate system the pose starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given translation in the desired coordinate system.
+   */
+  public static Translation3d convert(
+      Translation3d translation, CoordinateSystem from, CoordinateSystem to) {
+    return translation.rotateBy(from.m_rotation.minus(to.m_rotation));
+  }
+
+  /**
+   * Converts the given rotation from one coordinate system to another.
+   *
+   * @param rotation The rotation to convert.
+   * @param from The coordinate system the rotation starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given rotation in the desired coordinate system.
+   */
+  public static Rotation3d convert(
+      Rotation3d rotation, CoordinateSystem from, CoordinateSystem to) {
+    return rotation.rotateBy(from.m_rotation.minus(to.m_rotation));
+  }
+
+  /**
+   * Converts the given pose from one coordinate system to another.
+   *
+   * @param pose The pose to convert.
+   * @param from The coordinate system the pose starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given pose in the desired coordinate system.
+   */
+  public static Pose3d convert(Pose3d pose, CoordinateSystem from, CoordinateSystem to) {
+    return new Pose3d(
+        convert(pose.getTranslation(), from, to), convert(pose.getRotation(), from, to));
+  }
+
+  /**
+   * Converts the given transform from one coordinate system to another.
+   *
+   * @param transform The transform to convert.
+   * @param from The coordinate system the transform starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given transform in the desired coordinate system.
+   */
+  public static Transform3d convert(
+      Transform3d transform, CoordinateSystem from, CoordinateSystem to) {
+    return new Transform3d(
+        convert(transform.getTranslation(), from, to), convert(transform.getRotation(), from, to));
+  }
+}
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 f64eff4..bce832e 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
@@ -11,17 +11,14 @@
 import edu.wpi.first.math.interpolation.Interpolatable;
 import java.util.Objects;
 
-/** Represents a 2d pose containing translational and rotational elements. */
+/** Represents a 2D pose containing translational and rotational elements. */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Pose2d implements Interpolatable<Pose2d> {
   private final Translation2d m_translation;
   private final Rotation2d m_rotation;
 
-  /**
-   * Constructs a pose at the origin facing toward the positive X axis. (Translation2d{0, 0} and
-   * Rotation{0})
-   */
+  /** Constructs a pose at the origin facing toward the positive X axis. */
   public Pose2d() {
     m_translation = new Translation2d();
     m_rotation = new Rotation2d();
@@ -42,8 +39,7 @@
   }
 
   /**
-   * Convenience constructors that takes in x and y values directly instead of having to construct a
-   * Translation2d.
+   * Constructs a pose with x and y translations instead of a separate Translation2d.
    *
    * @param x The x component of the translational component of the pose.
    * @param y The y component of the translational component of the pose.
@@ -57,8 +53,11 @@
   /**
    * Transforms the pose by the given transformation and returns the new transformed pose.
    *
-   * <p>The matrix multiplication is as follows [x_new] [cos, -sin, 0][transform.x] [y_new] += [sin,
-   * cos, 0][transform.y] [t_new] [0, 0, 1][transform.t]
+   * <pre>
+   * [x_new]    [cos, -sin, 0][transform.x]
+   * [y_new] += [sin,  cos, 0][transform.y]
+   * [t_new]    [  0,    0, 1][transform.t]
+   * </pre>
    *
    * @param other The transform to transform the pose by.
    * @return The transformed pose.
@@ -117,6 +116,26 @@
   }
 
   /**
+   * Multiplies the current pose by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Pose2d.
+   */
+  public Pose2d times(double scalar) {
+    return new Pose2d(m_translation.times(scalar), m_rotation.times(scalar));
+  }
+
+  /**
+   * Divides the current pose by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Pose2d.
+   */
+  public Pose2d div(double scalar) {
+    return times(1.0 / scalar);
+  }
+
+  /**
    * Transforms the pose by the given transformation and returns the new pose. See + operator for
    * the matrix multiplication performed.
    *
@@ -126,11 +145,11 @@
   public Pose2d transformBy(Transform2d other) {
     return new Pose2d(
         m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
-        m_rotation.plus(other.getRotation()));
+        other.getRotation().plus(m_rotation));
   }
 
   /**
-   * Returns the other pose relative to the current pose.
+   * Returns the current pose relative to the given pose.
    *
    * <p>This function can often be used for trajectory tracking or pose stabilization algorithms to
    * get the error between the reference and the current pose.
@@ -160,8 +179,8 @@
    *
    * @param twist The change in pose in the robot's coordinate frame since the previous pose update.
    *     For example, if a non-holonomic robot moves forward 0.01 meters and changes angle by 0.5
-   *     degrees since the previous pose update, the twist would be Twist2d{0.01, 0.0,
-   *     toRadians(0.5)}
+   *     degrees since the previous pose update, the twist would be Twist2d(0.01, 0.0,
+   *     Units.degreesToRadians(0.5)).
    * @return The new pose of the robot.
    */
   public Pose2d exp(Twist2d twist) {
@@ -190,8 +209,8 @@
   }
 
   /**
-   * Returns a Twist2d that maps this pose to the end pose. If c is the output of a.Log(b), then
-   * a.Exp(c) would yield b.
+   * Returns a Twist2d that maps this pose to the end pose. If c is the output of {@code a.Log(b)},
+   * then {@code a.Exp(c)} would yield b.
    *
    * @param end The end pose for the transformation.
    * @return The twist that maps this to end.
@@ -245,7 +264,6 @@
   }
 
   @Override
-  @SuppressWarnings("ParameterName")
   public Pose2d interpolate(Pose2d endValue, double t) {
     if (t < 0) {
       return this;
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
new file mode 100644
index 0000000..8e3a3fe
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.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.geometry;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+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.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N3;
+import java.util.Objects;
+
+/** Represents a 3D pose containing translational and rotational elements. */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Pose3d implements Interpolatable<Pose3d> {
+  private final Translation3d m_translation;
+  private final Rotation3d m_rotation;
+
+  /** Constructs a pose at the origin facing toward the positive X axis. */
+  public Pose3d() {
+    m_translation = new Translation3d();
+    m_rotation = new Rotation3d();
+  }
+
+  /**
+   * Constructs a pose with the specified translation and rotation.
+   *
+   * @param translation The translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  @JsonCreator
+  public Pose3d(
+      @JsonProperty(required = true, value = "translation") Translation3d translation,
+      @JsonProperty(required = true, value = "rotation") Rotation3d rotation) {
+    m_translation = translation;
+    m_rotation = rotation;
+  }
+
+  /**
+   * Constructs a pose with x, y, and z translations instead of a separate Translation3d.
+   *
+   * @param x The x component of the translational component of the pose.
+   * @param y The y component of the translational component of the pose.
+   * @param z The z component of the translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  public Pose3d(double x, double y, double z, Rotation3d rotation) {
+    m_translation = new Translation3d(x, y, z);
+    m_rotation = rotation;
+  }
+
+  /**
+   * Constructs a 3D pose from a 2D pose in the X-Y plane.
+   *
+   * @param pose The 2D pose.
+   */
+  public Pose3d(Pose2d pose) {
+    m_translation = new Translation3d(pose.getX(), pose.getY(), 0.0);
+    m_rotation = new Rotation3d(0.0, 0.0, pose.getRotation().getRadians());
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new transformed pose.
+   *
+   * @param other The transform to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose3d plus(Transform3d other) {
+    return transformBy(other);
+  }
+
+  /**
+   * Returns the Transform3d that maps the one pose to another.
+   *
+   * @param other The initial pose of the transformation.
+   * @return The transform that maps the other pose to the current pose.
+   */
+  public Transform3d minus(Pose3d other) {
+    final var pose = this.relativeTo(other);
+    return new Transform3d(pose.getTranslation(), pose.getRotation());
+  }
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return The translational component of the pose.
+   */
+  @JsonProperty
+  public Translation3d getTranslation() {
+    return m_translation;
+  }
+
+  /**
+   * Returns the X component of the pose's translation.
+   *
+   * @return The x component of the pose's translation.
+   */
+  public double getX() {
+    return m_translation.getX();
+  }
+
+  /**
+   * Returns the Y component of the pose's translation.
+   *
+   * @return The y component of the pose's translation.
+   */
+  public double getY() {
+    return m_translation.getY();
+  }
+
+  /**
+   * Returns the Z component of the pose's translation.
+   *
+   * @return The z component of the pose's translation.
+   */
+  public double getZ() {
+    return m_translation.getZ();
+  }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return The rotational component of the pose.
+   */
+  @JsonProperty
+  public Rotation3d getRotation() {
+    return m_rotation;
+  }
+
+  /**
+   * Multiplies the current pose by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Pose3d.
+   */
+  public Pose3d times(double scalar) {
+    return new Pose3d(m_translation.times(scalar), m_rotation.times(scalar));
+  }
+
+  /**
+   * Divides the current pose by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Pose3d.
+   */
+  public Pose3d div(double scalar) {
+    return times(1.0 / scalar);
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new pose. See + operator for
+   * the matrix multiplication performed.
+   *
+   * @param other The transform to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose3d transformBy(Transform3d other) {
+    return new Pose3d(
+        m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
+        other.getRotation().plus(m_rotation));
+  }
+
+  /**
+   * Returns the current pose relative to the given pose.
+   *
+   * <p>This function can often be used for trajectory tracking or pose stabilization algorithms to
+   * get the error between the reference and the current pose.
+   *
+   * @param other The pose that is the origin of the new coordinate frame that the current pose will
+   *     be converted into.
+   * @return The current pose relative to the new origin pose.
+   */
+  public Pose3d relativeTo(Pose3d other) {
+    var transform = new Transform3d(other, this);
+    return new Pose3d(transform.getTranslation(), transform.getRotation());
+  }
+
+  /**
+   * Obtain a new Pose3d from a (constant curvature) velocity.
+   *
+   * <p>The twist is a change in pose in the robot's coordinate frame since the previous pose
+   * update. When the user runs exp() on the previous known field-relative pose with the argument
+   * being the twist, the user will receive the new field-relative pose.
+   *
+   * <p>"Exp" represents the pose exponential, which is solving a differential equation moving the
+   * pose forward in time.
+   *
+   * @param twist The change in pose in the robot's coordinate frame since the previous pose update.
+   *     For example, if a non-holonomic robot moves forward 0.01 meters and changes angle by 0.5
+   *     degrees since the previous pose update, the twist would be Twist3d(0.01, 0.0, 0.0, new new
+   *     Rotation3d(0.0, 0.0, Units.degreesToRadians(0.5))).
+   * @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);
+  }
+
+  /**
+   * Returns a Twist3d that maps this pose to the end pose. If c is the output of {@code a.Log(b)},
+   * then {@code a.Exp(c)} would yield b.
+   *
+   * @param end The end pose for the transformation.
+   * @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()));
+
+    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));
+  }
+
+  /**
+   * Returns a Pose2d representing this Pose3d projected into the X-Y plane.
+   *
+   * @return A Pose2d representing this Pose3d projected into the X-Y plane.
+   */
+  public Pose2d toPose2d() {
+    return new Pose2d(m_translation.toTranslation2d(), m_rotation.toRotation2d());
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Pose3d(%s, %s)", m_translation, m_rotation);
+  }
+
+  /**
+   * Checks equality between this Pose3d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Pose3d) {
+      return ((Pose3d) obj).m_translation.equals(m_translation)
+          && ((Pose3d) obj).m_rotation.equals(m_rotation);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_translation, m_rotation);
+  }
+
+  @Override
+  public Pose3d interpolate(Pose3d endValue, double t) {
+    if (t < 0) {
+      return this;
+    } else if (t >= 1) {
+      return endValue;
+    } else {
+      var twist = this.log(endValue);
+      var scaledTwist =
+          new Twist3d(
+              twist.dx * t, twist.dy * t, twist.dz * t, twist.rx * t, twist.ry * t, twist.rz * t);
+      return this.exp(scaledTwist);
+    }
+  }
+
+  /**
+   * 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);
+  }
+}
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
new file mode 100644
index 0000000..cadfaa4
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
@@ -0,0 +1,186 @@
+// 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.geometry;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.numbers.N3;
+import java.util.Objects;
+
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Quaternion {
+  private final double m_r;
+  private final Vector<N3> m_v;
+
+  /** 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);
+  }
+
+  /**
+   * Constructs a quaternion with the given components.
+   *
+   * @param w W component of the quaternion.
+   * @param x X component of the quaternion.
+   * @param y Y component of the quaternion.
+   * @param z Z component of the quaternion.
+   */
+  @JsonCreator
+  public Quaternion(
+      @JsonProperty(required = true, value = "W") double w,
+      @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);
+  }
+
+  /**
+   * Multiply with another quaternion.
+   *
+   * @param other The other quaternion.
+   * @return The quaternion product.
+   */
+  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;
+
+    // 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));
+
+    // 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));
+  }
+
+  @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));
+  }
+
+  /**
+   * Checks equality between this Quaternion and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    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 false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_r, m_v);
+  }
+
+  /**
+   * Returns the inverse of the quaternion.
+   *
+   * @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));
+  }
+
+  /**
+   * Normalizes the quaternion.
+   *
+   * @return The normalized quaternion.
+   */
+  public Quaternion normalize() {
+    double norm = Math.sqrt(getW() * getW() + getX() * getX() + getY() * getY() + getZ() * getZ());
+    if (norm == 0.0) {
+      return new Quaternion();
+    } else {
+      return new Quaternion(getW() / norm, getX() / norm, getY() / norm, getZ() / norm);
+    }
+  }
+
+  /**
+   * Returns W component of the quaternion.
+   *
+   * @return W component of the quaternion.
+   */
+  @JsonProperty(value = "W")
+  public double getW() {
+    return m_r;
+  }
+
+  /**
+   * Returns X component of the quaternion.
+   *
+   * @return X component of the quaternion.
+   */
+  @JsonProperty(value = "X")
+  public double getX() {
+    return m_v.get(0, 0);
+  }
+
+  /**
+   * Returns Y component of the quaternion.
+   *
+   * @return Y component of the quaternion.
+   */
+  @JsonProperty(value = "Y")
+  public double getY() {
+    return m_v.get(1, 0);
+  }
+
+  /**
+   * Returns Z component of the quaternion.
+   *
+   * @return Z component of the quaternion.
+   */
+  @JsonProperty(value = "Z")
+  public double getZ() {
+    return m_v.get(2, 0);
+  }
+
+  /**
+   * Returns the rotation vector representation of this quaternion.
+   *
+   * <p>This is also the log operator of SO(3).
+   *
+   * @return The rotation vector representation of this quaternion.
+   */
+  public Vector<N3> toRotationVector() {
+    // See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
+    // Sound State Representation through Encapsulation of Manifolds"
+    //
+    // https://arxiv.org/pdf/1107.1119.pdf
+    double norm = m_v.norm();
+
+    if (norm < 1e-9) {
+      return m_v.times(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);
+      } else {
+        return m_v.times(2.0 * Math.atan2(norm, getW()) / norm);
+      }
+    }
+  }
+}
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 08e5438..5be6156 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,9 +10,16 @@
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.util.Units;
 import java.util.Objects;
 
-/** A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). */
+/**
+ * A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
+ *
+ * <p>The angle is continuous, that is if a Rotation2d is constructed with 361 degrees, it will
+ * return 361 degrees. This allows algorithms that wouldn't want to see a discontinuity in the
+ * rotations as it sweeps past from 360 to 0 on the second time around.
+ */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Rotation2d implements Interpolatable<Rotation2d> {
@@ -28,7 +35,7 @@
   }
 
   /**
-   * Constructs a Rotation2d with the given radian value. The x and y don't have to be normalized.
+   * Constructs a Rotation2d with the given radian value.
    *
    * @param value The value of the angle in radians.
    */
@@ -58,6 +65,16 @@
   }
 
   /**
+   * Constructs and returns a Rotation2d with the given radian value.
+   *
+   * @param radians The value of the angle in radians.
+   * @return The rotation object with the desired angle value.
+   */
+  public static Rotation2d fromRadians(double radians) {
+    return new Rotation2d(radians);
+  }
+
+  /**
    * Constructs and returns a Rotation2d with the given degree value.
    *
    * @param degrees The value of the angle in degrees.
@@ -68,9 +85,20 @@
   }
 
   /**
+   * Constructs and returns a Rotation2d with the given number of rotations.
+   *
+   * @param rotations The value of the angle in rotations.
+   * @return The rotation object with the desired angle value.
+   */
+  public static Rotation2d fromRotations(double rotations) {
+    return new Rotation2d(Units.rotationsToRadians(rotations));
+  }
+
+  /**
    * Adds two rotations together, with the result being bounded between -pi and pi.
    *
-   * <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) = Rotation2d{-pi/2}
+   * <p>For example, <code>Rotation2d.fromDegrees(30).plus(Rotation2d.fromDegrees(60))</code> equals
+   * <code>Rotation2d(Math.PI/2.0)</code>
    *
    * @param other The rotation to add.
    * @return The sum of the two rotations.
@@ -82,7 +110,8 @@
   /**
    * Subtracts the new rotation from the current rotation and returns the new rotation.
    *
-   * <p>For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) = Rotation2d{-pi/2}
+   * <p>For example, <code>Rotation2d.fromDegrees(10).minus(Rotation2d.fromDegrees(100))</code>
+   * equals <code>Rotation2d(-Math.PI/2.0)</code>
    *
    * @param other The rotation to subtract.
    * @return The difference between the two rotations.
@@ -112,6 +141,16 @@
   }
 
   /**
+   * Divides the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Rotation2d.
+   */
+  public Rotation2d div(double scalar) {
+    return times(1.0 / scalar);
+  }
+
+  /**
    * Adds the new rotation to the current rotation using a rotation matrix.
    *
    * <p>The matrix multiplication is as follows:
@@ -131,9 +170,10 @@
   }
 
   /**
-   * Returns the radian value of the rotation.
+   * Returns the radian value of the Rotation2d.
    *
-   * @return The radian value of the rotation.
+   * @return The radian value of the Rotation2d.
+   * @see edu.wpi.first.math.MathUtil#angleModulus(double) to constrain the angle within (-pi, pi]
    */
   @JsonProperty
   public double getRadians() {
@@ -141,36 +181,47 @@
   }
 
   /**
-   * Returns the degree value of the rotation.
+   * Returns the degree value of the Rotation2d.
    *
-   * @return The degree value of the rotation.
+   * @return The degree value of the Rotation2d.
+   * @see edu.wpi.first.math.MathUtil#inputModulus(double, double, double) to constrain the angle
+   *     within (-180, 180]
    */
   public double getDegrees() {
     return Math.toDegrees(m_value);
   }
 
   /**
-   * Returns the cosine of the rotation.
+   * Returns the number of rotations of the Rotation2d.
    *
-   * @return The cosine of the rotation.
+   * @return The number of rotations of the Rotation2d.
+   */
+  public double getRotations() {
+    return Units.radiansToRotations(m_value);
+  }
+
+  /**
+   * Returns the cosine of the Rotation2d.
+   *
+   * @return The cosine of the Rotation2d.
    */
   public double getCos() {
     return m_cos;
   }
 
   /**
-   * Returns the sine of the rotation.
+   * Returns the sine of the Rotation2d.
    *
-   * @return The sine of the rotation.
+   * @return The sine of the Rotation2d.
    */
   public double getSin() {
     return m_sin;
   }
 
   /**
-   * Returns the tangent of the rotation.
+   * Returns the tangent of the Rotation2d.
    *
-   * @return The tangent of the rotation.
+   * @return The tangent of the Rotation2d.
    */
   public double getTan() {
     return m_sin / m_cos;
@@ -202,8 +253,7 @@
   }
 
   @Override
-  @SuppressWarnings("ParameterName")
   public Rotation2d interpolate(Rotation2d endValue, double t) {
-    return new Rotation2d(MathUtil.interpolate(this.getRadians(), endValue.getRadians(), t));
+    return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
   }
 }
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
new file mode 100644
index 0000000..3226e31
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
@@ -0,0 +1,403 @@
+// 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.geometry;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+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.MathSharedStore;
+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.Vector;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N3;
+import java.util.Objects;
+import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
+
+/** 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();
+
+  /** Constructs a Rotation3d with a default angle of 0 degrees. */
+  public Rotation3d() {}
+
+  /**
+   * Constructs a Rotation3d from a quaternion.
+   *
+   * @param q The quaternion.
+   */
+  @JsonCreator
+  public Rotation3d(@JsonProperty(required = true, value = "quaternion") Quaternion q) {
+    m_q = q.normalize();
+  }
+
+  /**
+   * Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
+   *
+   * <p>Extrinsic rotations occur in that order around the axes in the fixed global frame rather
+   * than the body frame.
+   *
+   * <p>Angles are measured counterclockwise with the rotation axis pointing "out of the page". If
+   * you point your right thumb along the positive axis direction, your fingers curl in the
+   * direction of positive rotation.
+   *
+   * @param roll The counterclockwise rotation angle around the X axis (roll) in radians.
+   * @param pitch The counterclockwise rotation angle around the Y axis (pitch) in radians.
+   * @param yaw The counterclockwise rotation angle around the Z axis (yaw) in radians.
+   */
+  public Rotation3d(double roll, double pitch, double yaw) {
+    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
+    double cr = Math.cos(roll * 0.5);
+    double sr = Math.sin(roll * 0.5);
+
+    double cp = Math.cos(pitch * 0.5);
+    double sp = Math.sin(pitch * 0.5);
+
+    double cy = Math.cos(yaw * 0.5);
+    double sy = Math.sin(yaw * 0.5);
+
+    m_q =
+        new Quaternion(
+            cr * cp * cy + sr * sp * sy,
+            sr * cp * cy - cr * sp * sy,
+            cr * sp * cy + sr * cp * sy,
+            cr * cp * sy - sr * sp * cy);
+  }
+
+  /**
+   * Constructs a Rotation3d with the given axis-angle representation. The axis doesn't have to be
+   * normalized.
+   *
+   * @param axis The rotation axis.
+   * @param angleRadians The rotation around the axis in radians.
+   */
+  public Rotation3d(Vector<N3> axis, double angleRadians) {
+    double norm = axis.norm();
+    if (norm == 0.0) {
+      return;
+    }
+
+    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
+    var v = axis.times(1.0 / norm).times(Math.sin(angleRadians / 2.0));
+    m_q = new Quaternion(Math.cos(angleRadians / 2.0), v.get(0, 0), v.get(1, 0), v.get(2, 0));
+  }
+
+  /**
+   * Constructs a Rotation3d from a rotation matrix.
+   *
+   * @param rotationMatrix The rotation matrix.
+   * @throws IllegalArgumentException if the rotation matrix isn't special orthogonal.
+   */
+  public Rotation3d(Matrix<N3, N3> rotationMatrix) {
+    final var 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.times(R.transpose()).minus(Matrix.eye(Nat.N3())).normF() > 1e-9) {
+      var builder = new StringBuilder("Rotation matrix isn't orthogonal\n\nR =\n");
+      builder.append(R.getStorage().toString()).append('\n');
+
+      var msg = builder.toString();
+      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(msg);
+    }
+    if (Math.abs(R.det() - 1.0) > 1e-9) {
+      var builder =
+          new StringBuilder("Rotation matrix is orthogonal but not special orthogonal\n\nR =\n");
+      builder.append(R.getStorage().toString()).append('\n');
+
+      var msg = builder.toString();
+      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(msg);
+    }
+
+    // Turn rotation matrix into a quaternion
+    // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+    double trace = R.get(0, 0) + R.get(1, 1) + R.get(2, 2);
+    double w;
+    double x;
+    double y;
+    double z;
+
+    if (trace > 0.0) {
+      double s = 0.5 / Math.sqrt(trace + 1.0);
+      w = 0.25 / s;
+      x = (R.get(2, 1) - R.get(1, 2)) * s;
+      y = (R.get(0, 2) - R.get(2, 0)) * s;
+      z = (R.get(1, 0) - R.get(0, 1)) * s;
+    } else {
+      if (R.get(0, 0) > R.get(1, 1) && R.get(0, 0) > R.get(2, 2)) {
+        double s = 2.0 * Math.sqrt(1.0 + R.get(0, 0) - R.get(1, 1) - R.get(2, 2));
+        w = (R.get(2, 1) - R.get(1, 2)) / s;
+        x = 0.25 * s;
+        y = (R.get(0, 1) + R.get(1, 0)) / s;
+        z = (R.get(0, 2) + R.get(2, 0)) / s;
+      } else if (R.get(1, 1) > R.get(2, 2)) {
+        double s = 2.0 * Math.sqrt(1.0 + R.get(1, 1) - R.get(0, 0) - R.get(2, 2));
+        w = (R.get(0, 2) - R.get(2, 0)) / s;
+        x = (R.get(0, 1) + R.get(1, 0)) / s;
+        y = 0.25 * s;
+        z = (R.get(1, 2) + R.get(2, 1)) / s;
+      } else {
+        double s = 2.0 * Math.sqrt(1.0 + R.get(2, 2) - R.get(0, 0) - R.get(1, 1));
+        w = (R.get(1, 0) - R.get(0, 1)) / s;
+        x = (R.get(0, 2) + R.get(2, 0)) / s;
+        y = (R.get(1, 2) + R.get(2, 1)) / s;
+        z = 0.25 * s;
+      }
+    }
+
+    m_q = new Quaternion(w, x, y, z);
+  }
+
+  /**
+   * Constructs a Rotation3d that rotates the initial vector onto the final vector.
+   *
+   * <p>This is useful for turning a 3D vector (final) into an orientation relative to a coordinate
+   * system vector (initial).
+   *
+   * @param initial The initial vector.
+   * @param last The final vector.
+   */
+  public Rotation3d(Vector<N3> initial, Vector<N3> last) {
+    double dot = initial.dot(last);
+    double normProduct = initial.norm() * last.norm();
+    double dotNorm = dot / normProduct;
+
+    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.
+      return;
+    } else if (dotNorm < -1.0 + 1E-9) {
+      // If the dot product is -1, the two vectors point in opposite directions
+      // so a 180 degree rotation is required. Any orthogonal vector can be used
+      // for it. Q in the QR decomposition is an orthonormal basis, so it
+      // contains orthogonal unit vectors.
+      var X =
+          new MatBuilder<>(Nat.N3(), Nat.N1())
+              .fill(initial.get(0, 0), initial.get(1, 0), initial.get(2, 0));
+      final var qr = DecompositionFactory_DDRM.qr(3, 1);
+      qr.decompose(X.getStorage().getMatrix());
+      final var Q = qr.getQ(null, false);
+
+      // w = cos(θ/2) = cos(90°) = 0
+      //
+      // For x, y, and z, we use the second column of Q because the first is
+      // parallel instead of orthogonal. The third column would also work.
+      m_q = new Quaternion(0.0, Q.get(0, 1), Q.get(1, 1), Q.get(2, 1));
+    } else {
+      // initial x last
+      var axis =
+          VecBuilder.fill(
+              initial.get(1, 0) * last.get(2, 0) - last.get(1, 0) * initial.get(2, 0),
+              last.get(0, 0) * initial.get(2, 0) - initial.get(0, 0) * last.get(2, 0),
+              initial.get(0, 0) * last.get(1, 0) - last.get(0, 0) * initial.get(1, 0));
+
+      // https://stackoverflow.com/a/11741520
+      m_q =
+          new Quaternion(normProduct + dot, axis.get(0, 0), axis.get(1, 0), axis.get(2, 0))
+              .normalize();
+    }
+  }
+
+  /**
+   * Adds two rotations together.
+   *
+   * @param other The rotation to add.
+   * @return The sum of the two rotations.
+   */
+  public Rotation3d plus(Rotation3d other) {
+    return rotateBy(other);
+  }
+
+  /**
+   * Subtracts the new rotation from the current rotation and returns the new rotation.
+   *
+   * @param other The rotation to subtract.
+   * @return The difference between the two rotations.
+   */
+  public Rotation3d minus(Rotation3d other) {
+    return rotateBy(other.unaryMinus());
+  }
+
+  /**
+   * Takes the inverse of the current rotation.
+   *
+   * @return The inverse of the current rotation.
+   */
+  public Rotation3d unaryMinus() {
+    return new Rotation3d(m_q.inverse());
+  }
+
+  /**
+   * Multiplies the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Rotation3d.
+   */
+  public Rotation3d times(double scalar) {
+    // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
+    if (m_q.getW() >= 0.0) {
+      return new Rotation3d(
+          VecBuilder.fill(m_q.getX(), m_q.getY(), m_q.getZ()),
+          2.0 * scalar * Math.acos(m_q.getW()));
+    } else {
+      return new Rotation3d(
+          VecBuilder.fill(-m_q.getX(), -m_q.getY(), -m_q.getZ()),
+          2.0 * scalar * Math.acos(-m_q.getW()));
+    }
+  }
+
+  /**
+   * Divides the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Rotation3d.
+   */
+  public Rotation3d div(double scalar) {
+    return times(1.0 / scalar);
+  }
+
+  /**
+   * Adds the new rotation to the current rotation.
+   *
+   * @param other The rotation to rotate by.
+   * @return The new rotated Rotation3d.
+   */
+  public Rotation3d rotateBy(Rotation3d other) {
+    return new Rotation3d(other.m_q.times(m_q));
+  }
+
+  /**
+   * Returns the quaternion representation of the Rotation3d.
+   *
+   * @return The quaternion representation of the Rotation3d.
+   */
+  @JsonProperty(value = "quaternion")
+  public Quaternion getQuaternion() {
+    return m_q;
+  }
+
+  /**
+   * Returns the counterclockwise rotation angle around the X axis (roll) in radians.
+   *
+   * @return The counterclockwise rotation angle around the X axis (roll) in radians.
+   */
+  public double getX() {
+    final var w = m_q.getW();
+    final var x = m_q.getX();
+    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));
+  }
+
+  /**
+   * Returns the counterclockwise rotation angle around the Y axis (pitch) in radians.
+   *
+   * @return The counterclockwise rotation angle around the Y axis (pitch) in radians.
+   */
+  public double getY() {
+    final var w = m_q.getW();
+    final var x = m_q.getX();
+    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
+    double ratio = 2.0 * (w * y - z * x);
+    if (Math.abs(ratio) >= 1.0) {
+      return Math.copySign(Math.PI / 2.0, ratio);
+    } else {
+      return Math.asin(ratio);
+    }
+  }
+
+  /**
+   * Returns the counterclockwise rotation angle around the Z axis (yaw) in radians.
+   *
+   * @return The counterclockwise rotation angle around the Z axis (yaw) in radians.
+   */
+  public double getZ() {
+    final var w = m_q.getW();
+    final var x = m_q.getX();
+    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));
+  }
+
+  /**
+   * Returns the axis in the axis-angle representation of this rotation.
+   *
+   * @return The axis in the axis-angle representation.
+   */
+  public Vector<N3> getAxis() {
+    double norm =
+        Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
+    if (norm == 0.0) {
+      return VecBuilder.fill(0.0, 0.0, 0.0);
+    } else {
+      return VecBuilder.fill(m_q.getX() / norm, m_q.getY() / norm, m_q.getZ() / norm);
+    }
+  }
+
+  /**
+   * Returns the angle in radians in the axis-angle representation of this rotation.
+   *
+   * @return The angle in radians in the axis-angle representation of this rotation.
+   */
+  public double getAngle() {
+    double norm =
+        Math.sqrt(m_q.getX() * m_q.getX() + m_q.getY() * m_q.getY() + m_q.getZ() * m_q.getZ());
+    return 2.0 * Math.atan2(norm, m_q.getW());
+  }
+
+  /**
+   * Returns a Rotation2d representing this Rotation3d projected into the X-Y plane.
+   *
+   * @return A Rotation2d representing this Rotation3d projected into the X-Y plane.
+   */
+  public Rotation2d toRotation2d() {
+    return new Rotation2d(getZ());
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Rotation3d(%s)", m_q);
+  }
+
+  /**
+   * Checks equality between this Rotation3d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Rotation3d) {
+      var other = (Rotation3d) obj;
+      return m_q.equals(other.m_q);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_q);
+  }
+
+  @Override
+  public Rotation3d interpolate(Rotation3d endValue, double t) {
+    return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
+  }
+}
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 dd35670..c3c6b0c 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
@@ -47,7 +47,7 @@
   }
 
   /**
-   * Scales the transform by the scalar.
+   * Multiplies the transform by the scalar.
    *
    * @param scalar The scalar.
    * @return The scaled Transform2d.
@@ -57,6 +57,16 @@
   }
 
   /**
+   * Divides the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform2d.
+   */
+  public Transform2d div(double scalar) {
+    return times(1.0 / scalar);
+  }
+
+  /**
    * Composes two transformations.
    *
    * @param other The transform to compose with this one.
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
new file mode 100644
index 0000000..4920ef6
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
@@ -0,0 +1,162 @@
+// 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.geometry;
+
+import java.util.Objects;
+
+/** Represents a transformation for a Pose3d. */
+public class Transform3d {
+  private final Translation3d m_translation;
+  private final Rotation3d m_rotation;
+
+  /**
+   * Constructs the transform that maps the initial pose to the final pose.
+   *
+   * @param initial The initial pose for the transformation.
+   * @param last The final pose for the transformation.
+   */
+  public Transform3d(Pose3d initial, Pose3d last) {
+    // We are rotating the difference between the translations
+    // using a clockwise rotation matrix. This transforms the global
+    // delta into a local delta (relative to the initial pose).
+    m_translation =
+        last.getTranslation()
+            .minus(initial.getTranslation())
+            .rotateBy(initial.getRotation().unaryMinus());
+
+    m_rotation = last.getRotation().minus(initial.getRotation());
+  }
+
+  /**
+   * Constructs a transform with the given translation and rotation components.
+   *
+   * @param translation Translational component of the transform.
+   * @param rotation Rotational component of the transform.
+   */
+  public Transform3d(Translation3d translation, Rotation3d rotation) {
+    m_translation = translation;
+    m_rotation = rotation;
+  }
+
+  /** Constructs the identity transform -- maps an initial pose to itself. */
+  public Transform3d() {
+    m_translation = new Translation3d();
+    m_rotation = new Rotation3d();
+  }
+
+  /**
+   * Multiplies the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform3d.
+   */
+  public Transform3d times(double scalar) {
+    return new Transform3d(m_translation.times(scalar), m_rotation.times(scalar));
+  }
+
+  /**
+   * Divides the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform3d.
+   */
+  public Transform3d div(double scalar) {
+    return times(1.0 / scalar);
+  }
+
+  /**
+   * Composes two transformations.
+   *
+   * @param other The transform to compose with this one.
+   * @return The composition of the two transformations.
+   */
+  public Transform3d plus(Transform3d other) {
+    return new Transform3d(new Pose3d(), new Pose3d().transformBy(this).transformBy(other));
+  }
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return The translational component of the transform.
+   */
+  public Translation3d getTranslation() {
+    return m_translation;
+  }
+
+  /**
+   * Returns the X component of the transformation's translation.
+   *
+   * @return The x component of the transformation's translation.
+   */
+  public double getX() {
+    return m_translation.getX();
+  }
+
+  /**
+   * Returns the Y component of the transformation's translation.
+   *
+   * @return The y component of the transformation's translation.
+   */
+  public double getY() {
+    return m_translation.getY();
+  }
+
+  /**
+   * Returns the Z component of the transformation's translation.
+   *
+   * @return The z component of the transformation's translation.
+   */
+  public double getZ() {
+    return m_translation.getZ();
+  }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return Reference to the rotational component of the transform.
+   */
+  public Rotation3d getRotation() {
+    return m_rotation;
+  }
+
+  /**
+   * Invert the transformation. This is useful for undoing a transformation.
+   *
+   * @return The inverted transformation.
+   */
+  public Transform3d inverse() {
+    // We are rotating the difference between the translations
+    // using a clockwise rotation matrix. This transforms the global
+    // delta into a local delta (relative to the initial pose).
+    return new Transform3d(
+        getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
+        getRotation().unaryMinus());
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Transform3d(%s, %s)", m_translation, m_rotation);
+  }
+
+  /**
+   * Checks equality between this Transform3d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Transform3d) {
+      return ((Transform3d) obj).m_translation.equals(m_translation)
+          && ((Transform3d) obj).m_rotation.equals(m_rotation);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_translation, m_rotation);
+  }
+}
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 84eaea8..2d57edc 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
@@ -13,13 +13,11 @@
 import java.util.Objects;
 
 /**
- * Represents a translation in 2d space. This object can be used to represent a point or a vector.
+ * Represents a translation in 2D space. This object can be used to represent a point or a vector.
  *
- * <p>This assumes that you are using conventional mathematical axes. When the robot is placed on
- * the origin, facing toward the X direction, moving forward increases the X, whereas moving to the
- * left increases the Y.
+ * <p>This assumes that you are using conventional mathematical axes. When the robot is at the
+ * origin facing in the positive X direction, forward is positive X and left is positive Y.
  */
-@SuppressWarnings({"ParameterName", "MemberName"})
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Translation2d implements Interpolatable<Translation2d> {
@@ -58,10 +56,9 @@
   }
 
   /**
-   * Calculates the distance between two translations in 2d space.
+   * Calculates the distance between two translations in 2D space.
    *
-   * <p>This function uses the pythagorean theorem to calculate the distance. distance = sqrt((x2 -
-   * x1)^2 + (y2 - y1)^2)
+   * <p>The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²).
    *
    * @param other The translation to compute the distance to.
    * @return The distance between the two translations.
@@ -73,7 +70,7 @@
   /**
    * Returns the X component of the translation.
    *
-   * @return The x component of the translation.
+   * @return The X component of the translation.
    */
   @JsonProperty
   public double getX() {
@@ -83,7 +80,7 @@
   /**
    * Returns the Y component of the translation.
    *
-   * @return The y component of the translation.
+   * @return The Y component of the translation.
    */
   @JsonProperty
   public double getY() {
@@ -100,13 +97,27 @@
   }
 
   /**
-   * Applies a rotation to the translation in 2d space.
+   * Returns the angle this translation forms with the positive X axis.
+   *
+   * @return The angle of the translation
+   */
+  public Rotation2d getAngle() {
+    return new Rotation2d(m_x, m_y);
+  }
+
+  /**
+   * Applies a rotation to the translation in 2D space.
    *
    * <p>This multiplies the translation vector by a counterclockwise rotation matrix of the given
-   * angle. [x_new] [other.cos, -other.sin][x] [y_new] = [other.sin, other.cos][y]
+   * angle.
    *
-   * <p>For example, rotating a Translation2d of {2, 0} by 90 degrees will return a Translation2d of
-   * {0, 2}.
+   * <pre>
+   * [x_new]   [other.cos, -other.sin][x]
+   * [y_new] = [other.sin,  other.cos][y]
+   * </pre>
+   *
+   * <p>For example, rotating a Translation2d of &lt;2, 0&gt; by 90 degrees will return a
+   * Translation2d of &lt;0, 2&gt;.
    *
    * @param other The rotation to rotate the translation by.
    * @return The new rotated translation.
@@ -117,9 +128,9 @@
   }
 
   /**
-   * Adds two translations in 2d space and returns the sum. This is similar to vector addition.
+   * Returns the sum of two translations in 2D space.
    *
-   * <p>For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} = Translation2d{3.0, 8.0}
+   * <p>For example, Translation3d(1.0, 2.5) + Translation3d(2.0, 5.5) = Translation3d{3.0, 8.0).
    *
    * @param other The translation to add.
    * @return The sum of the translations.
@@ -129,9 +140,9 @@
   }
 
   /**
-   * Subtracts the other translation from the other translation and returns the difference.
+   * Returns the difference between two translations.
    *
-   * <p>For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} = Translation2d{4.0, 2.0}
+   * <p>For example, Translation2d(5.0, 4.0) - Translation2d(1.0, 2.0) = Translation2d(4.0, 2.0).
    *
    * @param other The translation to subtract.
    * @return The difference between the two translations.
@@ -142,7 +153,7 @@
 
   /**
    * Returns the inverse of the current translation. This is equivalent to rotating by 180 degrees,
-   * flipping the point over both axes, or simply negating both components of the translation.
+   * flipping the point over both axes, or negating all components of the translation.
    *
    * @return The inverse of the current translation.
    */
@@ -151,9 +162,9 @@
   }
 
   /**
-   * Multiplies the translation by a scalar and returns the new translation.
+   * Returns the translation multiplied by a scalar.
    *
-   * <p>For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
+   * <p>For example, Translation2d(2.0, 2.5) * 2 = Translation2d(4.0, 5.0).
    *
    * @param scalar The scalar to multiply by.
    * @return The scaled translation.
@@ -163,9 +174,9 @@
   }
 
   /**
-   * Divides the translation by a scalar and returns the new translation.
+   * Returns the translation divided by a scalar.
    *
-   * <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+   * <p>For example, Translation3d(2.0, 2.5) / 2 = Translation3d(1.0, 1.25).
    *
    * @param scalar The scalar to multiply by.
    * @return The reference to the new mutated object.
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
new file mode 100644
index 0000000..810f56c
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
@@ -0,0 +1,234 @@
+// 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.geometry;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.interpolation.Interpolatable;
+import java.util.Objects;
+
+/**
+ * Represents a translation in 3D space. This object can be used to represent a point or a vector.
+ *
+ * <p>This assumes that you are using conventional mathematical axes. When the robot is at the
+ * origin facing in the positive X direction, forward is positive X, left is positive Y, and up is
+ * positive Z.
+ */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Translation3d implements Interpolatable<Translation3d> {
+  private final double m_x;
+  private final double m_y;
+  private final double m_z;
+
+  /** Constructs a Translation3d with X, Y, and Z components equal to zero. */
+  public Translation3d() {
+    this(0.0, 0.0, 0.0);
+  }
+
+  /**
+   * Constructs a Translation3d with the X, Y, and Z components equal to the provided values.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   * @param z The z component of the translation.
+   */
+  @JsonCreator
+  public Translation3d(
+      @JsonProperty(required = true, value = "x") double x,
+      @JsonProperty(required = true, value = "y") double y,
+      @JsonProperty(required = true, value = "z") double z) {
+    m_x = x;
+    m_y = y;
+    m_z = z;
+  }
+
+  /**
+   * Constructs a Translation3d with the provided distance and angle. This is essentially converting
+   * from polar coordinates to Cartesian coordinates.
+   *
+   * @param distance The distance from the origin to the end of the translation.
+   * @param angle The angle between the x-axis and the translation vector.
+   */
+  public Translation3d(double distance, Rotation3d angle) {
+    final var rectangular = new Translation3d(distance, 0.0, 0.0).rotateBy(angle);
+    m_x = rectangular.getX();
+    m_y = rectangular.getY();
+    m_z = rectangular.getZ();
+  }
+
+  /**
+   * Calculates the distance between two translations in 3D space.
+   *
+   * <p>The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²).
+   *
+   * @param other The translation to compute the distance to.
+   * @return The distance between the two translations.
+   */
+  public double getDistance(Translation3d other) {
+    return Math.sqrt(
+        Math.pow(other.m_x - m_x, 2) + Math.pow(other.m_y - m_y, 2) + Math.pow(other.m_z - m_z, 2));
+  }
+
+  /**
+   * Returns the X component of the translation.
+   *
+   * @return The X component of the translation.
+   */
+  @JsonProperty
+  public double getX() {
+    return m_x;
+  }
+
+  /**
+   * Returns the Y component of the translation.
+   *
+   * @return The Y component of the translation.
+   */
+  @JsonProperty
+  public double getY() {
+    return m_y;
+  }
+
+  /**
+   * Returns the Z component of the translation.
+   *
+   * @return The Z component of the translation.
+   */
+  @JsonProperty
+  public double getZ() {
+    return m_z;
+  }
+
+  /**
+   * Returns the norm, or distance from the origin to the translation.
+   *
+   * @return The norm of the translation.
+   */
+  public double getNorm() {
+    return Math.sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
+  }
+
+  /**
+   * Applies a rotation to the translation in 3D space.
+   *
+   * <p>For example, rotating a Translation3d of &lt;2, 0, 0&gt; by 90 degrees around the Z axis
+   * will return a Translation3d of &lt;0, 2, 0&gt;.
+   *
+   * @param other The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  public Translation3d rotateBy(Rotation3d other) {
+    final var p = new Quaternion(0.0, m_x, m_y, m_z);
+    final var qprime = other.getQuaternion().times(p).times(other.getQuaternion().inverse());
+    return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
+  }
+
+  /**
+   * Returns a Translation2d representing this Translation3d projected into the X-Y plane.
+   *
+   * @return A Translation2d representing this Translation3d projected into the X-Y plane.
+   */
+  public Translation2d toTranslation2d() {
+    return new Translation2d(m_x, m_y);
+  }
+
+  /**
+   * Returns the sum of two translations in 3D space.
+   *
+   * <p>For example, Translation3d(1.0, 2.5, 3.5) + Translation3d(2.0, 5.5, 7.5) =
+   * Translation3d{3.0, 8.0, 11.0).
+   *
+   * @param other The translation to add.
+   * @return The sum of the translations.
+   */
+  public Translation3d plus(Translation3d other) {
+    return new Translation3d(m_x + other.m_x, m_y + other.m_y, m_z + other.m_z);
+  }
+
+  /**
+   * Returns the difference between two translations.
+   *
+   * <p>For example, Translation3d(5.0, 4.0, 3.0) - Translation3d(1.0, 2.0, 3.0) =
+   * Translation3d(4.0, 2.0, 0.0).
+   *
+   * @param other The translation to subtract.
+   * @return The difference between the two translations.
+   */
+  public Translation3d minus(Translation3d other) {
+    return new Translation3d(m_x - other.m_x, m_y - other.m_y, m_z - other.m_z);
+  }
+
+  /**
+   * Returns the inverse of the current translation. This is equivalent to negating all components
+   * of the translation.
+   *
+   * @return The inverse of the current translation.
+   */
+  public Translation3d unaryMinus() {
+    return new Translation3d(-m_x, -m_y, -m_z);
+  }
+
+  /**
+   * Returns the translation multiplied by a scalar.
+   *
+   * <p>For example, Translation3d(2.0, 2.5, 4.5) * 2 = Translation3d(4.0, 5.0, 9.0).
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled translation.
+   */
+  public Translation3d times(double scalar) {
+    return new Translation3d(m_x * scalar, m_y * scalar, m_z * scalar);
+  }
+
+  /**
+   * Returns the translation divided by a scalar.
+   *
+   * <p>For example, Translation3d(2.0, 2.5, 4.5) / 2 = Translation3d(1.0, 1.25, 2.25).
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The reference to the new mutated object.
+   */
+  public Translation3d div(double scalar) {
+    return new Translation3d(m_x / scalar, m_y / scalar, m_z / scalar);
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Translation3d(X: %.2f, Y: %.2f, Z: %.2f)", m_x, m_y, m_z);
+  }
+
+  /**
+   * Checks equality between this Translation3d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Translation3d) {
+      return Math.abs(((Translation3d) obj).m_x - m_x) < 1E-9
+          && Math.abs(((Translation3d) obj).m_y - m_y) < 1E-9
+          && Math.abs(((Translation3d) obj).m_z - m_z) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_x, m_y, m_z);
+  }
+
+  @Override
+  public Translation3d interpolate(Translation3d endValue, double t) {
+    return new Translation3d(
+        MathUtil.interpolate(this.getX(), endValue.getX(), t),
+        MathUtil.interpolate(this.getY(), endValue.getY(), t),
+        MathUtil.interpolate(this.getZ(), endValue.getZ(), t));
+  }
+}
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 c73d236..be6831e 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
@@ -7,12 +7,11 @@
 import java.util.Objects;
 
 /**
- * A change in distance along arc since the last pose update. We can use ideas from differential
- * calculus to create new Pose2ds from a Twist2d and vise versa.
+ * 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.
  *
  * <p>A Twist can be used to represent a difference between two poses.
  */
-@SuppressWarnings("MemberName")
 public class Twist2d {
   /** Linear "dx" component. */
   public double dx;
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
new file mode 100644
index 0000000..b78505e
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
@@ -0,0 +1,85 @@
+// 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.geometry;
+
+import java.util.Objects;
+
+/**
+ * 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.
+ *
+ * <p>A Twist can be used to represent a difference between two poses.
+ */
+public class Twist3d {
+  /** Linear "dx" component. */
+  public double dx;
+
+  /** Linear "dy" component. */
+  public double dy;
+
+  /** Linear "dz" component. */
+  public double dz;
+
+  /** Rotation vector x component (radians). */
+  public double rx;
+
+  /** Rotation vector y component (radians). */
+  public double ry;
+
+  /** Rotation vector z component (radians). */
+  public double rz;
+
+  public Twist3d() {}
+
+  /**
+   * Constructs a Twist3d with the given values.
+   *
+   * @param dx Change in x direction relative to robot.
+   * @param dy Change in y direction relative to robot.
+   * @param dz Change in z direction relative to robot.
+   * @param rx Rotation vector x component.
+   * @param ry Rotation vector y component.
+   * @param rz Rotation vector z component.
+   */
+  public Twist3d(double dx, double dy, double dz, double rx, double ry, double rz) {
+    this.dx = dx;
+    this.dy = dy;
+    this.dz = dz;
+    this.rx = rx;
+    this.ry = ry;
+    this.rz = rz;
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "Twist3d(dX: %.2f, dY: %.2f, dZ: %.2f, rX: %.2f, rY: %.2f, rZ: %.2f)",
+        dx, dy, dz, rx, ry, rz);
+  }
+
+  /**
+   * Checks equality between this Twist3d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Twist3d) {
+      return Math.abs(((Twist3d) obj).dx - dx) < 1E-9
+          && Math.abs(((Twist3d) obj).dy - dy) < 1E-9
+          && Math.abs(((Twist3d) obj).dz - dz) < 1E-9
+          && Math.abs(((Twist3d) obj).rx - rx) < 1E-9
+          && Math.abs(((Twist3d) obj).ry - ry) < 1E-9
+          && Math.abs(((Twist3d) obj).rz - rz) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(dx, dy, dz, rx, ry, rz);
+  }
+}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolatable.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolatable.java
index f10820f..9fe09fe 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolatable.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolatable.java
@@ -20,6 +20,5 @@
    * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
    * @return The interpolated value.
    */
-  @SuppressWarnings("ParameterName")
   T interpolate(T endValue, double t);
 }
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 676bd7c..7e0712d 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
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.math.MathUtil;
 import java.util.NavigableMap;
+import java.util.Optional;
 import java.util.TreeMap;
 
 /**
@@ -16,10 +17,10 @@
  *
  * @param <T> The type stored in this buffer.
  */
-public class TimeInterpolatableBuffer<T> {
+public final class TimeInterpolatableBuffer<T> {
   private final double m_historySize;
   private final InterpolateFunction<T> m_interpolatingFunc;
-  private final NavigableMap<Double, T> m_buffer = new TreeMap<>();
+  private final NavigableMap<Double, T> m_pastSnapshots = new TreeMap<>();
 
   private TimeInterpolatableBuffer(
       InterpolateFunction<T> interpolateFunction, double historySizeSeconds) {
@@ -70,7 +71,7 @@
    */
   public void addSample(double timeSeconds, T sample) {
     cleanUp(timeSeconds);
-    m_buffer.put(timeSeconds, sample);
+    m_pastSnapshots.put(timeSeconds, sample);
   }
 
   /**
@@ -79,10 +80,10 @@
    * @param time The current timestamp.
    */
   private void cleanUp(double time) {
-    while (!m_buffer.isEmpty()) {
-      var entry = m_buffer.firstEntry();
+    while (!m_pastSnapshots.isEmpty()) {
+      var entry = m_pastSnapshots.firstEntry();
       if (time - entry.getKey() >= m_historySize) {
-        m_buffer.remove(entry.getKey());
+        m_pastSnapshots.remove(entry.getKey());
       } else {
         return;
       }
@@ -91,48 +92,58 @@
 
   /** Clear all old samples. */
   public void clear() {
-    m_buffer.clear();
+    m_pastSnapshots.clear();
   }
 
   /**
-   * Sample the buffer at the given time. If the buffer is empty, this will return null.
+   * Sample the buffer at the given time. If the buffer is empty, an empty Optional is returned.
    *
    * @param timeSeconds The time at which to sample.
-   * @return The interpolated value at that timestamp. Might be null.
+   * @return The interpolated value at that timestamp or an empty Optional.
    */
-  @SuppressWarnings("UnnecessaryParentheses")
-  public T getSample(double timeSeconds) {
-    if (m_buffer.isEmpty()) {
-      return null;
+  public Optional<T> getSample(double timeSeconds) {
+    if (m_pastSnapshots.isEmpty()) {
+      return Optional.empty();
     }
 
     // Special case for when the requested time is the same as a sample
-    var nowEntry = m_buffer.get(timeSeconds);
+    var nowEntry = m_pastSnapshots.get(timeSeconds);
     if (nowEntry != null) {
-      return nowEntry;
+      return Optional.of(nowEntry);
     }
 
-    var topBound = m_buffer.ceilingEntry(timeSeconds);
-    var bottomBound = m_buffer.floorEntry(timeSeconds);
+    var topBound = m_pastSnapshots.ceilingEntry(timeSeconds);
+    var bottomBound = m_pastSnapshots.floorEntry(timeSeconds);
 
     // Return null if neither sample exists, and the opposite bound if the other is null
     if (topBound == null && bottomBound == null) {
-      return null;
+      return Optional.empty();
     } else if (topBound == null) {
-      return bottomBound.getValue();
+      return Optional.of(bottomBound.getValue());
     } else if (bottomBound == null) {
-      return topBound.getValue();
+      return Optional.of(topBound.getValue());
     } else {
       // Otherwise, interpolate. Because T is between [0, 1], we want the ratio of (the difference
       // between the current time and bottom bound) and (the difference between top and bottom
       // bounds).
-      return m_interpolatingFunc.interpolate(
-          bottomBound.getValue(),
-          topBound.getValue(),
-          ((timeSeconds - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey())));
+      return Optional.of(
+          m_interpolatingFunc.interpolate(
+              bottomBound.getValue(),
+              topBound.getValue(),
+              (timeSeconds - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey())));
     }
   }
 
+  /**
+   * Grant access to the internal sample buffer. Used in Pose Estimation to replay odometry inputs
+   * stored within this buffer.
+   *
+   * @return The internal sample buffer.
+   */
+  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
@@ -143,7 +154,6 @@
      * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
      * @return The interpolated value.
      */
-    @SuppressWarnings("ParameterName")
     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 451c008..6c98337 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
@@ -16,7 +16,6 @@
  * component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum
  * will often have all three components.
  */
-@SuppressWarnings("MemberName")
 public class ChassisSpeeds {
   /** Represents forward velocity w.r.t the robot frame of reference. (Fwd is +) */
   public double vxMetersPerSecond;
@@ -69,6 +68,27 @@
         omegaRadiansPerSecond);
   }
 
+  /**
+   * Converts a user provided field-relative ChassisSpeeds object into a robot-relative
+   * ChassisSpeeds object.
+   *
+   * @param fieldRelativeSpeeds The ChassisSpeeds object representing the speeds in the field frame
+   *     of reference. Positive x is away from your alliance wall. Positive y is to your left when
+   *     standing behind the alliance wall.
+   * @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 robot's frame of reference.
+   */
+  public static ChassisSpeeds fromFieldRelativeSpeeds(
+      ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) {
+    return fromFieldRelativeSpeeds(
+        fieldRelativeSpeeds.vxMetersPerSecond,
+        fieldRelativeSpeeds.vyMetersPerSecond,
+        fieldRelativeSpeeds.omegaRadiansPerSecond,
+        robotAngle);
+  }
+
   @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 7984e39..0dfb016 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
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.geometry.Twist2d;
 
 /**
  * Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel
@@ -15,7 +16,6 @@
  * whereas forward kinematics converts left and right component velocities into a linear and angular
  * chassis speed.
  */
-@SuppressWarnings("MemberName")
 public class DifferentialDriveKinematics {
   public final double trackWidthMeters;
 
@@ -58,4 +58,20 @@
         chassisSpeeds.vxMetersPerSecond
             + trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
   }
+
+  /**
+   * 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
+   * the field using changes in the distance driven by each wheel on the robot.
+   *
+   * @param leftDistanceMeters The distance measured by the left side encoder.
+   * @param rightDistanceMeters The distance measured by the right side encoder.
+   * @return The resulting Twist2d.
+   */
+  public Twist2d toTwist2d(double leftDistanceMeters, double rightDistanceMeters) {
+    return new Twist2d(
+        (leftDistanceMeters + rightDistanceMeters) / 2,
+        0,
+        (rightDistanceMeters - leftDistanceMeters) / trackWidthMeters);
+  }
 }
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 0139573..e8f97f5 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
@@ -33,42 +33,59 @@
    * Constructs a DifferentialDriveOdometry object.
    *
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistanceMeters The distance traveled by the left encoder.
+   * @param rightDistanceMeters The distance traveled by the right encoder.
    * @param initialPoseMeters The starting position of the robot on the field.
    */
-  public DifferentialDriveOdometry(Rotation2d gyroAngle, Pose2d initialPoseMeters) {
+  public DifferentialDriveOdometry(
+      Rotation2d gyroAngle,
+      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;
+
     MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
   }
 
   /**
-   * Constructs a DifferentialDriveOdometry object with the default pose at the origin.
+   * Constructs a DifferentialDriveOdometry object.
    *
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistanceMeters The distance traveled by the left encoder.
+   * @param rightDistanceMeters The distance traveled by the right encoder.
    */
-  public DifferentialDriveOdometry(Rotation2d gyroAngle) {
-    this(gyroAngle, new Pose2d());
+  public DifferentialDriveOdometry(
+      Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
+    this(gyroAngle, leftDistanceMeters, rightDistanceMeters, new Pose2d());
   }
 
   /**
    * Resets the robot's position on the field.
    *
-   * <p>You NEED to reset your encoders (to zero) when calling this method.
-   *
    * <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 poseMeters The position on the field that your robot is at.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistanceMeters The distance traveled by the left encoder.
+   * @param rightDistanceMeters The distance traveled by the right encoder.
+   * @param poseMeters The position on the field that your robot is at.
    */
-  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+  public void resetPosition(
+      Rotation2d gyroAngle,
+      double leftDistanceMeters,
+      double rightDistanceMeters,
+      Pose2d poseMeters) {
     m_poseMeters = poseMeters;
     m_previousAngle = poseMeters.getRotation();
     m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
 
-    m_prevLeftDistance = 0.0;
-    m_prevRightDistance = 0.0;
+    m_prevLeftDistance = leftDistanceMeters;
+    m_prevRightDistance = rightDistanceMeters;
   }
 
   /**
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 20a38a9..d4b235e 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
@@ -5,7 +5,6 @@
 package edu.wpi.first.math.kinematics;
 
 /** Represents the wheel speeds for a differential drive drivetrain. */
-@SuppressWarnings("MemberName")
 public class DifferentialDriveWheelSpeeds {
   /** Speed of the left side of the robot. */
   public double leftMetersPerSecond;
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 0807aba..23204d4 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
@@ -7,6 +7,7 @@
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
 import org.ejml.simple.SimpleMatrix;
 
 /**
@@ -15,8 +16,8 @@
  *
  * <p>The inverse kinematics (converting from a desired chassis velocity to individual wheel speeds)
  * uses the relative locations of the wheels with respect to the center of rotation. The center of
- * rotation for inverse kinematics is also variable. This means that you can set your set your
- * center of rotation in a corner of the robot to perform special evasion maneuvers.
+ * rotation for inverse kinematics is also variable. This means that you can set your center of
+ * rotation in a corner of the robot to perform special evasion maneuvers.
  *
  * <p>Forward kinematics (converting an array of wheel speeds into the overall chassis motion) is
  * performs the exact opposite of what inverse kinematics does. Since this is an overdetermined
@@ -154,6 +155,28 @@
   }
 
   /**
+   * 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
+   * changes in the distance driven by each wheel on the robot.
+   *
+   * @param wheelDeltas The distances driven by each wheel.
+   * @return The resulting Twist2d.
+   */
+  public Twist2d toTwist2d(MecanumDriveWheelPositions wheelDeltas) {
+    var wheelDeltasVector = new SimpleMatrix(4, 1);
+    wheelDeltasVector.setColumn(
+        0,
+        0,
+        wheelDeltas.frontLeftMeters,
+        wheelDeltas.frontRightMeters,
+        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));
+  }
+
+  /**
    * Construct inverse kinematics matrix from wheel locations.
    *
    * @param fl The location of the front-left wheel relative to the physical center of the robot.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java
index b504acc..a63eaea 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java
@@ -5,7 +5,6 @@
 package edu.wpi.first.math.kinematics;
 
 /** Represents the motor voltages for a mecanum drive drivetrain. */
-@SuppressWarnings("MemberName")
 public class MecanumDriveMotorVoltages {
   /** Voltage of the front left motor. */
   public double frontLeftVoltage;
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 b3c79c9..40a77e2 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
@@ -8,8 +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;
-import edu.wpi.first.util.WPIUtilJNI;
 
 /**
  * Class for mecanum drive odometry. Odometry allows you to track the robot's position on the field
@@ -21,7 +19,7 @@
 public class MecanumDriveOdometry {
   private final MecanumDriveKinematics m_kinematics;
   private Pose2d m_poseMeters;
-  private double m_prevTimeSeconds = -1;
+  private MecanumDriveWheelPositions m_previousWheelPositions;
 
   private Rotation2d m_gyroOffset;
   private Rotation2d m_previousAngle;
@@ -31,14 +29,24 @@
    *
    * @param kinematics The mecanum drive kinematics for your drivetrain.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The distances driven by each wheel.
    * @param initialPoseMeters The starting position of the robot on the field.
    */
   public MecanumDriveOdometry(
-      MecanumDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPoseMeters) {
+      MecanumDriveKinematics kinematics,
+      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);
     MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
   }
 
@@ -47,9 +55,13 @@
    *
    * @param kinematics The mecanum drive kinematics for your drivetrain.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The distances driven by each wheel.
    */
-  public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
-    this(kinematics, gyroAngle, new Pose2d());
+  public MecanumDriveOdometry(
+      MecanumDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      MecanumDriveWheelPositions wheelPositions) {
+    this(kinematics, gyroAngle, wheelPositions, new Pose2d());
   }
 
   /**
@@ -58,13 +70,21 @@
    * <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 poseMeters The position on the field that your robot is at.
    * @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(Pose2d poseMeters, Rotation2d gyroAngle) {
+  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);
   }
 
   /**
@@ -78,48 +98,37 @@
 
   /**
    * Updates the robot's position on the field using forward kinematics and integration of the pose
-   * over time. This method takes in the current time as a parameter to calculate period (difference
-   * between two timestamps). The period is used to calculate the change in distance from a
-   * velocity. This also takes in an angle parameter which is used instead of the angular rate that
-   * is calculated from forward kinematics.
+   * 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 currentTimeSeconds The current time in seconds.
    * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelSpeeds The current wheel speeds.
+   * @param wheelPositions The distances driven by each wheel.
    * @return The new pose of the robot.
    */
-  public Pose2d updateWithTime(
-      double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
-    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
-    m_prevTimeSeconds = currentTimeSeconds;
-
+  public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
     var angle = gyroAngle.plus(m_gyroOffset);
 
-    var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
-    var newPose =
-        m_poseMeters.exp(
-            new Twist2d(
-                chassisState.vxMetersPerSecond * period,
-                chassisState.vyMetersPerSecond * period,
-                angle.minus(m_previousAngle).getRadians()));
+    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);
-    return m_poseMeters;
-  }
+    m_previousWheelPositions =
+        new MecanumDriveWheelPositions(
+            wheelPositions.frontLeftMeters,
+            wheelPositions.frontRightMeters,
+            wheelPositions.rearLeftMeters,
+            wheelPositions.rearRightMeters);
 
-  /**
-   * Updates the robot's position on the field using forward kinematics and integration of the pose
-   * over time. This method automatically calculates the current time to calculate period
-   * (difference between two timestamps). The period is used to calculate the change in distance
-   * from a velocity. This also takes in an angle parameter which is used instead of the angular
-   * rate that is calculated from forward kinematics.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelSpeeds The current wheel speeds.
-   * @return The new pose of the robot.
-   */
-  public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
-    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds);
+    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
new file mode 100644
index 0000000..9ff3341
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
@@ -0,0 +1,49 @@
+// 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;
+
+public class MecanumDriveWheelPositions {
+  /** Distance measured by the front left wheel. */
+  public double frontLeftMeters;
+
+  /** Distance measured by the front right wheel. */
+  public double frontRightMeters;
+
+  /** Distance measured by the rear left wheel. */
+  public double rearLeftMeters;
+
+  /** Distance measured by the rear right wheel. */
+  public double rearRightMeters;
+
+  /** Constructs a MecanumDriveWheelPositions with zeros for all member fields. */
+  public MecanumDriveWheelPositions() {}
+
+  /**
+   * Constructs a MecanumDriveWheelPositions.
+   *
+   * @param frontLeftMeters Distance measured by the front left wheel.
+   * @param frontRightMeters Distance measured by the front right wheel.
+   * @param rearLeftMeters Distance measured by the rear left wheel.
+   * @param rearRightMeters Distance measured by the rear right wheel.
+   */
+  public MecanumDriveWheelPositions(
+      double frontLeftMeters,
+      double frontRightMeters,
+      double rearLeftMeters,
+      double rearRightMeters) {
+    this.frontLeftMeters = frontLeftMeters;
+    this.frontRightMeters = frontRightMeters;
+    this.rearLeftMeters = rearLeftMeters;
+    this.rearRightMeters = 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);
+  }
+}
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 ef354ef..1dcfc85 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
@@ -6,7 +6,6 @@
 
 import java.util.stream.DoubleStream;
 
-@SuppressWarnings("MemberName")
 public class MecanumDriveWheelSpeeds {
   /** Speed of the front left wheel. */
   public double frontLeftMetersPerSecond;
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 dc1b9e4..98df547 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
@@ -8,6 +8,7 @@
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.geometry.Rotation2d;
 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;
@@ -18,8 +19,8 @@
  *
  * <p>The inverse kinematics (converting from a desired chassis velocity to individual module
  * states) uses the relative locations of the modules with respect to the center of rotation. The
- * center of rotation for inverse kinematics is also variable. This means that you can set your set
- * your center of rotation in a corner of the robot to perform special evasion maneuvers.
+ * center of rotation for inverse kinematics is also variable. This means that you can set your
+ * center of rotation in a corner of the robot to perform special evasion maneuvers.
  *
  * <p>Forward kinematics (converting an array of module states into the overall chassis motion) is
  * performs the exact opposite of what inverse kinematics does. Since this is an overdetermined
@@ -38,13 +39,15 @@
 
   private final int m_numModules;
   private final Translation2d[] m_modules;
+  private final SwerveModuleState[] m_moduleStates;
   private Translation2d m_prevCoR = new Translation2d();
 
   /**
    * 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 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.
+   * 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.
    *
    * @param wheelsMeters The locations of the wheels relative to the physical center of the robot.
    */
@@ -54,6 +57,8 @@
     }
     m_numModules = wheelsMeters.length;
     m_modules = Arrays.copyOf(wheelsMeters, m_numModules);
+    m_moduleStates = new SwerveModuleState[m_numModules];
+    Arrays.fill(m_moduleStates, new SwerveModuleState());
     m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
 
     for (int i = 0; i < m_numModules; i++) {
@@ -74,6 +79,9 @@
    * argument is defaulted to that use case. However, if you wish to change the center of rotation
    * for evasive maneuvers, vision alignment, or for any other use case, you can do so.
    *
+   * <p>In the case that the desired chassis speeds are zero (i.e. the robot will be stationary),
+   * the previously calculated module angle will be maintained.
+   *
    * @param chassisSpeeds The desired chassis speed.
    * @param centerOfRotationMeters The center of rotation. For example, if you set the center of
    *     rotation at one corner of the robot and provide a chassis speed that only has a dtheta
@@ -83,9 +91,19 @@
    *     attainable max velocity. Use the {@link #desaturateWheelSpeeds(SwerveModuleState[], double)
    *     DesaturateWheelSpeeds} function to rectify this issue.
    */
-  @SuppressWarnings("LocalVariableName")
+  @SuppressWarnings("PMD.MethodReturnsInternalArray")
   public SwerveModuleState[] toSwerveModuleStates(
       ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
+    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;
+      }
+
+      return m_moduleStates;
+    }
+
     if (!centerOfRotationMeters.equals(m_prevCoR)) {
       for (int i = 0; i < m_numModules; i++) {
         m_inverseKinematics.setRow(
@@ -113,7 +131,6 @@
         chassisSpeeds.omegaRadiansPerSecond);
 
     var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
-    SwerveModuleState[] moduleStates = new SwerveModuleState[m_numModules];
 
     for (int i = 0; i < m_numModules; i++) {
       double x = moduleStatesMatrix.get(i * 2, 0);
@@ -122,10 +139,10 @@
       double speed = Math.hypot(x, y);
       Rotation2d angle = new Rotation2d(x, y);
 
-      moduleStates[i] = new SwerveModuleState(speed, angle);
+      m_moduleStates[i] = new SwerveModuleState(speed, angle);
     }
 
-    return moduleStates;
+    return m_moduleStates;
   }
 
   /**
@@ -171,6 +188,35 @@
   }
 
   /**
+   * 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
+   *     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) {
+      throw new IllegalArgumentException(
+          "Number of modules is not consistent with number of wheel locations provided in "
+              + "constructor");
+    }
+    var moduleDeltaMatrix = new SimpleMatrix(m_numModules * 2, 1);
+
+    for (int i = 0; i < m_numModules; i++) {
+      var module = wheelDeltas[i];
+      moduleDeltaMatrix.set(i * 2, 0, module.distanceMeters * module.angle.getCos());
+      moduleDeltaMatrix.set(i * 2 + 1, module.distanceMeters * module.angle.getSin());
+    }
+
+    var chassisDeltaVector = m_forwardKinematics.mult(moduleDeltaMatrix);
+    return new Twist2d(
+        chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0));
+  }
+
+  /**
    * Renormalizes the wheel speeds if any individual speed is above the specified maximum.
    *
    * <p>Sometimes, after inverse kinematics, the requested speed from one or more modules may be
@@ -192,4 +238,48 @@
       }
     }
   }
+
+  /**
+   * Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well
+   * as getting rid of joystick saturation at edges of joystick.
+   *
+   * <p>Sometimes, after inverse kinematics, the requested speed from one or more modules may be
+   * above the max attainable speed for the driving motor on that module. To fix this issue, one can
+   * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
+   * absolute threshold, while maintaining the ratio of speeds between modules.
+   *
+   * @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 attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach
+   * @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot
+   *     can reach while translating
+   * @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can
+   *     reach while rotating
+   */
+  public static void desaturateWheelSpeeds(
+      SwerveModuleState[] moduleStates,
+      ChassisSpeeds currentChassisSpeed,
+      double attainableMaxModuleSpeedMetersPerSecond,
+      double attainableMaxTranslationalSpeedMetersPerSecond,
+      double attainableMaxRotationalVelocityRadiansPerSecond) {
+    double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
+
+    if (attainableMaxTranslationalSpeedMetersPerSecond == 0
+        || attainableMaxRotationalVelocityRadiansPerSecond == 0
+        || realMaxSpeed == 0) {
+      return;
+    }
+    double translationalK =
+        Math.hypot(currentChassisSpeed.vxMetersPerSecond, currentChassisSpeed.vyMetersPerSecond)
+            / attainableMaxTranslationalSpeedMetersPerSecond;
+    double rotationalK =
+        Math.abs(currentChassisSpeed.omegaRadiansPerSecond)
+            / attainableMaxRotationalVelocityRadiansPerSecond;
+    double k = Math.max(translationalK, rotationalK);
+    double scale = Math.min(k * attainableMaxModuleSpeedMetersPerSecond / realMaxSpeed, 1);
+    for (SwerveModuleState moduleState : moduleStates) {
+      moduleState.speedMetersPerSecond *= scale;
+    }
+  }
 }
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 8b4161e..c2e188f 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
@@ -8,8 +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;
-import edu.wpi.first.util.WPIUtilJNI;
 
 /**
  * Class for swerve drive odometry. Odometry allows you to track the robot's position on the field
@@ -22,24 +20,38 @@
 public class SwerveDriveOdometry {
   private final SwerveDriveKinematics m_kinematics;
   private Pose2d m_poseMeters;
-  private double m_prevTimeSeconds = -1;
 
   private Rotation2d m_gyroOffset;
   private Rotation2d m_previousAngle;
+  private final int m_numModules;
+  private SwerveModulePosition[] m_previousModulePositions;
 
   /**
    * Constructs a SwerveDriveOdometry object.
    *
    * @param kinematics The swerve drive kinematics for your drivetrain.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param modulePositions The wheel positions reported by each module.
    * @param initialPose The starting position of the robot on the field.
    */
   public SwerveDriveOdometry(
-      SwerveDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPose) {
+      SwerveDriveKinematics kinematics,
+      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;
+
+    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);
+    }
+
     MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
   }
 
@@ -48,9 +60,13 @@
    *
    * @param kinematics The swerve drive kinematics for your drivetrain.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param modulePositions The wheel positions reported by each module.
    */
-  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
-    this(kinematics, gyroAngle, new Pose2d());
+  public SwerveDriveOdometry(
+      SwerveDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      SwerveModulePosition[] modulePositions) {
+    this(kinematics, gyroAngle, modulePositions, new Pose2d());
   }
 
   /**
@@ -59,13 +75,28 @@
    * <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 pose The position on the field that your robot is at.
+   * <p>Similarly, module positions do not need to be reset in user code.
+   *
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param modulePositions The wheel positions reported by each module.,
+   * @param pose The position on the field that your robot is at.
    */
-  public void resetPosition(Pose2d pose, Rotation2d gyroAngle) {
+  public void resetPosition(
+      Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) {
+    if (modulePositions.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);
+    }
   }
 
   /**
@@ -79,51 +110,43 @@
 
   /**
    * Updates the robot's position on the field using forward kinematics and integration of the pose
-   * over time. This method takes in the current time as a parameter to calculate period (difference
-   * between two timestamps). The period is used to calculate the change in distance from a
-   * velocity. This also takes in an angle parameter which is used instead of the angular rate that
-   * is calculated from forward kinematics.
-   *
-   * @param currentTimeSeconds The current time in seconds.
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param moduleStates The current state of all swerve modules. Please provide the states in the
-   *     same order in which you instantiated your SwerveDriveKinematics.
-   * @return The new pose of the robot.
-   */
-  public Pose2d updateWithTime(
-      double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
-    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
-    m_prevTimeSeconds = currentTimeSeconds;
-
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var chassisState = m_kinematics.toChassisSpeeds(moduleStates);
-    var newPose =
-        m_poseMeters.exp(
-            new Twist2d(
-                chassisState.vxMetersPerSecond * period,
-                chassisState.vyMetersPerSecond * period,
-                angle.minus(m_previousAngle).getRadians()));
-
-    m_previousAngle = angle;
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-
-    return m_poseMeters;
-  }
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and integration of the pose
    * over time. This method automatically calculates the current time to calculate period
    * (difference between two timestamps). The period is used to calculate the change in distance
    * from a velocity. This also takes in an angle parameter which is used instead of the angular
    * rate that is calculated from forward kinematics.
    *
    * @param gyroAngle The angle reported by the gyroscope.
-   * @param moduleStates The current state of all swerve modules. Please provide the states in the
-   *     same order in which you instantiated your SwerveDriveKinematics.
+   * @param modulePositions The current position of all swerve modules. Please provide the positions
+   *     in the same order in which you instantiated your SwerveDriveKinematics.
    * @return The new pose of the robot.
    */
-  public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
-    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
+  public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
+    if (modulePositions.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;
   }
 }
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
new file mode 100644
index 0000000..cdd7834
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.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.geometry.Rotation2d;
+import java.util.Objects;
+
+/** Represents the state of one swerve module. */
+public class SwerveModulePosition implements Comparable<SwerveModulePosition> {
+  /** Distance measured by the wheel of the module. */
+  public double distanceMeters;
+
+  /** Angle of the module. */
+  public Rotation2d angle = Rotation2d.fromDegrees(0);
+
+  /** Constructs a SwerveModulePosition with zeros for distance and angle. */
+  public SwerveModulePosition() {}
+
+  /**
+   * Constructs a SwerveModulePosition.
+   *
+   * @param distanceMeters The distance measured by the wheel of the module.
+   * @param angle The angle of the module.
+   */
+  public SwerveModulePosition(double distanceMeters, Rotation2d angle) {
+    this.distanceMeters = distanceMeters;
+    this.angle = angle;
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof SwerveModulePosition) {
+      return Double.compare(distanceMeters, ((SwerveModulePosition) obj).distanceMeters) == 0;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(distanceMeters);
+  }
+
+  /**
+   * Compares two swerve module positions. One swerve module is "greater" than the other if its
+   * distance is higher than the other.
+   *
+   * @param other The other swerve module.
+   * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
+   */
+  @Override
+  public int compareTo(SwerveModulePosition other) {
+    return Double.compare(this.distanceMeters, other.distanceMeters);
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "SwerveModulePosition(Distance: %.2f m, Angle: %s)", distanceMeters, angle);
+  }
+}
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 6a9c48c..ec7fd9f 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
@@ -8,7 +8,6 @@
 import java.util.Objects;
 
 /** Represents the state of one swerve module. */
-@SuppressWarnings("MemberName")
 public class SwerveModuleState implements Comparable<SwerveModuleState> {
   /** Speed of the wheel of the module. */
   public double speedMetersPerSecond;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java
index 9bbeaf6..6e13039 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java
@@ -19,7 +19,6 @@
    * @param yInitialControlVector The control vector for the initial point in the y dimension.
    * @param yFinalControlVector The control vector for the final point in the y dimension.
    */
-  @SuppressWarnings("ParameterName")
   public CubicHermiteSpline(
       double[] xInitialControlVector,
       double[] xFinalControlVector,
@@ -81,25 +80,25 @@
   private SimpleMatrix makeHermiteBasis() {
     if (hermiteBasis == null) {
       // 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) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
+      // the coefficients of the spline P(t) = a₃t³ + a₂t² + a₁t + a₀.
       //
-      // P(i)    = P(0)  = a0
-      // P'(i)   = P'(0) = a1
-      // P(i+1)  = P(1)  = a3 + a2 + a1 + a0
-      // P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1
+      // P(i)    = P(0)  = a₀
+      // P'(i)   = P'(0) = a₁
+      // P(i+1)  = P(1)  = a₃ + a₂ + a₁ + a₀
+      // P'(i+1) = P'(1) = 3a₃ + 2a₂ + a₁
       //
-      // [ P(i)    ] = [ 0 0 0 1 ][ a3 ]
-      // [ P'(i)   ] = [ 0 0 1 0 ][ a2 ]
-      // [ P(i+1)  ] = [ 1 1 1 1 ][ a1 ]
-      // [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ]
+      // [P(i)   ] = [0 0 0 1][a₃]
+      // [P'(i)  ] = [0 0 1 0][a₂]
+      // [P(i+1) ] = [1 1 1 1][a₁]
+      // [P'(i+1)] = [3 2 1 0][a₀]
       //
       // To solve for the coefficients, we can invert the 4x4 matrix and move it
       // to the other side of the equation.
       //
-      // [ a3 ] = [  2  1 -2  1 ][ P(i)    ]
-      // [ a2 ] = [ -3 -2  3 -1 ][ P'(i)   ]
-      // [ a1 ] = [  0  1  0  0 ][ P(i+1)  ]
-      // [ a0 ] = [  1  0  0  0 ][ P'(i+1) ]
+      // [a₃] = [ 2  1 -2  1][P(i)   ]
+      // [a₂] = [-3 -2  3 -1][P'(i)  ]
+      // [a₁] = [ 0  1  0  0][P(i+1) ]
+      // [a₀] = [ 1  0  0  0][P'(i+1)]
       hermiteBasis =
           new SimpleMatrix(
               4,
diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java b/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java
index 8bad7b1..30b3cae 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java
@@ -7,7 +7,6 @@
 import edu.wpi.first.math.geometry.Pose2d;
 
 /** Represents a pair of a pose and a curvature. */
-@SuppressWarnings("MemberName")
 public class PoseWithCurvature {
   // Represents the pose.
   public Pose2d poseMeters;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java
index 4017044..be90999 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java
@@ -19,7 +19,6 @@
    * @param yInitialControlVector The control vector for the initial point in the y dimension.
    * @param yFinalControlVector The control vector for the final point in the y dimension.
    */
-  @SuppressWarnings("ParameterName")
   public QuinticHermiteSpline(
       double[] xInitialControlVector,
       double[] xFinalControlVector,
@@ -80,33 +79,33 @@
    */
   private SimpleMatrix makeHermiteBasis() {
     if (hermiteBasis == null) {
-      // Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
-      // vectors, we want to find the coefficients of the spline
-      // P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
+      // Given P(i), P'(i), P"(i), P(i+1), 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₂t² + a₁t + a₀.
       //
-      // P(i)     = P(0)   = a0
-      // P'(i)    = P'(0)  = a1
-      // P''(i)   = P''(0) = 2 * a2
-      // P(i+1)   = P(1)   = a5 + a4 + a3 + a2 + a1 + a0
-      // P'(i+1)  = P'(1)  = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1
-      // P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2
+      // P(i)    = P(0)  = a₀
+      // P'(i)   = P'(0) = a₁
+      // P''(i)  = P"(0) = 2a₂
+      // P(i+1)  = P(1)  = a₅ + a₄ + a₃ + a₂ + a₁ + a₀
+      // P'(i+1) = P'(1) = 5a₅ + 4a₄ + 3a₃ + 2a₂ + a₁
+      // P"(i+1) = P"(1) = 20a₅ + 12a₄ + 6a₃ + 2a₂
       //
-      // [ P(i)     ] = [  0  0  0  0  0  1 ][ a5 ]
-      // [ P'(i)    ] = [  0  0  0  0  1  0 ][ a4 ]
-      // [ P''(i)   ] = [  0  0  0  2  0  0 ][ a3 ]
-      // [ P(i+1)   ] = [  1  1  1  1  1  1 ][ a2 ]
-      // [ P'(i+1)  ] = [  5  4  3  2  1  0 ][ a1 ]
-      // [ P''(i+1) ] = [ 20 12  6  2  0  0 ][ a0 ]
+      // [P(i)   ] = [ 0  0  0  0  0  1][a₅]
+      // [P'(i)  ] = [ 0  0  0  0  1  0][a₄]
+      // [P"(i)  ] = [ 0  0  0  2  0  0][a₃]
+      // [P(i+1) ] = [ 1  1  1  1  1  1][a₂]
+      // [P'(i+1)] = [ 5  4  3  2  1  0][a₁]
+      // [P"(i+1)] = [20 12  6  2  0  0][a₀]
       //
       // To solve for the coefficients, we can invert the 6x6 matrix and move it
       // to the other side of the equation.
       //
-      // [ a5 ] = [  -6.0  -3.0  -0.5   6.0  -3.0   0.5 ][ P(i)     ]
-      // [ a4 ] = [  15.0   8.0   1.5 -15.0   7.0  -1.0 ][ P'(i)    ]
-      // [ a3 ] = [ -10.0  -6.0  -1.5  10.0  -4.0   0.5 ][ P''(i)   ]
-      // [ a2 ] = [   0.0   0.0   0.5   0.0   0.0   0.0 ][ P(i+1)   ]
-      // [ a1 ] = [   0.0   1.0   0.0   0.0   0.0   0.0 ][ P'(i+1)  ]
-      // [ a0 ] = [   1.0   0.0   0.0   0.0   0.0   0.0 ][ P''(i+1) ]
+      // [a₅] = [ -6.0  -3.0  -0.5   6.0  -3.0   0.5][P(i)   ]
+      // [a₄] = [ 15.0   8.0   1.5 -15.0   7.0  -1.0][P'(i)  ]
+      // [a₃] = [-10.0  -6.0  -1.5  10.0  -4.0   0.5][P"(i)  ]
+      // [a₂] = [  0.0   0.0   0.5   0.0   0.0   0.0][P(i+1) ]
+      // [a₁] = [  0.0   1.0   0.0   0.0   0.0   0.0][P'(i+1)]
+      // [a₀] = [  1.0   0.0   0.0   0.0   0.0   0.0][P"(i+1)]
       hermiteBasis =
           new SimpleMatrix(
               6,
diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java b/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java
index 5451eea..83b35f3 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java
@@ -35,7 +35,6 @@
    * @param t The point t
    * @return The pose and curvature at that point.
    */
-  @SuppressWarnings("ParameterName")
   public PoseWithCurvature getPoint(double t) {
     SimpleMatrix polynomialBases = new SimpleMatrix(m_degree + 1, 1);
     final var coefficients = getCoefficients();
@@ -46,7 +45,7 @@
     }
 
     // This simply multiplies by the coefficients. We need to divide out t some
-    // n number of times where n is the derivative we want to take.
+    // n number of times when n is the derivative we want to take.
     SimpleMatrix combined = coefficients.mult(polynomialBases);
 
     // Get x and y
@@ -85,7 +84,6 @@
    * <p>Each element in each array represents the value of the derivative at the index. For example,
    * the value of x[2] is the second derivative in the x dimension.
    */
-  @SuppressWarnings("MemberName")
   public static class ControlVector {
     public double[] x;
     public double[] y;
@@ -96,7 +94,6 @@
      * @param x The x dimension of the control vector.
      * @param y The y dimension of the control vector.
      */
-    @SuppressWarnings("ParameterName")
     public ControlVector(double[] x, double[] y) {
       this.x = Arrays.copyOf(x, x.length);
       this.y = Arrays.copyOf(y, y.length);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java
index e5c67f8..651b028 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java
@@ -46,7 +46,7 @@
    * Returns quintic splines from a set of waypoints.
    *
    * @param waypoints The waypoints
-   * @return List of splines.
+   * @return array of splines.
    */
   public static QuinticHermiteSpline[] getQuinticSplinesFromWaypoints(List<Pose2d> waypoints) {
     QuinticHermiteSpline[] splines = new QuinticHermiteSpline[waypoints.size() - 1];
@@ -78,7 +78,6 @@
    * @return A vector of cubic hermite splines that interpolate through the provided waypoints and
    *     control vectors.
    */
-  @SuppressWarnings("LocalVariableName")
   public static CubicHermiteSpline[] getCubicSplinesFromControlVectors(
       Spline.ControlVector start, Translation2d[] waypoints, Spline.ControlVector end) {
     CubicHermiteSpline[] splines = new CubicHermiteSpline[waypoints.length + 1];
@@ -202,7 +201,6 @@
    * @param controlVectors The control vectors.
    * @return A vector of quintic hermite splines that interpolate through the provided waypoints.
    */
-  @SuppressWarnings("LocalVariableName")
   public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors(
       Spline.ControlVector[] controlVectors) {
     QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length - 1];
@@ -228,7 +226,6 @@
    * @param d the vector on the rhs
    * @param solutionVector the unknown (solution) vector, modified in-place
    */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
   private static void thomasAlgorithm(
       double[] a, double[] b, double[] c, double[] d, double[] solutionVector) {
     int N = d.length;
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 88afc6d..c1a87f2 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
@@ -46,7 +46,6 @@
    */
   private static final int kMaxIterations = 5000;
 
-  @SuppressWarnings("MemberName")
   private static class StackContents {
     final double t1;
     final double t0;
@@ -57,7 +56,6 @@
     }
   }
 
-  @SuppressWarnings("serial")
   public static class MalformedSplineException extends RuntimeException {
     /**
      * Create a new exception with the given message.
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 ffbd99e..1871803 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
@@ -9,7 +9,6 @@
 import edu.wpi.first.math.Pair;
 import org.ejml.simple.SimpleMatrix;
 
-@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
 public final class Discretization {
   private Discretization() {
     // Utility class
@@ -25,6 +24,7 @@
    */
   public static <States extends Num> Matrix<States, States> discretizeA(
       Matrix<States, States> contA, double dtSeconds) {
+    // A_d = eᴬᵀ
     return contA.times(dtSeconds).exp();
   }
 
@@ -38,24 +38,26 @@
    * @param dtSeconds Discretization timestep.
    * @return a Pair representing discA and diskB.
    */
-  @SuppressWarnings("LocalVariableName")
   public static <States extends Num, Inputs extends Num>
       Pair<Matrix<States, States>, Matrix<States, Inputs>> discretizeAB(
           Matrix<States, States> contA, Matrix<States, Inputs> contB, double dtSeconds) {
-    var scaledA = contA.times(dtSeconds);
-    var scaledB = contB.times(dtSeconds);
-
     int states = contA.getNumRows();
     int inputs = contB.getNumCols();
+
+    // M = [A  B]
+    //     [0  0]
     var M = new Matrix<>(new SimpleMatrix(states + inputs, states + inputs));
-    M.assignBlock(0, 0, scaledA);
-    M.assignBlock(0, scaledA.getNumCols(), scaledB);
-    var phi = M.exp();
+    M.assignBlock(0, 0, contA);
+    M.assignBlock(0, contA.getNumCols(), contB);
+
+    //  ϕ = eᴹᵀ = [A_d  B_d]
+    //            [ 0    I ]
+    var phi = M.times(dtSeconds).exp();
 
     var discA = new Matrix<States, States>(new SimpleMatrix(states, states));
-    var discB = new Matrix<States, Inputs>(new SimpleMatrix(states, inputs));
-
     discA.extractFrom(0, 0, phi);
+
+    var discB = new Matrix<States, Inputs>(new SimpleMatrix(states, inputs));
     discB.extractFrom(0, contB.getNumRows(), phi);
 
     return new Pair<>(discA, discB);
@@ -70,7 +72,6 @@
    * @param dtSeconds Discretization timestep.
    * @return a pair representing the discrete system matrix and process noise covariance matrix.
    */
-  @SuppressWarnings("LocalVariableName")
   public static <States extends Num>
       Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQ(
           Matrix<States, States> contA, Matrix<States, States> contQ, double dtSeconds) {
@@ -79,18 +80,22 @@
     // Make continuous Q symmetric if it isn't already
     Matrix<States, States> Q = contQ.plus(contQ.transpose()).div(2.0);
 
-    // Set up the matrix M = [[-A, Q], [0, A.T]]
+    // M = [−A  Q ]
+    //     [ 0  Aᵀ]
     final var M = new Matrix<>(new SimpleMatrix(2 * states, 2 * states));
     M.assignBlock(0, 0, contA.times(-1.0));
     M.assignBlock(0, states, Q);
     M.assignBlock(states, 0, new Matrix<>(new SimpleMatrix(states, states)));
     M.assignBlock(states, states, contA.transpose());
 
+    // ϕ = eᴹᵀ = [−A_d  A_d⁻¹Q_d]
+    //           [ 0      A_dᵀ  ]
     final var phi = M.times(dtSeconds).exp();
 
-    // Phi12 = phi[0:States,        States:2*States]
-    // Phi22 = phi[States:2*States, States:2*States]
+    // ϕ₁₂ = A_d⁻¹Q_d
     final Matrix<States, States> phi12 = phi.block(states, states, 0, states);
+
+    // ϕ₂₂ = A_dᵀ
     final Matrix<States, States> phi22 = phi.block(states, states, states, states);
 
     final var discA = phi22.transpose();
@@ -109,9 +114,12 @@
    * <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.
    *
-   * <p>The exponential of A*t, which is only N x N, is relatively cheap. 2) 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>
+   *   <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.
@@ -119,10 +127,44 @@
    * @param dtSeconds Discretization timestep.
    * @return a pair representing the discrete system matrix and process noise covariance matrix.
    */
-  @SuppressWarnings("LocalVariableName")
   public static <States extends Num>
       Pair<Matrix<States, States>, Matrix<States, States>> 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);
 
@@ -130,17 +172,18 @@
     double lastCoeff = dtSeconds;
 
     // Aᵀⁿ
-    Matrix<States, States> Atn = contA.transpose();
+    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));
+      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());
+      ATn = ATn.times(contA.transpose());
     }
 
     var discA = discretizeA(contA, dtSeconds);
@@ -157,11 +200,12 @@
    * Note that dt=0.0 divides R by zero.
    *
    * @param <O> Nat representing the number of outputs.
-   * @param R Continuous measurement noise covariance matrix.
+   * @param contR Continuous measurement noise covariance matrix.
    * @param dtSeconds Discretization timestep.
    * @return Discretized version of the provided continuous measurement noise covariance matrix.
    */
-  public static <O extends Num> Matrix<O, O> discretizeR(Matrix<O, O> R, double dtSeconds) {
-    return R.div(dtSeconds);
+  public static <O extends Num> Matrix<O, O> discretizeR(Matrix<O, O> contR, double dtSeconds) {
+    // R_d = 1/T R
+    return contR.div(dtSeconds);
   }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
index 9e35cfd..d7b6602 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
@@ -8,76 +8,70 @@
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.numbers.N1;
 
-@SuppressWarnings("ClassTypeParameterName")
 public class LinearSystem<States extends Num, Inputs extends Num, Outputs extends Num> {
   /** Continuous system matrix. */
-  @SuppressWarnings("MemberName")
   private final Matrix<States, States> m_A;
 
   /** Continuous input matrix. */
-  @SuppressWarnings("MemberName")
   private final Matrix<States, Inputs> m_B;
 
   /** Output matrix. */
-  @SuppressWarnings("MemberName")
   private final Matrix<Outputs, States> m_C;
 
   /** Feedthrough matrix. */
-  @SuppressWarnings("MemberName")
   private final Matrix<Outputs, Inputs> m_D;
 
   /**
    * Construct a new LinearSystem from the four system matrices.
    *
-   * @param a The system matrix A.
-   * @param b The input matrix B.
-   * @param c The output matrix C.
-   * @param d The feedthrough matrix D.
+   * @param A The system matrix A.
+   * @param B The input matrix B.
+   * @param C The output matrix C.
+   * @param D The feedthrough matrix D.
    * @throws IllegalArgumentException if any matrix element isn't finite.
    */
-  @SuppressWarnings("ParameterName")
   public LinearSystem(
-      Matrix<States, States> a,
-      Matrix<States, Inputs> b,
-      Matrix<Outputs, States> c,
-      Matrix<Outputs, Inputs> d) {
-    for (int row = 0; row < a.getNumRows(); ++row) {
-      for (int col = 0; col < a.getNumCols(); ++col) {
-        if (!Double.isFinite(a.get(row, col))) {
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<Outputs, States> C,
+      Matrix<Outputs, Inputs> D) {
+    for (int row = 0; row < A.getNumRows(); ++row) {
+      for (int col = 0; col < A.getNumCols(); ++col) {
+        if (!Double.isFinite(A.get(row, col))) {
           throw new IllegalArgumentException(
               "Elements of A aren't finite. This is usually due to model implementation errors.");
         }
       }
     }
-    for (int row = 0; row < b.getNumRows(); ++row) {
-      for (int col = 0; col < b.getNumCols(); ++col) {
-        if (!Double.isFinite(b.get(row, col))) {
+    for (int row = 0; row < B.getNumRows(); ++row) {
+      for (int col = 0; col < B.getNumCols(); ++col) {
+        if (!Double.isFinite(B.get(row, col))) {
           throw new IllegalArgumentException(
               "Elements of B aren't finite. This is usually due to model implementation errors.");
         }
       }
     }
-    for (int row = 0; row < c.getNumRows(); ++row) {
-      for (int col = 0; col < c.getNumCols(); ++col) {
-        if (!Double.isFinite(c.get(row, col))) {
+    for (int row = 0; row < C.getNumRows(); ++row) {
+      for (int col = 0; col < C.getNumCols(); ++col) {
+        if (!Double.isFinite(C.get(row, col))) {
           throw new IllegalArgumentException(
               "Elements of C aren't finite. This is usually due to model implementation errors.");
         }
       }
     }
-    for (int row = 0; row < d.getNumRows(); ++row) {
-      for (int col = 0; col < d.getNumCols(); ++col) {
-        if (!Double.isFinite(d.get(row, col))) {
+    for (int row = 0; row < D.getNumRows(); ++row) {
+      for (int col = 0; col < D.getNumCols(); ++col) {
+        if (!Double.isFinite(D.get(row, col))) {
           throw new IllegalArgumentException(
               "Elements of D aren't finite. This is usually due to model implementation errors.");
         }
       }
     }
 
-    this.m_A = a;
-    this.m_B = b;
-    this.m_C = c;
-    this.m_D = d;
+    this.m_A = A;
+    this.m_B = B;
+    this.m_C = C;
+    this.m_D = D;
   }
 
   /**
@@ -170,7 +164,6 @@
    * @param dtSeconds Timestep for model update.
    * @return the updated x.
    */
-  @SuppressWarnings("ParameterName")
   public Matrix<States, N1> calculateX(
       Matrix<States, N1> x, Matrix<Inputs, N1> clampedU, double dtSeconds) {
     var discABpair = Discretization.discretizeAB(m_A, m_B, dtSeconds);
@@ -187,7 +180,6 @@
    * @param clampedU The control input.
    * @return the updated output matrix Y.
    */
-  @SuppressWarnings("ParameterName")
   public Matrix<Outputs, N1> calculateY(Matrix<States, N1> x, Matrix<Inputs, N1> clampedU) {
     return m_C.times(x).plus(m_D.times(clampedU));
   }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
index dcd4e58..02b1da0 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
@@ -21,14 +21,13 @@
  *
  * <p>For everything in this file, "inputs" and "outputs" are defined from the perspective of the
  * plant. This means U is an input and Y is an output (because you give the plant U (powers) and it
- * gives you back a Y (sensor values). This is the opposite of what they mean from the perspective
+ * gives you back a Y (sensor values)). This is the opposite of what they mean from the perspective
  * of the controller (U is an output because that's what goes to the motors and Y is an input
  * because that's what comes back from the sensors).
  *
  * <p>For more on the underlying math, read
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
  */
-@SuppressWarnings("ClassTypeParameterName")
 public class LinearSystemLoop<States extends Num, Inputs extends Num, Outputs extends Num> {
   private final LinearQuadraticRegulator<States, Inputs, Outputs> m_controller;
   private final LinearPlantInversionFeedforward<States, Inputs, Outputs> m_feedforward;
@@ -315,7 +314,6 @@
    *
    * @param y Measurement vector.
    */
-  @SuppressWarnings("ParameterName")
   public void correct(Matrix<Outputs, N1> y) {
     getObserver().correct(getU(), y);
   }
@@ -327,7 +325,6 @@
    *
    * @param dtSeconds Timestep for model update.
    */
-  @SuppressWarnings("LocalVariableName")
   public void predict(double dtSeconds) {
     var u =
         clampInput(
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 274b10a..94be369 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
@@ -24,7 +24,7 @@
    * @param dtSeconds The time over which to integrate.
    * @return the integration of dx/dt = f(x) for dt.
    */
-  @SuppressWarnings("ParameterName")
+  @SuppressWarnings("overloads")
   public static double rk4(DoubleFunction<Double> f, double x, double dtSeconds) {
     final var h = dtSeconds;
     final var k1 = f.apply(x);
@@ -44,7 +44,7 @@
    * @param dtSeconds The time over which to integrate.
    * @return The result of Runge Kutta integration (4th order).
    */
-  @SuppressWarnings("ParameterName")
+  @SuppressWarnings("overloads")
   public static double rk4(
       BiFunction<Double, Double, Double> f, double x, Double u, double dtSeconds) {
     final var h = dtSeconds;
@@ -68,7 +68,7 @@
    * @param dtSeconds The time over which to integrate.
    * @return the integration of dx/dt = f(x, u) for dt.
    */
-  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  @SuppressWarnings("overloads")
   public static <States extends Num, Inputs extends Num> Matrix<States, N1> rk4(
       BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
       Matrix<States, N1> x,
@@ -93,7 +93,7 @@
    * @param dtSeconds The time over which to integrate.
    * @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
    */
-  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  @SuppressWarnings("overloads")
   public static <States extends Num> Matrix<States, N1> rk4(
       Function<Matrix<States, N1>, Matrix<States, N1>> f, Matrix<States, N1> x, double dtSeconds) {
     final var h = dtSeconds;
@@ -107,139 +107,6 @@
   }
 
   /**
-   * Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described in
-   * https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method. By default, the max
-   * error is 1e-6.
-   *
-   * @param <States> A Num representing the states of the system to integrate.
-   * @param <Inputs> A Num representing the inputs of the system to integrate.
-   * @param f The function to integrate. It must take two arguments x and u.
-   * @param x The initial value of x.
-   * @param u The value u held constant over the integration period.
-   * @param dtSeconds The time over which to integrate.
-   * @return the integration of dx/dt = f(x, u) for dt.
-   */
-  @SuppressWarnings("MethodTypeParameterName")
-  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkf45(
-      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
-      Matrix<States, N1> x,
-      Matrix<Inputs, N1> u,
-      double dtSeconds) {
-    return rkf45(f, x, u, dtSeconds, 1e-6);
-  }
-
-  /**
-   * Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described in
-   * https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
-   *
-   * @param <States> A Num representing the states of the system to integrate.
-   * @param <Inputs> A Num representing the inputs of the system to integrate.
-   * @param f The function to integrate. It must take two arguments x and u.
-   * @param x The initial value of x.
-   * @param u The value u held constant over the integration period.
-   * @param dtSeconds The time over which to integrate.
-   * @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6.
-   * @return the integration of dx/dt = f(x, u) for dt.
-   */
-  @SuppressWarnings("MethodTypeParameterName")
-  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkf45(
-      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
-      Matrix<States, N1> x,
-      Matrix<Inputs, N1> u,
-      double dtSeconds,
-      double maxError) {
-    // See
-    // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
-    // for the Butcher tableau the following arrays came from.
-
-    // final double[5][5]
-    final double[][] A = {
-      {1.0 / 4.0},
-      {3.0 / 32.0, 9.0 / 32.0},
-      {1932.0 / 2197.0, -7200.0 / 2197.0, 7296.0 / 2197.0},
-      {439.0 / 216.0, -8.0, 3680.0 / 513.0, -845.0 / 4104.0},
-      {-8.0 / 27.0, 2.0, -3544.0 / 2565.0, 1859.0 / 4104.0, -11.0 / 40.0}
-    };
-
-    // final double[6]
-    final double[] b1 = {
-      16.0 / 135.0, 0.0, 6656.0 / 12825.0, 28561.0 / 56430.0, -9.0 / 50.0, 2.0 / 55.0
-    };
-
-    // final double[6]
-    final double[] b2 = {25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0};
-
-    Matrix<States, N1> newX;
-    double truncationError;
-
-    double dtElapsed = 0.0;
-    double h = dtSeconds;
-
-    // Loop until we've gotten to our desired dt
-    while (dtElapsed < dtSeconds) {
-      do {
-        // Only allow us to advance up to the dt remaining
-        h = Math.min(h, dtSeconds - dtElapsed);
-
-        // Notice how the derivative in the Wikipedia notation is dy/dx.
-        // That means their y is our x and their x is our t
-        var k1 = f.apply(x, u);
-        var k2 = f.apply(x.plus(k1.times(A[0][0]).times(h)), u);
-        var k3 = f.apply(x.plus(k1.times(A[1][0]).plus(k2.times(A[1][1])).times(h)), u);
-        var k4 =
-            f.apply(
-                x.plus(k1.times(A[2][0]).plus(k2.times(A[2][1])).plus(k3.times(A[2][2])).times(h)),
-                u);
-        var k5 =
-            f.apply(
-                x.plus(
-                    k1.times(A[3][0])
-                        .plus(k2.times(A[3][1]))
-                        .plus(k3.times(A[3][2]))
-                        .plus(k4.times(A[3][3]))
-                        .times(h)),
-                u);
-        var k6 =
-            f.apply(
-                x.plus(
-                    k1.times(A[4][0])
-                        .plus(k2.times(A[4][1]))
-                        .plus(k3.times(A[4][2]))
-                        .plus(k4.times(A[4][3]))
-                        .plus(k5.times(A[4][4]))
-                        .times(h)),
-                u);
-
-        newX =
-            x.plus(
-                k1.times(b1[0])
-                    .plus(k2.times(b1[1]))
-                    .plus(k3.times(b1[2]))
-                    .plus(k4.times(b1[3]))
-                    .plus(k5.times(b1[4]))
-                    .plus(k6.times(b1[5]))
-                    .times(h));
-        truncationError =
-            (k1.times(b1[0] - b2[0])
-                    .plus(k2.times(b1[1] - b2[1]))
-                    .plus(k3.times(b1[2] - b2[2]))
-                    .plus(k4.times(b1[3] - b2[3]))
-                    .plus(k5.times(b1[4] - b2[4]))
-                    .plus(k6.times(b1[5] - b2[5]))
-                    .times(h))
-                .normF();
-
-        h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
-      } while (truncationError > maxError);
-
-      dtElapsed += h;
-      x = newX;
-    }
-
-    return x;
-  }
-
-  /**
    * Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt. By default, the max
    * error is 1e-6.
    *
@@ -251,7 +118,7 @@
    * @param dtSeconds The time over which to integrate.
    * @return the integration of dx/dt = f(x, u) for dt.
    */
-  @SuppressWarnings("MethodTypeParameterName")
+  @SuppressWarnings("overloads")
   public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
       BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
       Matrix<States, N1> x,
@@ -272,7 +139,7 @@
    * @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6.
    * @return the integration of dx/dt = f(x, u) for dt.
    */
-  @SuppressWarnings("MethodTypeParameterName")
+  @SuppressWarnings("overloads")
   public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
       BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
       Matrix<States, N1> x,
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java
index 6c2c896..43eba93 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java
@@ -30,7 +30,6 @@
    * @param x Vector argument.
    * @return The numerical Jacobian with respect to x for f(x, u, ...).
    */
-  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
   public static <Rows extends Num, Cols extends Num, States extends Num>
       Matrix<Rows, Cols> numericalJacobian(
           Nat<Rows> rows,
@@ -44,7 +43,6 @@
       var dxMinus = x.copy();
       dxPlus.set(i, 0, dxPlus.get(i, 0) + kEpsilon);
       dxMinus.set(i, 0, dxMinus.get(i, 0) - kEpsilon);
-      @SuppressWarnings("LocalVariableName")
       var dF = f.apply(dxPlus).minus(f.apply(dxMinus)).div(2 * kEpsilon);
 
       result.setColumn(i, Matrix.changeBoundsUnchecked(dF));
@@ -67,7 +65,6 @@
    * @param u Input vector.
    * @return The numerical Jacobian with respect to x for f(x, u, ...).
    */
-  @SuppressWarnings({"LambdaParameterName", "MethodTypeParameterName"})
   public static <Rows extends Num, States extends Num, Inputs extends Num, Outputs extends Num>
       Matrix<Rows, States> numericalJacobianX(
           Nat<Rows> rows,
@@ -91,7 +88,6 @@
    * @param u Input vector.
    * @return the numerical Jacobian with respect to u for f(x, u).
    */
-  @SuppressWarnings({"LambdaParameterName", "MethodTypeParameterName"})
   public static <Rows extends Num, States extends Num, Inputs extends Num>
       Matrix<Rows, Inputs> numericalJacobianU(
           Nat<Rows> rows,
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 94c117f..cd431f7 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
@@ -8,28 +8,13 @@
 
 /** Holds the constants for a DC motor. */
 public class DCMotor {
-  @SuppressWarnings("MemberName")
   public final double nominalVoltageVolts;
-
-  @SuppressWarnings("MemberName")
   public final double stallTorqueNewtonMeters;
-
-  @SuppressWarnings("MemberName")
   public final double stallCurrentAmps;
-
-  @SuppressWarnings("MemberName")
   public final double freeCurrentAmps;
-
-  @SuppressWarnings("MemberName")
   public final double freeSpeedRadPerSec;
-
-  @SuppressWarnings("MemberName")
   public final double rOhms;
-
-  @SuppressWarnings("MemberName")
   public final double KvRadPerSecPerVolt;
-
-  @SuppressWarnings("MemberName")
   public final double KtNMPerAmp;
 
   /**
@@ -64,7 +49,7 @@
   /**
    * Estimate the current being drawn by this motor.
    *
-   * @param speedRadiansPerSec The speed of the rotor.
+   * @param speedRadiansPerSec The speed of the motor.
    * @param voltageInputVolts The input voltage.
    * @return The estimated current.
    */
@@ -73,6 +58,54 @@
   }
 
   /**
+   * Calculate the torque produced by the motor for a given current.
+   *
+   * @param currentAmpere The current drawn by the motor.
+   * @return The torque produced.
+   */
+  public double getTorque(double currentAmpere) {
+    return currentAmpere * KtNMPerAmp;
+  }
+
+  /**
+   * Calculate the voltage provided to the motor at a given torque and angular velocity.
+   *
+   * @param torqueNm The torque produced by the motor.
+   * @param speedRadiansPerSec The speed of the motor.
+   * @return The voltage of the motor.
+   */
+  public double getVoltage(double torqueNm, double speedRadiansPerSec) {
+    return 1.0 / KvRadPerSecPerVolt * speedRadiansPerSec + 1.0 / KtNMPerAmp * rOhms * torqueNm;
+  }
+
+  /**
+   * Calculate the speed of 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.
+   */
+  public double getSpeed(double torqueNm, double voltageInputVolts) {
+    return voltageInputVolts - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt;
+  }
+
+  /**
+   * Returns a copy of this motor with the given gearbox reduction applied.
+   *
+   * @param gearboxReduction The gearbox reduction.
+   * @return A motor with the gearbox reduction applied.
+   */
+  public DCMotor withReduction(double gearboxReduction) {
+    return new DCMotor(
+        nominalVoltageVolts,
+        stallTorqueNewtonMeters * gearboxReduction,
+        stallCurrentAmps,
+        freeCurrentAmps,
+        freeSpeedRadPerSec / gearboxReduction,
+        1);
+  }
+
+  /**
    * Return a gearbox of CIM motors.
    *
    * @param numMotors Number of motors in the gearbox.
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 1a05eb6..332e690 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
@@ -20,14 +20,13 @@
    * Create a state-space model of an elevator system. The states of the system are [position,
    * velocity]ᵀ, inputs are [voltage], and outputs are [position].
    *
-   * @param motor The motor (or gearbox) attached to the arm.
+   * @param motor The motor (or gearbox) attached to the carriage.
    * @param massKg The mass of the elevator carriage, in kilograms.
-   * @param radiusMeters The radius of thd driving drum of the elevator, in meters.
+   * @param radiusMeters The radius of the elevator's driving drum, in meters.
    * @param G The reduction between motor and drum, as a ratio of output to input.
    * @return A LinearSystem representing the given characterized constants.
    * @throws IllegalArgumentException if massKg &lt;= 0, radiusMeters &lt;= 0, or G &lt;= 0.
    */
-  @SuppressWarnings("ParameterName")
   public static LinearSystem<N2, N1, N1> createElevatorSystem(
       DCMotor motor, double massKg, double radiusMeters, double G) {
     if (massKg <= 0.0) {
@@ -63,15 +62,14 @@
    * velocity], inputs are [voltage], and outputs are [angular velocity].
    *
    * @param motor The motor (or gearbox) attached to the flywheel.
-   * @param jKgMetersSquared The moment of inertia J of the flywheel.
+   * @param JKgMetersSquared The moment of inertia J of the flywheel.
    * @param G The reduction between motor and drum, as a ratio of output to input.
    * @return A LinearSystem representing the given characterized constants.
-   * @throws IllegalArgumentException if jKgMetersSquared &lt;= 0 or G &lt;= 0.
+   * @throws IllegalArgumentException if JKgMetersSquared &lt;= 0 or G &lt;= 0.
    */
-  @SuppressWarnings("ParameterName")
   public static LinearSystem<N1, N1, N1> createFlywheelSystem(
-      DCMotor motor, double jKgMetersSquared, double G) {
-    if (jKgMetersSquared <= 0.0) {
+      DCMotor motor, double JKgMetersSquared, double G) {
+    if (JKgMetersSquared <= 0.0) {
       throw new IllegalArgumentException("J must be greater than zero.");
     }
     if (G <= 0.0) {
@@ -83,8 +81,8 @@
             -G
                 * G
                 * motor.KtNMPerAmp
-                / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgMetersSquared)),
-        VecBuilder.fill(G * motor.KtNMPerAmp / (motor.rOhms * jKgMetersSquared)),
+                / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)),
+        VecBuilder.fill(G * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)),
         Matrix.eye(Nat.N1()),
         new Matrix<>(Nat.N1(), Nat.N1()));
   }
@@ -94,16 +92,15 @@
    * position, angular velocity], inputs are [voltage], and outputs are [angular position, angular
    * velocity].
    *
-   * @param motor The motor (or gearbox) attached to the DC motor.
-   * @param jKgMetersSquared The moment of inertia J of the DC motor.
+   * @param motor The motor (or gearbox) attached to system.
+   * @param JKgMetersSquared The moment of inertia J of the DC motor.
    * @param G The reduction between motor and drum, as a ratio of output to input.
    * @return A LinearSystem representing the given characterized constants.
-   * @throws IllegalArgumentException if jKgMetersSquared &lt;= 0 or G &lt;= 0.
+   * @throws IllegalArgumentException if JKgMetersSquared &lt;= 0 or G &lt;= 0.
    */
-  @SuppressWarnings("ParameterName")
   public static LinearSystem<N2, N1, N2> createDCMotorSystem(
-      DCMotor motor, double jKgMetersSquared, double G) {
-    if (jKgMetersSquared <= 0.0) {
+      DCMotor motor, double JKgMetersSquared, double G) {
+    if (JKgMetersSquared <= 0.0) {
       throw new IllegalArgumentException("J must be greater than zero.");
     }
     if (G <= 0.0) {
@@ -119,26 +116,26 @@
                 -G
                     * G
                     * motor.KtNMPerAmp
-                    / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgMetersSquared)),
-        VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * jKgMetersSquared)),
+                    / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)),
+        VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)),
         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
-   * [v_left, v_right]ᵀ, inputs are [V_left, V_right]ᵀ and outputs are [v_left, v_right]ᵀ.
+   * [left velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are
+   * [left velocity, right velocity]ᵀ.
    *
-   * @param motor the gearbox representing the motors driving the drivetrain.
-   * @param massKg the mass of the robot.
-   * @param rMeters the radius of the wheels in meters.
-   * @param rbMeters the radius of the base (half the track width) in meters.
-   * @param JKgMetersSquared the moment of inertia of the robot.
-   * @param G the gearing reduction as output over input.
+   * @param motor The motor (or gearbox) driving the drivetrain.
+   * @param massKg The mass of the robot in kilograms.
+   * @param rMeters The radius of the wheels in meters.
+   * @param rbMeters The radius of the base (half the track width) in meters.
+   * @param JKgMetersSquared The moment of inertia of the robot.
+   * @param G The gearing reduction as output over input.
    * @return A LinearSystem representing a differential drivetrain.
    * @throws IllegalArgumentException if m &lt;= 0, r &lt;= 0, rb &lt;= 0, J &lt;= 0, or G &lt;= 0.
    */
-  @SuppressWarnings({"LocalVariableName", "ParameterName"})
   public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem(
       DCMotor motor,
       double massKg,
@@ -181,16 +178,15 @@
    * angular velocity], inputs are [voltage], and outputs are [angle].
    *
    * @param motor The motor (or gearbox) attached to the arm.
-   * @param jKgSquaredMeters The moment of inertia J of the arm.
+   * @param JKgSquaredMeters The moment of inertia J of the arm.
    * @param G The gearing between the motor and arm, in output over input. Most of the time this
    *     will be greater than 1.
    * @return A LinearSystem representing the given characterized constants.
    */
-  @SuppressWarnings("ParameterName")
   public static LinearSystem<N2, N1, N1> createSingleJointedArmSystem(
-      DCMotor motor, double jKgSquaredMeters, double G) {
-    if (jKgSquaredMeters <= 0.0) {
-      throw new IllegalArgumentException("jKgSquaredMeters must be greater than zero.");
+      DCMotor motor, double JKgSquaredMeters, double G) {
+    if (JKgSquaredMeters <= 0.0) {
+      throw new IllegalArgumentException("JKgSquaredMeters must be greater than zero.");
     }
     if (G <= 0.0) {
       throw new IllegalArgumentException("G must be greater than zero.");
@@ -204,27 +200,30 @@
                 0,
                 -Math.pow(G, 2)
                     * motor.KtNMPerAmp
-                    / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgSquaredMeters)),
-        VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * jKgSquaredMeters)),
+                    / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)),
+        VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * JKgSquaredMeters)),
         Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
         new Matrix<>(Nat.N1(), Nat.N1()));
   }
 
   /**
-   * Identify a velocity system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). These
-   * constants cam be found using SysId. The states of the system are [velocity], inputs are
-   * [voltage], and outputs are [velocity].
+   * Create a state-space model for a 1 DOF velocity system from its kV (volts/(unit/sec)) and kA
+   * (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are
+   * [velocity], inputs are [voltage], and outputs are [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.
    *
-   * @param kV The velocity gain, in volts per (units per second)
-   * @param kA The acceleration gain, in volts per (units per second squared)
+   * <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>
    */
-  @SuppressWarnings("ParameterName")
   public static LinearSystem<N1, N1, N1> identifyVelocitySystem(double kV, double kA) {
     if (kV <= 0.0) {
       throw new IllegalArgumentException("Kv must be greater than zero.");
@@ -241,20 +240,23 @@
   }
 
   /**
-   * Identify a position system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). These
-   * constants cam be found using SysId. The states of the system are [position, velocity]ᵀ, inputs
-   * are [voltage], and outputs are [position].
+   * Create a state-space model for a 1 DOF position system from its kV (volts/(unit/sec)) and kA
+   * (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are
+   * [position, velocity]ᵀ, inputs are [voltage], and outputs are [position].
    *
    * <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.
    *
-   * @param kV The velocity gain, in volts per (units per second)
-   * @param kA The acceleration gain, in volts per (units per second squared)
+   * <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²)
    * @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>
    */
-  @SuppressWarnings("ParameterName")
   public static LinearSystem<N2, N1, N1> identifyPositionSystem(double kV, double kA) {
     if (kV <= 0.0) {
       throw new IllegalArgumentException("Kv must be greater than zero.");
@@ -271,22 +273,23 @@
   }
 
   /**
-   * Identify a standard differential drive drivetrain, given the drivetrain's kV and kA in both
-   * linear (volts/(meter/sec) and volts/(meter/sec^2)) and angular (volts/(meter/sec) and
-   * volts/(meter/sec^2)) cases. This can be found using SysId. The states of the system are [left
-   * velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are [left
-   * velocity, right velocity]ᵀ.
+   * Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear
+   * {(volts/(meter/sec), (volts/(meter/sec²))} and angular {(volts/(radian/sec)),
+   * (volts/(radian/sec²))} cases. These constants can be found using SysId.
    *
-   * @param kVLinear The linear velocity gain, volts per (meter per second).
-   * @param kALinear The linear acceleration gain, volts per (meter per second squared).
-   * @param kVAngular The angular velocity gain, volts per (meter per second).
-   * @param kAAngular The angular acceleration gain, volts per (meter per second squared).
+   * <p>States: [[left velocity], [right velocity]]<br>
+   * Inputs: [[left voltage], [right voltage]]<br>
+   * Outputs: [[left velocity], [right velocity]]
+   *
+   * @param kVLinear The linear velocity gain in volts per (meters per second).
+   * @param kALinear The linear acceleration gain in volts per (meters per second squared).
+   * @param kVAngular The angular velocity gain in volts per (meters per second).
+   * @param kAAngular The angular acceleration gain in volts per (meters per second squared).
    * @return A LinearSystem representing the given characterized constants.
    * @throws IllegalArgumentException if kVLinear &lt;= 0, kALinear &lt;= 0, kVAngular &lt;= 0, or
    *     kAAngular &lt;= 0.
    * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
    */
-  @SuppressWarnings("ParameterName")
   public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
       double kVLinear, double kALinear, double kVAngular, double kAAngular) {
     if (kVLinear <= 0.0) {
@@ -315,23 +318,25 @@
   }
 
   /**
-   * Identify a standard differential drive drivetrain, given the drivetrain's kV and kA in both
-   * linear (volts/(meter/sec) and volts/(meter/sec^2)) and angular (volts/(radian/sec) and
-   * volts/(radian/sec^2)) cases. This can be found using SysId. The states of the system are [left
-   * velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are [left
-   * velocity, right velocity]ᵀ.
+   * Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear
+   * {(volts/(meter/sec)), (volts/(meter/sec²))} and angular {(volts/(radian/sec)),
+   * (volts/(radian/sec²))} cases. This can be found using SysId.
    *
-   * @param kVLinear The linear velocity gain, volts per (meter per second).
-   * @param kALinear The linear acceleration gain, volts per (meter per second squared).
-   * @param kVAngular The angular velocity gain, volts per (radians per second).
-   * @param kAAngular The angular acceleration gain, volts per (radians per second squared).
-   * @param trackwidth The width of the drivetrain in meters.
+   * <p>States: [[left velocity], [right velocity]]<br>
+   * Inputs: [[left voltage], [right voltage]]<br>
+   * Outputs: [[left velocity], [right velocity]]
+   *
+   * @param kVLinear The linear velocity gain in volts per (meters per second).
+   * @param kALinear The linear acceleration gain in volts per (meters per second squared).
+   * @param kVAngular The angular velocity gain in volts per (radians per second).
+   * @param kAAngular The angular acceleration gain in volts per (radians per second squared).
+   * @param trackwidth The distance between the differential drive's left and right wheels, in
+   *     meters.
    * @return A LinearSystem representing the given characterized constants.
    * @throws IllegalArgumentException if kVLinear &lt;= 0, kALinear &lt;= 0, kVAngular &lt;= 0,
    *     kAAngular &lt;= 0, or trackwidth &lt;= 0.
    * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
    */
-  @SuppressWarnings("ParameterName")
   public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
       double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
     if (kVLinear <= 0.0) {
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 2ec4244..f716dc0 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
@@ -44,7 +44,6 @@
    * @param t The fraction for interpolation.
    * @return The interpolated value.
    */
-  @SuppressWarnings("ParameterName")
   private static double lerp(double startValue, double endValue, double t) {
     return startValue + (endValue - startValue) * t;
   }
@@ -57,7 +56,6 @@
    * @param t The fraction for interpolation.
    * @return The interpolated pose.
    */
-  @SuppressWarnings("ParameterName")
   private static Pose2d lerp(Pose2d startValue, Pose2d endValue, double t) {
     return startValue.plus((endValue.minus(startValue)).times(t));
   }
@@ -254,7 +252,6 @@
    * Represents a time-parameterized trajectory. The trajectory contains of various States that
    * represent the pose, curvature, time elapsed, velocity, and acceleration at that point.
    */
-  @SuppressWarnings("MemberName")
   public static class State {
     // The time elapsed since the beginning of the trajectory.
     @JsonProperty("time")
@@ -309,7 +306,6 @@
      * @param i The interpolant (fraction).
      * @return The interpolated state.
      */
-    @SuppressWarnings("ParameterName")
     State interpolate(State endValue, double i) {
       // Find the new t value.
       final double newT = lerp(timeSeconds, endValue.timeSeconds, i);
@@ -332,7 +328,7 @@
       final double newV = velocityMetersPerSecond + (accelerationMetersPerSecondSq * deltaT);
 
       // Calculate the change in position.
-      // delta_s = v_0 t + 0.5 at^2
+      // delta_s = v_0 t + 0.5at²
       final double newS =
           (velocityMetersPerSecond * deltaT
                   + 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2))
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java
index 0827c15..ce90ce6 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java
@@ -15,14 +15,13 @@
 import edu.wpi.first.math.spline.SplineParameterizer;
 import edu.wpi.first.math.spline.SplineParameterizer.MalformedSplineException;
 import java.util.ArrayList;
-import java.util.Arrays;
 import java.util.Collection;
 import java.util.List;
 import java.util.function.BiConsumer;
 
 public final class TrajectoryGenerator {
   private static final Trajectory kDoNothingTrajectory =
-      new Trajectory(Arrays.asList(new Trajectory.State()));
+      new Trajectory(List.of(new Trajectory.State()));
   private static BiConsumer<String, StackTraceElement[]> errorFunc;
 
   /** Private constructor because this is a utility class. */
@@ -193,7 +192,6 @@
    * @param config The configuration for the trajectory.
    * @return The generated trajectory.
    */
-  @SuppressWarnings("LocalVariableName")
   public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config) {
     final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
 
@@ -263,7 +261,6 @@
   }
 
   // Work around type erasure signatures
-  @SuppressWarnings("serial")
   public static class ControlVectorList extends ArrayList<Spline.ControlVector> {
     public ControlVectorList(int initialCapacity) {
       super(initialCapacity);
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 d7fbf59..cf9b6f8 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
@@ -90,7 +90,7 @@
       // acceleration, since acceleration limits may be a function of velocity.
       while (true) {
         // Enforce global max velocity and max reachable velocity by global
-        // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
+        // acceleration limit. v_f = √(v_i² + 2ad).
         constrainedState.maxVelocityMetersPerSecond =
             Math.min(
                 maxVelocityMetersPerSecond,
@@ -164,7 +164,7 @@
 
       while (true) {
         // Enforce max velocity limit (reverse)
-        // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
+        // v_f = √(v_i² + 2ad), where v_i = successor.
         double newMaxVelocity =
             Math.sqrt(
                 successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond
@@ -294,7 +294,6 @@
     }
   }
 
-  @SuppressWarnings("MemberName")
   private static class ConstrainedState {
     PoseWithCurvature pose;
     double distanceMeters;
@@ -320,7 +319,6 @@
     }
   }
 
-  @SuppressWarnings("serial")
   public static class TrajectoryGenerationException extends RuntimeException {
     public TrajectoryGenerationException(String message) {
       super(message);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
index 5fb2c34..2cbba90 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
@@ -68,7 +68,7 @@
   }
 
   /**
-   * Imports a Trajectory from a PathWeaver-style JSON file.
+   * Imports a Trajectory from a JSON file exported from PathWeaver.
    *
    * @param path The path of the json file to import from
    * @return The trajectory represented by the file.
@@ -90,7 +90,7 @@
   }
 
   /**
-   * Deserializes a Trajectory from PathWeaver-style JSON.
+   * Deserializes a Trajectory from JSON exported from PathWeaver.
    *
    * @param json The string containing the serialized JSON
    * @return the trajectory represented by the JSON
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 d41a8eb..3b6559e 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
@@ -51,10 +51,8 @@
   private double m_endDeccel;
 
   public static class Constraints {
-    @SuppressWarnings("MemberName")
     public final double maxVelocity;
 
-    @SuppressWarnings("MemberName")
     public final double maxAcceleration;
 
     /**
@@ -71,10 +69,8 @@
   }
 
   public static class State {
-    @SuppressWarnings("MemberName")
     public double position;
 
-    @SuppressWarnings("MemberName")
     public double velocity;
 
     public State() {}
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java
index 13138e8..7a47fca 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java
@@ -38,12 +38,12 @@
   @Override
   public double getMaxVelocityMetersPerSecond(
       Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
-    // ac = v^2 / r
-    // k (curvature) = 1 / r
+    // ac = v²/r
+    // k (curvature) = 1/r
 
-    // therefore, ac = v^2 * k
-    // ac / k = v^2
-    // v = std::sqrt(ac / k)
+    // therefore, ac = v²k
+    // ac/k = v²
+    // v = √(ac/k)
 
     return Math.sqrt(
         m_maxCentripetalAccelerationMetersPerSecondSq / Math.abs(curvatureRadPerMeter));
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java
index 4c7e814..0cc6bc5 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java
@@ -111,7 +111,7 @@
                       / 2);
     }
 
-    // When turning about a point inside of the wheelbase (i.e. radius less than half
+    // When turning about a point inside the wheelbase (i.e. radius less than half
     // the trackwidth), the inner wheel's direction changes, but the magnitude remains
     // the same.  The formula above changes sign for the inner wheel when this happens.
     // We can accurately account for this by simply negating the inner wheel.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java
index c3bd226..e0647f2 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java
@@ -23,7 +23,6 @@
    * @param rotation The rotation to apply to all radii around the origin.
    * @param constraint The constraint to enforce when the robot is within the region.
    */
-  @SuppressWarnings("ParameterName")
   public EllipticalRegionConstraint(
       Translation2d center,
       double xWidth,
@@ -65,11 +64,16 @@
    * @return Whether the robot pose is within the constraint region.
    */
   public boolean isPoseInRegion(Pose2d robotPose) {
-    // The region (disk) bounded by the ellipse is given by the equation:
-    // ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
+    // The region bounded by the ellipse is given by the equation:
+    //
+    // (x−h)²/Rx² + (y−k)²/Ry² ≤ 1
+    //
+    // Multiply by Rx²Ry² for efficiency reasons:
+    //
+    // (x−h)²Ry² + (y−k)²Rx² ≤ Rx²Ry²
+    //
     // If the inequality is satisfied, then it is inside the ellipse; otherwise
     // it is outside the ellipse.
-    // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
     return Math.pow(robotPose.getX() - m_center.getX(), 2) * Math.pow(m_radii.getY(), 2)
             + Math.pow(robotPose.getY() - m_center.getY(), 2) * Math.pow(m_radii.getX(), 2)
         <= Math.pow(m_radii.getX(), 2) * Math.pow(m_radii.getY(), 2);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java
index bbf30f7..cc0087c 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java
@@ -36,7 +36,6 @@
       Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond);
 
   /** Represents a minimum and maximum acceleration. */
-  @SuppressWarnings("MemberName")
   class MinMax {
     public double minAccelerationMetersPerSecondSq = -Double.MAX_VALUE;
     public double maxAccelerationMetersPerSecondSq = +Double.MAX_VALUE;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/util/Units.java b/wpimath/src/main/java/edu/wpi/first/math/util/Units.java
index 017f4d0..ce2b38e 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/util/Units.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/util/Units.java
@@ -91,7 +91,7 @@
    * Converts given degrees to rotations.
    *
    * @param degrees The degrees to convert.
-   * @return rotations Converted from radians.
+   * @return rotations Converted from degrees.
    */
   public static double degreesToRotations(double degrees) {
     return degrees / 360;
diff --git a/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp b/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp
new file mode 100644
index 0000000..97968dc
--- /dev/null
+++ b/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp
@@ -0,0 +1,19 @@
+// 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/ComputerVisionUtil.h"
+
+#include "units/math.h"
+
+namespace frc {
+
+frc::Pose3d ObjectToRobotPose(const frc::Pose3d& objectInField,
+                              const frc::Transform3d& cameraToObject,
+                              const frc::Transform3d& robotToCamera) {
+  const auto objectToCamera = cameraToObject.Inverse();
+  const auto cameraToRobot = robotToCamera.Inverse();
+  return objectInField + objectToCamera + cameraToRobot;
+}
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/cpp/MathUtil.cpp b/wpimath/src/main/native/cpp/MathUtil.cpp
deleted file mode 100644
index 19cd214..0000000
--- a/wpimath/src/main/native/cpp/MathUtil.cpp
+++ /dev/null
@@ -1,23 +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 "frc/MathUtil.h"
-
-#include <cmath>
-
-namespace frc {
-
-double ApplyDeadband(double value, double deadband) {
-  if (std::abs(value) > deadband) {
-    if (value > 0.0) {
-      return (value - deadband) / (1.0 - deadband);
-    } else {
-      return (value + deadband) / (1.0 - deadband);
-    }
-  } else {
-    return 0.0;
-  }
-}
-
-}  // namespace frc
diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
index 8f1145d..fc532db 100644
--- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
+++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
@@ -6,33 +6,31 @@
 
 namespace frc {
 
-Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose) {
-  return Eigen::Vector<double, 3>{pose.Translation().X().value(),
-                                  pose.Translation().Y().value(),
-                                  pose.Rotation().Radians().value()};
+Vectord<3> PoseTo3dVector(const Pose2d& pose) {
+  return Vectord<3>{pose.Translation().X().value(),
+                    pose.Translation().Y().value(),
+                    pose.Rotation().Radians().value()};
 }
 
-Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose) {
-  return Eigen::Vector<double, 4>{pose.Translation().X().value(),
-                                  pose.Translation().Y().value(),
-                                  pose.Rotation().Cos(), pose.Rotation().Sin()};
+Vectord<4> PoseTo4dVector(const Pose2d& pose) {
+  return Vectord<4>{pose.Translation().X().value(),
+                    pose.Translation().Y().value(), pose.Rotation().Cos(),
+                    pose.Rotation().Sin()};
 }
 
 template <>
-bool IsStabilizable<1, 1>(const Eigen::Matrix<double, 1, 1>& A,
-                          const Eigen::Matrix<double, 1, 1>& B) {
+bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B) {
   return detail::IsStabilizableImpl<1, 1>(A, B);
 }
 
 template <>
-bool IsStabilizable<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
-                          const Eigen::Matrix<double, 2, 1>& B) {
+bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B) {
   return detail::IsStabilizableImpl<2, 1>(A, B);
 }
 
-Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose) {
-  return Eigen::Vector<double, 3>{pose.X().value(), pose.Y().value(),
-                                  pose.Rotation().Radians().value()};
+Vectord<3> PoseToVector(const Pose2d& pose) {
+  return Vectord<3>{pose.X().value(), pose.Y().value(),
+                    pose.Rotation().Radians().value()};
 }
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
new file mode 100644
index 0000000..9788023
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
@@ -0,0 +1,85 @@
+// 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/controller/DifferentialDriveAccelerationLimiter.h"
+
+#include <utility>
+
+#include "Eigen/QR"
+
+using namespace frc;
+
+DifferentialDriveAccelerationLimiter::DifferentialDriveAccelerationLimiter(
+    LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
+    units::meters_per_second_squared_t maxLinearAccel,
+    units::radians_per_second_squared_t maxAngularAccel)
+    : DifferentialDriveAccelerationLimiter(system, trackwidth, -maxLinearAccel,
+                                           maxLinearAccel, maxAngularAccel) {}
+
+DifferentialDriveAccelerationLimiter::DifferentialDriveAccelerationLimiter(
+    LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
+    units::meters_per_second_squared_t minLinearAccel,
+    units::meters_per_second_squared_t maxLinearAccel,
+    units::radians_per_second_squared_t maxAngularAccel)
+    : m_system{std::move(system)},
+      m_trackwidth{trackwidth},
+      m_minLinearAccel{minLinearAccel},
+      m_maxLinearAccel{maxLinearAccel},
+      m_maxAngularAccel{maxAngularAccel} {
+  if (minLinearAccel > maxLinearAccel) {
+    throw std::invalid_argument(
+        "maxLinearAccel must be greater than minLinearAccel");
+  }
+}
+
+DifferentialDriveWheelVoltages DifferentialDriveAccelerationLimiter::Calculate(
+    units::meters_per_second_t leftVelocity,
+    units::meters_per_second_t rightVelocity, units::volt_t leftVoltage,
+    units::volt_t rightVoltage) {
+  Vectord<2> u{leftVoltage.value(), rightVoltage.value()};
+
+  // Find unconstrained wheel accelerations
+  Vectord<2> x{leftVelocity.value(), rightVelocity.value()};
+  Vectord<2> dxdt = m_system.A() * x + m_system.B() * u;
+
+  // Convert from wheel accelerations to linear and angular accelerations
+  //
+  // a = (dxdt(0) + dx/dt(1)) / 2
+  //   = 0.5 dxdt(0) + 0.5 dxdt(1)
+  //
+  // α = (dxdt(1) - dxdt(0)) / trackwidth
+  //   = -1/trackwidth dxdt(0) + 1/trackwidth dxdt(1)
+  //
+  // [a] = [          0.5           0.5][dxdt(0)]
+  // [α]   [-1/trackwidth  1/trackwidth][dxdt(1)]
+  //
+  // accels = M dxdt where M = [0.5, 0.5; -1/trackwidth, 1/trackwidth]
+  Matrixd<2, 2> M{{0.5, 0.5},
+                  {-1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()}};
+  Vectord<2> accels = M * dxdt;
+
+  // Constrain the linear and angular accelerations
+  if (accels(0) > m_maxLinearAccel.value()) {
+    accels(0) = m_maxLinearAccel.value();
+  } else if (accels(0) < m_minLinearAccel.value()) {
+    accels(0) = m_minLinearAccel.value();
+  }
+  if (accels(1) > m_maxAngularAccel.value()) {
+    accels(1) = m_maxAngularAccel.value();
+  } else if (accels(1) < -m_maxAngularAccel.value()) {
+    accels(1) = -m_maxAngularAccel.value();
+  }
+
+  // Convert the constrained linear and angular accelerations back to wheel
+  // accelerations
+  dxdt = M.householderQr().solve(accels);
+
+  // Find voltages for the given wheel accelerations
+  //
+  // dx/dt = Ax + Bu
+  // u = B⁻¹(dx/dt - Ax)
+  u = m_system.B().householderQr().solve(dxdt - m_system.A() * x);
+
+  return {units::volt_t{u(0)}, units::volt_t{u(1)}};
+}
diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
new file mode 100644
index 0000000..e1b2732
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/DifferentialDriveFeedforward.h"
+
+#include "frc/EigenCore.h"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+
+DifferentialDriveFeedforward::DifferentialDriveFeedforward(
+    decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
+    decltype(1_V / 1_rad_per_s) kVAngular,
+    decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth)
+    : m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
+          kVLinear, kALinear, kVAngular, kAAngular, trackwidth)} {}
+
+DifferentialDriveFeedforward::DifferentialDriveFeedforward(
+    decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
+    decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular)
+    : m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
+          kVLinear, kALinear, kVAngular, kAAngular)} {}
+
+DifferentialDriveWheelVoltages DifferentialDriveFeedforward::Calculate(
+    units::meters_per_second_t currentLeftVelocity,
+    units::meters_per_second_t nextLeftVelocity,
+    units::meters_per_second_t currentRightVelocity,
+    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};
+  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 23b22cd..def7234 100644
--- a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
+++ b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
@@ -15,7 +15,9 @@
     ProfiledPIDController<units::radian> thetaController)
     : m_xController(std::move(xController)),
       m_yController(std::move(yController)),
-      m_thetaController(std::move(thetaController)) {}
+      m_thetaController(std::move(thetaController)) {
+  m_thetaController.EnableContinuousInput(0_deg, 360.0_deg);
+}
 
 bool HolonomicDriveController::AtReference() const {
   const auto& eTranslate = m_poseError.Translation();
@@ -32,8 +34,9 @@
 }
 
 ChassisSpeeds HolonomicDriveController::Calculate(
-    const Pose2d& currentPose, const Pose2d& poseRef,
-    units::meters_per_second_t linearVelocityRef, const Rotation2d& angleRef) {
+    const Pose2d& currentPose, const Pose2d& trajectoryPose,
+    units::meters_per_second_t desiredLinearVelocity,
+    const Rotation2d& desiredHeading) {
   // If this is the first run, then we need to reset the theta controller to the
   // current pose's heading.
   if (m_firstRun) {
@@ -42,13 +45,13 @@
   }
 
   // Calculate feedforward velocities (field-relative)
-  auto xFF = linearVelocityRef * poseRef.Rotation().Cos();
-  auto yFF = linearVelocityRef * poseRef.Rotation().Sin();
-  auto thetaFF = units::radians_per_second_t(m_thetaController.Calculate(
-      currentPose.Rotation().Radians(), angleRef.Radians()));
+  auto xFF = desiredLinearVelocity * trajectoryPose.Rotation().Cos();
+  auto yFF = desiredLinearVelocity * trajectoryPose.Rotation().Sin();
+  auto thetaFF = units::radians_per_second_t{m_thetaController.Calculate(
+      currentPose.Rotation().Radians(), desiredHeading.Radians())};
 
-  m_poseError = poseRef.RelativeTo(currentPose);
-  m_rotationError = angleRef - currentPose.Rotation();
+  m_poseError = trajectoryPose.RelativeTo(currentPose);
+  m_rotationError = desiredHeading - currentPose.Rotation();
 
   if (!m_enabled) {
     return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF,
@@ -56,10 +59,10 @@
   }
 
   // Calculate feedback velocities (based on position error).
-  auto xFeedback = units::meters_per_second_t(
-      m_xController.Calculate(currentPose.X().value(), poseRef.X().value()));
-  auto yFeedback = units::meters_per_second_t(
-      m_yController.Calculate(currentPose.Y().value(), poseRef.Y().value()));
+  auto xFeedback = units::meters_per_second_t{m_xController.Calculate(
+      currentPose.X().value(), trajectoryPose.X().value())};
+  auto yFeedback = units::meters_per_second_t{m_yController.Calculate(
+      currentPose.Y().value(), trajectoryPose.Y().value())};
 
   // Return next output.
   return ChassisSpeeds::FromFieldRelativeSpeeds(
@@ -68,9 +71,9 @@
 
 ChassisSpeeds HolonomicDriveController::Calculate(
     const Pose2d& currentPose, const Trajectory::State& desiredState,
-    const Rotation2d& angleRef) {
+    const Rotation2d& desiredHeading) {
   return Calculate(currentPose, desiredState.pose, desiredState.velocity,
-                   angleRef);
+                   desiredHeading);
 }
 
 void HolonomicDriveController::SetEnabled(bool enabled) {
diff --git a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
new file mode 100644
index 0000000..17a0c56
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
@@ -0,0 +1,153 @@
+// 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/controller/LTVDifferentialDriveController.h"
+
+#include <cmath>
+
+#include "frc/MathUtil.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/controller/LinearQuadraticRegulator.h"
+
+using namespace frc;
+
+namespace {
+
+/**
+ * States of the drivetrain system.
+ */
+class State {
+ public:
+  /// X position in global coordinate frame.
+  [[maybe_unused]] static constexpr int kX = 0;
+
+  /// Y position in global coordinate frame.
+  static constexpr int kY = 1;
+
+  /// Heading in global coordinate frame.
+  static constexpr int kHeading = 2;
+
+  /// Left encoder velocity.
+  [[maybe_unused]] static constexpr int kLeftVelocity = 3;
+
+  /// Right encoder velocity.
+  [[maybe_unused]] static constexpr int kRightVelocity = 4;
+};
+
+}  // namespace
+
+LTVDifferentialDriveController::LTVDifferentialDriveController(
+    const frc::LinearSystem<2, 2, 2>& plant, units::meter_t trackwidth,
+    const wpi::array<double, 5>& Qelems, const wpi::array<double, 2>& Relems,
+    units::second_t dt)
+    : m_trackwidth{trackwidth} {
+  // Control law derivation is in section 8.7 of
+  // https://file.tavsys.net/control/controls-engineering-in-frc.pdf
+  Matrixd<5, 5> A{
+      {0.0, 0.0, 0.0, 0.5, 0.5},
+      {0.0, 0.0, 0.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, -1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()},
+      {0.0, 0.0, 0.0, plant.A(0, 0), plant.A(0, 1)},
+      {0.0, 0.0, 0.0, plant.A(1, 0), plant.A(1, 1)}};
+  Matrixd<5, 2> B{{0.0, 0.0},
+                  {0.0, 0.0},
+                  {0.0, 0.0},
+                  {plant.B(0, 0), plant.B(0, 1)},
+                  {plant.B(1, 0), plant.B(1, 1)}};
+  Matrixd<5, 5> Q = frc::MakeCostMatrix(Qelems);
+  Matrixd<2, 2> R = frc::MakeCostMatrix(Relems);
+
+  // dx/dt = Ax + Bu
+  // 0 = Ax + Bu
+  // Ax = -Bu
+  // x = -A⁻¹Bu
+  units::meters_per_second_t maxV{
+      -plant.A().householderQr().solve(plant.B() * Vectord<2>{12.0, 12.0})(0)};
+
+  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());
+    } else {
+      A(State::kY, State::kHeading) = velocity.value();
+      m_table.insert(velocity,
+                     frc::LinearQuadraticRegulator<5, 2>{A, B, Q, R, dt}.K());
+    }
+  }
+}
+
+bool LTVDifferentialDriveController::AtReference() const {
+  return std::abs(m_error(0)) < m_tolerance(0) &&
+         std::abs(m_error(1)) < m_tolerance(1) &&
+         std::abs(m_error(2)) < m_tolerance(2) &&
+         std::abs(m_error(3)) < m_tolerance(3) &&
+         std::abs(m_error(4)) < m_tolerance(4);
+}
+
+void LTVDifferentialDriveController::SetTolerance(
+    const Pose2d& poseTolerance,
+    units::meters_per_second_t leftVelocityTolerance,
+    units::meters_per_second_t rightVelocityTolerance) {
+  m_tolerance =
+      Vectord<5>{poseTolerance.X().value(), poseTolerance.Y().value(),
+                 poseTolerance.Rotation().Radians().value(),
+                 leftVelocityTolerance.value(), rightVelocityTolerance.value()};
+}
+
+DifferentialDriveWheelVoltages LTVDifferentialDriveController::Calculate(
+    const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
+    units::meters_per_second_t rightVelocity, const Pose2d& poseRef,
+    units::meters_per_second_t leftVelocityRef,
+    units::meters_per_second_t rightVelocityRef) {
+  // This implements the linear time-varying differential drive controller in
+  // theorem 9.6.3 of https://tavsys.net/controls-in-frc.
+  Vectord<5> x{currentPose.X().value(), currentPose.Y().value(),
+               currentPose.Rotation().Radians().value(), leftVelocity.value(),
+               rightVelocity.value()};
+
+  Matrixd<5, 5> inRobotFrame = Matrixd<5, 5>::Identity();
+  inRobotFrame(0, 0) = std::cos(x(State::kHeading));
+  inRobotFrame(0, 1) = std::sin(x(State::kHeading));
+  inRobotFrame(1, 0) = -std::sin(x(State::kHeading));
+  inRobotFrame(1, 1) = std::cos(x(State::kHeading));
+
+  Vectord<5> r{poseRef.X().value(), poseRef.Y().value(),
+               poseRef.Rotation().Radians().value(), leftVelocityRef.value(),
+               rightVelocityRef.value()};
+  m_error = r - x;
+  m_error(State::kHeading) =
+      frc::AngleModulus(units::radian_t{m_error(State::kHeading)}).value();
+
+  units::meters_per_second_t velocity{(leftVelocity + rightVelocity) / 2.0};
+  const auto& K = m_table[velocity];
+
+  Vectord<2> u = K * inRobotFrame * m_error;
+
+  return DifferentialDriveWheelVoltages{units::volt_t{u(0)},
+                                        units::volt_t{u(1)}};
+}
+
+DifferentialDriveWheelVoltages LTVDifferentialDriveController::Calculate(
+    const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
+    units::meters_per_second_t rightVelocity,
+    const Trajectory::State& desiredState) {
+  // v = (v_r + v_l) / 2     (1)
+  // w = (v_r - v_l) / (2r)  (2)
+  // k = w / v               (3)
+  //
+  // v_l = v - wr
+  // v_l = v - (vk)r
+  // v_l = v(1 - kr)
+  //
+  // v_r = v + wr
+  // v_r = v + (vk)r
+  // v_r = v(1 + kr)
+  return Calculate(
+      currentPose, leftVelocity, rightVelocity, desiredState.pose,
+      desiredState.velocity *
+          (1 - (desiredState.curvature / 1_rad * m_trackwidth / 2.0)),
+      desiredState.velocity *
+          (1 + (desiredState.curvature / 1_rad * m_trackwidth / 2.0)));
+}
diff --git a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
new file mode 100644
index 0000000..256b757
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
@@ -0,0 +1,133 @@
+// 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/controller/LTVUnicycleController.h"
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/controller/LinearQuadraticRegulator.h"
+#include "units/math.h"
+
+using namespace frc;
+
+namespace {
+
+/**
+ * States of the drivetrain system.
+ */
+class State {
+ public:
+  /// X position in global coordinate frame.
+  [[maybe_unused]] static constexpr int kX = 0;
+
+  /// Y position in global coordinate frame.
+  static constexpr int kY = 1;
+
+  /// Heading in global coordinate frame.
+  static constexpr int kHeading = 2;
+};
+
+}  // namespace
+
+LTVUnicycleController::LTVUnicycleController(
+    units::second_t dt, units::meters_per_second_t maxVelocity)
+    : LTVUnicycleController{{0.0625, 0.125, 2.0}, {1.0, 2.0}, dt, maxVelocity} {
+}
+
+LTVUnicycleController::LTVUnicycleController(
+    const wpi::array<double, 3>& Qelems, const wpi::array<double, 2>& Relems,
+    units::second_t dt, units::meters_per_second_t maxVelocity) {
+  // The change in global pose for a unicycle is defined by the following three
+  // equations.
+  //
+  // ẋ = v cosθ
+  // ẏ = v sinθ
+  // θ̇ = ω
+  //
+  // Here's the model as a vector function where x = [x  y  θ]ᵀ and u = [v  ω]ᵀ.
+  //
+  //           [v cosθ]
+  // f(x, u) = [v sinθ]
+  //           [  ω   ]
+  //
+  // To create an LQR, we need to linearize this.
+  //
+  //               [0  0  −v sinθ]                  [cosθ  0]
+  // ∂f(x, u)/∂x = [0  0   v cosθ]    ∂f(x, u)/∂u = [sinθ  0]
+  //               [0  0     0   ]                  [ 0    1]
+  //
+  // We're going to make a cross-track error controller, so we'll apply a
+  // clockwise rotation matrix to the global tracking error to transform it into
+  // the robot's coordinate frame. Since the cross-track error is always
+  // measured from the robot's coordinate frame, the model used to compute the
+  // LQR should be linearized around θ = 0 at all times.
+  //
+  //     [0  0  −v sin0]        [cos0  0]
+  // A = [0  0   v cos0]    B = [sin0  0]
+  //     [0  0     0   ]        [ 0    1]
+  //
+  //     [0  0  0]              [1  0]
+  // A = [0  0  v]          B = [0  0]
+  //     [0  0  0]              [0  1]
+  Matrixd<3, 3> A = Matrixd<3, 3>::Zero();
+  Matrixd<3, 2> B{{1.0, 0.0}, {0.0, 0.0}, {0.0, 1.0}};
+  Matrixd<3, 3> Q = frc::MakeCostMatrix(Qelems);
+  Matrixd<2, 2> R = frc::MakeCostMatrix(Relems);
+
+  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());
+    } else {
+      A(State::kY, State::kHeading) = velocity.value();
+      m_table.insert(velocity,
+                     frc::LinearQuadraticRegulator<3, 2>{A, B, Q, R, dt}.K());
+    }
+  }
+}
+
+bool LTVUnicycleController::AtReference() const {
+  const auto& eTranslate = m_poseError.Translation();
+  const auto& eRotate = m_poseError.Rotation();
+  const auto& tolTranslate = m_poseTolerance.Translation();
+  const auto& tolRotate = m_poseTolerance.Rotation();
+  return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
+         units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
+         units::math::abs(eRotate.Radians()) < tolRotate.Radians();
+}
+
+void LTVUnicycleController::SetTolerance(const Pose2d& poseTolerance) {
+  m_poseTolerance = poseTolerance;
+}
+
+ChassisSpeeds LTVUnicycleController::Calculate(
+    const Pose2d& currentPose, const Pose2d& poseRef,
+    units::meters_per_second_t linearVelocityRef,
+    units::radians_per_second_t angularVelocityRef) {
+  if (!m_enabled) {
+    return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef};
+  }
+
+  m_poseError = poseRef.RelativeTo(currentPose);
+
+  const auto& K = m_table[linearVelocityRef];
+  Vectord<3> e{m_poseError.X().value(), m_poseError.Y().value(),
+               m_poseError.Rotation().Radians().value()};
+  Vectord<2> u = K * e;
+
+  return ChassisSpeeds{linearVelocityRef + units::meters_per_second_t{u(0)},
+                       0_mps,
+                       angularVelocityRef + units::radians_per_second_t{u(1)}};
+}
+
+ChassisSpeeds LTVUnicycleController::Calculate(
+    const Pose2d& currentPose, const Trajectory::State& desiredState) {
+  return Calculate(currentPose, desiredState.pose, desiredState.velocity,
+                   desiredState.velocity * desiredState.curvature);
+}
+
+void LTVUnicycleController::SetEnabled(bool enabled) {
+  m_enabled = enabled;
+}
diff --git a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
index 4d2fbe9..2ab27d1 100644
--- a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
+++ b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
@@ -6,61 +6,11 @@
 
 namespace frc {
 
-LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
-    const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
-    const wpi::array<double, 1>& Qelems, const wpi::array<double, 1>& Relems,
-    units::second_t dt)
-    : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
-                               MakeCostMatrix(Relems), dt) {}
-
-LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
-    const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
-    const Eigen::Matrix<double, 1, 1>& Q, const Eigen::Matrix<double, 1, 1>& R,
-    units::second_t dt)
-    : detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, dt) {}
-
-LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
-    const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
-    const Eigen::Matrix<double, 1, 1>& Q, const Eigen::Matrix<double, 1, 1>& R,
-    const Eigen::Matrix<double, 1, 1>& N, units::second_t dt)
-    : detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, N, dt) {}
-
-LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
-    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
-    const wpi::array<double, 2>& Qelems, const wpi::array<double, 1>& Relems,
-    units::second_t dt)
-    : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
-                               MakeCostMatrix(Relems), dt) {}
-
-LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
-    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,
-    units::second_t dt)
-    : detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, dt) {}
-
-LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
-    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, units::second_t dt)
-    : detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, N, dt) {}
-
-LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
-    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
-    const wpi::array<double, 2>& Qelems, const wpi::array<double, 2>& Relems,
-    units::second_t dt)
-    : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
-                               MakeCostMatrix(Relems), dt) {}
-
-LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
-    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,
-    units::second_t dt)
-    : detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, dt) {}
-
-LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
-    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, units::second_t dt)
-    : detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, N, dt) {}
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+    LinearQuadraticRegulator<1, 1>;
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+    LinearQuadraticRegulator<2, 1>;
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+    LinearQuadraticRegulator<2, 2>;
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp
index 34af2aa..6c89d25 100644
--- a/wpimath/src/main/native/cpp/controller/PIDController.cpp
+++ b/wpimath/src/main/native/cpp/controller/PIDController.cpp
@@ -24,7 +24,7 @@
         period.value());
     m_period = 20_ms;
     wpi::math::MathSharedStore::ReportWarning(
-        "{}", "Controller period defaulted to 20ms.");
+        "Controller period defaulted to 20ms.");
   }
   static int instances = 0;
   instances++;
@@ -65,11 +65,29 @@
 }
 
 units::second_t PIDController::GetPeriod() const {
-  return units::second_t(m_period);
+  return m_period;
+}
+
+double PIDController::GetPositionTolerance() const {
+  return m_positionTolerance;
+}
+
+double PIDController::GetVelocityTolerance() const {
+  return m_velocityTolerance;
 }
 
 void PIDController::SetSetpoint(double setpoint) {
   m_setpoint = setpoint;
+
+  if (m_continuous) {
+    double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+    m_positionError =
+        frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+  } else {
+    m_positionError = m_setpoint - m_measurement;
+  }
+
+  m_velocityError = (m_positionError - m_prevError) / m_period.value();
 }
 
 double PIDController::GetSetpoint() const {
@@ -77,19 +95,8 @@
 }
 
 bool PIDController::AtSetpoint() const {
-  double positionError;
-  if (m_continuous) {
-    double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
-    positionError =
-        frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
-  } else {
-    positionError = m_setpoint - m_measurement;
-  }
-
-  double velocityError = (positionError - m_prevError) / m_period.value();
-
-  return std::abs(positionError) < m_positionTolerance &&
-         std::abs(velocityError) < m_velocityTolerance;
+  return std::abs(m_positionError) < m_positionTolerance &&
+         std::abs(m_velocityError) < m_velocityTolerance;
 }
 
 void PIDController::EnableContinuousInput(double minimumInput,
@@ -136,7 +143,7 @@
     m_positionError =
         frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
   } else {
-    m_positionError = m_setpoint - measurement;
+    m_positionError = m_setpoint - m_measurement;
   }
 
   m_velocityError = (m_positionError - m_prevError) / m_period.value();
@@ -151,14 +158,15 @@
 }
 
 double PIDController::Calculate(double measurement, double setpoint) {
-  // Set setpoint to provided value
-  SetSetpoint(setpoint);
+  m_setpoint = setpoint;
   return Calculate(measurement);
 }
 
 void PIDController::Reset() {
+  m_positionError = 0;
   m_prevError = 0;
   m_totalError = 0;
+  m_velocityError = 0;
 }
 
 void PIDController::InitSendable(wpi::SendableBuilder& builder) {
diff --git a/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp b/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp
index fa58427..be678cb 100644
--- a/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp
+++ b/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp
@@ -4,9 +4,7 @@
 
 #include "frc/controller/ProfiledPIDController.h"
 
-void frc::detail::ReportProfiledPIDController() {
+int frc::detail::IncrementAndGetProfiledPIDControllerInstances() {
   static int instances = 0;
-  ++instances;
-  wpi::math::MathSharedStore::ReportUsage(
-      wpi::math::MathUsageId::kController_ProfiledPIDController, instances);
+  return ++instances;
 }
diff --git a/wpimath/src/main/native/cpp/controller/RamseteController.cpp b/wpimath/src/main/native/cpp/controller/RamseteController.cpp
index 0fea864..576b0a9 100644
--- a/wpimath/src/main/native/cpp/controller/RamseteController.cpp
+++ b/wpimath/src/main/native/cpp/controller/RamseteController.cpp
@@ -24,17 +24,13 @@
   }
 }
 
-RamseteController::RamseteController(double b, double zeta)
-    : RamseteController(units::unit_t<b_unit>{b},
-                        units::unit_t<zeta_unit>{zeta}) {}
-
 RamseteController::RamseteController(units::unit_t<b_unit> b,
                                      units::unit_t<zeta_unit> zeta)
     : m_b{b}, m_zeta{zeta} {}
 
 RamseteController::RamseteController()
-    : RamseteController(units::unit_t<b_unit>{2.0},
-                        units::unit_t<zeta_unit>{0.7}) {}
+    : RamseteController{units::unit_t<b_unit>{2.0},
+                        units::unit_t<zeta_unit>{0.7}} {}
 
 bool RamseteController::AtReference() const {
   const auto& eTranslate = m_poseError.Translation();
@@ -67,10 +63,13 @@
   const auto& vRef = linearVelocityRef;
   const auto& omegaRef = angularVelocityRef;
 
+  // k = 2ζ√(ω_ref² + b v_ref²)
   auto k = 2.0 * m_zeta *
            units::math::sqrt(units::math::pow<2>(omegaRef) +
                              m_b * units::math::pow<2>(vRef));
 
+  // v_cmd = v_ref cos(e_θ) + k e_x
+  // ω_cmd = ω_ref + k e_θ + b v_ref sinc(e_θ) e_y
   return ChassisSpeeds{vRef * m_poseError.Rotation().Cos() + k * eX, 0_mps,
                        omegaRef + k * eTheta + m_b * vRef * Sinc(eTheta) * eY};
 }
diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
index c5ed7a1..39e0d8c 100644
--- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
@@ -11,135 +11,154 @@
 
 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(
-    const Rotation2d& gyroAngle, const Pose2d& initialPose,
-    const wpi::array<double, 5>& stateStdDevs,
-    const wpi::array<double, 3>& localMeasurementStdDevs,
-    const wpi::array<double, 3>& visionMeasurmentStdDevs,
-    units::second_t nominalDt)
-    : m_observer(
-          &DifferentialDrivePoseEstimator::F,
-          [](const Eigen::Vector<double, 5>& x,
-             const Eigen::Vector<double, 3>& u) {
-            return Eigen::Vector<double, 3>{x(3, 0), x(4, 0), x(2, 0)};
-          },
-          stateStdDevs, localMeasurementStdDevs, frc::AngleMean<5, 5>(2),
-          frc::AngleMean<3, 5>(2), frc::AngleResidual<5>(2),
-          frc::AngleResidual<3>(2), frc::AngleAdd<5>(2), nominalDt),
-      m_nominalDt(nominalDt) {
-  SetVisionMeasurementStdDevs(visionMeasurmentStdDevs);
+    DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+    units::meter_t leftDistance, units::meter_t rightDistance,
+    const Pose2d& initialPose)
+    : DifferentialDrivePoseEstimator{
+          kinematics,  gyroAngle,          leftDistance,   rightDistance,
+          initialPose, {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}} {}
 
-  // Create correction mechanism for vision measurements.
-  m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
-                        const Eigen::Vector<double, 3>& y) {
-    m_observer.Correct<3>(
-        u, y,
-        [](const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>&) {
-          return x.block<3, 1>(0, 0);
-        },
-        m_visionContR, frc::AngleMean<3, 5>(2), frc::AngleResidual<3>(2),
-        frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
-  };
+DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
+    DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+    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];
+  }
 
-  m_gyroOffset = initialPose.Rotation() - gyroAngle;
-  m_previousAngle = initialPose.Rotation();
-  m_observer.SetXhat(FillStateVector(initialPose, 0_m, 0_m));
+  SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
 }
 
 void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
-    const wpi::array<double, 3>& visionMeasurmentStdDevs) {
-  // Create R (covariances) for vision measurements.
-  m_visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs);
+    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 Pose2d& pose, const Rotation2d& gyroAngle) {
+void DifferentialDrivePoseEstimator::ResetPosition(const Rotation2d& gyroAngle,
+                                                   units::meter_t leftDistance,
+                                                   units::meter_t rightDistance,
+                                                   const Pose2d& pose) {
   // Reset state estimate and error covariance
-  m_observer.Reset();
-  m_latencyCompensator.Reset();
-
-  m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m));
-
-  m_gyroOffset = GetEstimatedPosition().Rotation() - gyroAngle;
-  m_previousAngle = pose.Rotation();
+  m_odometry.ResetPosition(gyroAngle, leftDistance, rightDistance, pose);
+  m_poseBuffer.Clear();
 }
 
 Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
-  return Pose2d(units::meter_t(m_observer.Xhat(0)),
-                units::meter_t(m_observer.Xhat(1)),
-                Rotation2d(units::radian_t(m_observer.Xhat(2))));
+  return m_odometry.GetPose();
 }
 
 void DifferentialDrivePoseEstimator::AddVisionMeasurement(
     const Pose2d& visionRobotPose, units::second_t timestamp) {
-  m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
-      &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
-      m_visionCorrect, 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,
-    const DifferentialDriveWheelSpeeds& wheelSpeeds,
-    units::meter_t leftDistance, units::meter_t rightDistance) {
+Pose2d DifferentialDrivePoseEstimator::Update(const Rotation2d& gyroAngle,
+                                              units::meter_t leftDistance,
+                                              units::meter_t rightDistance) {
   return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
-                        wheelSpeeds, leftDistance, rightDistance);
+                        leftDistance, rightDistance);
 }
 
 Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
     units::second_t currentTime, const Rotation2d& gyroAngle,
-    const DifferentialDriveWheelSpeeds& wheelSpeeds,
     units::meter_t leftDistance, units::meter_t rightDistance) {
-  auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
-  m_prevTime = currentTime;
+  m_odometry.Update(gyroAngle, leftDistance, rightDistance);
 
-  auto angle = gyroAngle + m_gyroOffset;
-  auto omega = (gyroAngle - m_previousAngle).Radians() / dt;
+  // fmt::print("odo, {}, {}, {}, {}, {}, {}\n",
+  //   gyroAngle.Radians(),
+  //   leftDistance,
+  //   rightDistance,
+  //   GetEstimatedPosition().X(),
+  //   GetEstimatedPosition().Y(),
+  //   GetEstimatedPosition().Rotation().Radians()
+  // );
 
-  auto u = Eigen::Vector<double, 3>{
-      (wheelSpeeds.left + wheelSpeeds.right).value() / 2.0, 0.0, omega.value()};
-
-  m_previousAngle = angle;
-
-  auto localY = Eigen::Vector<double, 3>{
-      leftDistance.value(), rightDistance.value(), angle.Radians().value()};
-
-  m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
-  m_observer.Predict(u, dt);
-  m_observer.Correct(u, localY);
+  m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle,
+                                       leftDistance, rightDistance});
 
   return GetEstimatedPosition();
 }
-
-Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::F(
-    const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>& u) {
-  // Apply a rotation matrix. Note that we do not add x because Runge-Kutta does
-  // that for us.
-  auto& theta = x(2);
-  Eigen::Matrix<double, 5, 5> toFieldRotation{
-      {std::cos(theta), -std::sin(theta), 0.0, 0.0, 0.0},
-      {std::sin(theta), std::cos(theta), 0.0, 0.0, 0.0},
-      {0.0, 0.0, 1.0, 0.0, 0.0},
-      {0.0, 0.0, 0.0, 1.0, 0.0},
-      {0.0, 0.0, 0.0, 0.0, 1.0}};
-  return toFieldRotation *
-         Eigen::Vector<double, 5>{u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)};
-}
-
-template <int Dim>
-wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
-    const Eigen::Vector<double, Dim>& stdDevs) {
-  wpi::array<double, Dim> array;
-  for (size_t i = 0; i < Dim; ++i) {
-    array[i] = stdDevs(i);
-  }
-  return array;
-}
-
-Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::FillStateVector(
-    const Pose2d& pose, units::meter_t leftDistance,
-    units::meter_t rightDistance) {
-  return Eigen::Vector<double, 5>{pose.Translation().X().value(),
-                                  pose.Translation().Y().value(),
-                                  pose.Rotation().Radians().value(),
-                                  leftDistance.value(), rightDistance.value()};
-}
diff --git a/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp b/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp
index 1209eae..a56efe4 100644
--- a/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp
+++ b/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp
@@ -6,16 +6,7 @@
 
 namespace frc {
 
-KalmanFilter<1, 1, 1>::KalmanFilter(
-    LinearSystem<1, 1, 1>& plant, const wpi::array<double, 1>& stateStdDevs,
-    const wpi::array<double, 1>& measurementStdDevs, units::second_t dt)
-    : detail::KalmanFilterImpl<1, 1, 1>{plant, stateStdDevs, measurementStdDevs,
-                                        dt} {}
-
-KalmanFilter<2, 1, 1>::KalmanFilter(
-    LinearSystem<2, 1, 1>& plant, const wpi::array<double, 2>& stateStdDevs,
-    const wpi::array<double, 1>& measurementStdDevs, units::second_t dt)
-    : detail::KalmanFilterImpl<2, 1, 1>{plant, stateStdDevs, measurementStdDevs,
-                                        dt} {}
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT) KalmanFilter<1, 1, 1>;
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT) KalmanFilter<2, 1, 1>;
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
index 9d93647..9642e08 100644
--- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
@@ -11,106 +11,159 @@
 
 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(
-    const Rotation2d& gyroAngle, const Pose2d& initialPose,
-    MecanumDriveKinematics kinematics,
+    MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+    const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
+    : MecanumDrivePoseEstimator{kinematics,      gyroAngle,
+                                wheelPositions,  initialPose,
+                                {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}} {}
+
+frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
+    MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+    const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose,
     const wpi::array<double, 3>& stateStdDevs,
-    const wpi::array<double, 1>& localMeasurementStdDevs,
-    const wpi::array<double, 3>& visionMeasurementStdDevs,
-    units::second_t nominalDt)
-    : m_observer(
-          [](const Eigen::Vector<double, 3>& x,
-             const Eigen::Vector<double, 3>& u) { return u; },
-          [](const Eigen::Vector<double, 3>& x,
-             const Eigen::Vector<double, 3>& u) { return x.block<1, 1>(2, 0); },
-          stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
-          frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
-          frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
-      m_kinematics(kinematics),
-      m_nominalDt(nominalDt) {
+    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);
-
-  // Create vision correction mechanism.
-  m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
-                        const Eigen::Vector<double, 3>& y) {
-    m_observer.Correct<3>(
-        u, y,
-        [](const Eigen::Vector<double, 3>& x, const Eigen::Vector<double, 3>&) {
-          return x;
-        },
-        m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
-        frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
-  };
-
-  // Set initial state.
-  m_observer.SetXhat(PoseTo3dVector(initialPose));
-
-  // Calculate offsets.
-  m_gyroOffset = initialPose.Rotation() - gyroAngle;
-  m_previousAngle = initialPose.Rotation();
 }
 
 void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
-    const wpi::array<double, 3>& visionMeasurmentStdDevs) {
-  // Create R (covariances) for vision measurements.
-  m_visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs);
+    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 Pose2d& pose, const Rotation2d& gyroAngle) {
+    const Rotation2d& gyroAngle,
+    const MecanumDriveWheelPositions& wheelPositions, const Pose2d& pose) {
   // Reset state estimate and error covariance
-  m_observer.Reset();
-  m_latencyCompensator.Reset();
-
-  m_observer.SetXhat(PoseTo3dVector(pose));
-
-  m_gyroOffset = pose.Rotation() - gyroAngle;
-  m_previousAngle = pose.Rotation();
+  m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
+  m_poseBuffer.Clear();
 }
 
 Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
-  return Pose2d(m_observer.Xhat(0) * 1_m, m_observer.Xhat(1) * 1_m,
-                Rotation2d(units::radian_t{m_observer.Xhat(2)}));
+  return m_odometry.GetPose();
 }
 
 void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
     const Pose2d& visionRobotPose, units::second_t timestamp) {
-  m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
-      &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
-      m_visionCorrect, 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 MecanumDriveWheelSpeeds& wheelSpeeds) {
+    const Rotation2d& gyroAngle,
+    const MecanumDriveWheelPositions& wheelPositions) {
   return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
-                        wheelSpeeds);
+                        wheelPositions);
 }
 
 Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
     units::second_t currentTime, const Rotation2d& gyroAngle,
-    const MecanumDriveWheelSpeeds& wheelSpeeds) {
-  auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
-  m_prevTime = currentTime;
+    const MecanumDriveWheelPositions& wheelPositions) {
+  m_odometry.Update(gyroAngle, wheelPositions);
 
-  auto angle = gyroAngle + m_gyroOffset;
-  auto omega = (angle - m_previousAngle).Radians() / dt;
+  MecanumDriveWheelPositions internalWheelPositions{
+      wheelPositions.frontLeft, wheelPositions.frontRight,
+      wheelPositions.rearLeft, wheelPositions.rearRight};
 
-  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
-  auto fieldRelativeVelocities =
-      Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
-          .RotateBy(angle);
-
-  Eigen::Vector<double, 3> u{fieldRelativeVelocities.X().value(),
-                             fieldRelativeVelocities.Y().value(),
-                             omega.value()};
-
-  Eigen::Vector<double, 1> localY{angle.Radians().value()};
-  m_previousAngle = angle;
-
-  m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
-
-  m_observer.Predict(u, dt);
-  m_observer.Correct(u, localY);
+  m_poseBuffer.AddSample(
+      currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions});
 
   return GetEstimatedPosition();
 }
diff --git a/wpimath/src/main/native/cpp/estimator/SwerveDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/SwerveDrivePoseEstimator.cpp
new file mode 100644
index 0000000..f2ed301
--- /dev/null
+++ b/wpimath/src/main/native/cpp/estimator/SwerveDrivePoseEstimator.cpp
@@ -0,0 +1,12 @@
+// 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/estimator/SwerveDrivePoseEstimator.h"
+
+namespace frc {
+
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+    SwerveDrivePoseEstimator<4>;
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/cpp/estimator/UnscentedKalmanFilter.cpp b/wpimath/src/main/native/cpp/estimator/UnscentedKalmanFilter.cpp
new file mode 100644
index 0000000..d5e869b
--- /dev/null
+++ b/wpimath/src/main/native/cpp/estimator/UnscentedKalmanFilter.cpp
@@ -0,0 +1,14 @@
+// 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/estimator/UnscentedKalmanFilter.h"
+
+namespace frc {
+
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+    UnscentedKalmanFilter<3, 3, 1>;
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+    UnscentedKalmanFilter<5, 3, 3>;
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp
new file mode 100644
index 0000000..1d9c42a
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/CoordinateAxis.h"
+
+using namespace frc;
+
+CoordinateAxis::CoordinateAxis(double x, double y, double z) : m_axis{x, y, z} {
+  m_axis /= m_axis.norm();
+}
+
+const CoordinateAxis& CoordinateAxis::N() {
+  static const CoordinateAxis instance{1.0, 0.0, 0.0};
+  return instance;
+}
+
+const CoordinateAxis& CoordinateAxis::S() {
+  static const CoordinateAxis instance{-1.0, 0.0, 0.0};
+  return instance;
+}
+
+const CoordinateAxis& CoordinateAxis::E() {
+  static const CoordinateAxis instance{0.0, -1.0, 0.0};
+  return instance;
+}
+
+const CoordinateAxis& CoordinateAxis::W() {
+  static const CoordinateAxis instance{0.0, 1.0, 0.0};
+  return instance;
+}
+
+const CoordinateAxis& CoordinateAxis::U() {
+  static const CoordinateAxis instance{0.0, 0.0, 1.0};
+  return instance;
+}
+
+const CoordinateAxis& CoordinateAxis::D() {
+  static const CoordinateAxis instance{0.0, 0.0, -1.0};
+  return instance;
+}
diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp
new file mode 100644
index 0000000..d1ee93d
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp
@@ -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.
+
+#include "frc/geometry/CoordinateSystem.h"
+
+#include <cmath>
+#include <stdexcept>
+#include <utility>
+
+#include "Eigen/QR"
+
+using namespace frc;
+
+CoordinateSystem::CoordinateSystem(const CoordinateAxis& positiveX,
+                                   const CoordinateAxis& positiveY,
+                                   const CoordinateAxis& positiveZ) {
+  // 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;
+  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;
+
+  // The change of basis matrix should be a pure rotation. The Rotation3d
+  // constructor will verify this by checking for special orthogonality.
+  m_rotation = Rotation3d{R};
+}
+
+const CoordinateSystem& CoordinateSystem::NWU() {
+  static const CoordinateSystem instance{
+      CoordinateAxis::N(), CoordinateAxis::W(), CoordinateAxis::U()};
+  return instance;
+}
+
+const CoordinateSystem& CoordinateSystem::EDN() {
+  static const CoordinateSystem instance{
+      CoordinateAxis::E(), CoordinateAxis::D(), CoordinateAxis::N()};
+  return instance;
+}
+
+const CoordinateSystem& CoordinateSystem::NED() {
+  static const CoordinateSystem instance{
+      CoordinateAxis::N(), CoordinateAxis::E(), CoordinateAxis::D()};
+  return instance;
+}
+
+Translation3d CoordinateSystem::Convert(const Translation3d& translation,
+                                        const CoordinateSystem& from,
+                                        const CoordinateSystem& to) {
+  return translation.RotateBy(from.m_rotation - to.m_rotation);
+}
+
+Rotation3d CoordinateSystem::Convert(const Rotation3d& rotation,
+                                     const CoordinateSystem& from,
+                                     const CoordinateSystem& to) {
+  return rotation.RotateBy(from.m_rotation - to.m_rotation);
+}
+
+Pose3d CoordinateSystem::Convert(const Pose3d& pose,
+                                 const CoordinateSystem& from,
+                                 const CoordinateSystem& to) {
+  return Pose3d{Convert(pose.Translation(), from, to),
+                Convert(pose.Rotation(), from, to)};
+}
+
+Transform3d CoordinateSystem::Convert(const Transform3d& transform,
+                                      const CoordinateSystem& from,
+                                      const CoordinateSystem& to) {
+  return Transform3d{Convert(transform.Translation(), from, to),
+                     Convert(transform.Rotation(), from, to)};
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
index b7176cd..2648a90 100644
--- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
@@ -10,32 +10,9 @@
 
 using namespace frc;
 
-Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
-    : m_translation(translation), m_rotation(rotation) {}
-
-Pose2d::Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
-    : m_translation(x, y), m_rotation(rotation) {}
-
-Pose2d Pose2d::operator+(const Transform2d& other) const {
-  return TransformBy(other);
-}
-
 Transform2d Pose2d::operator-(const Pose2d& other) const {
   const auto pose = this->RelativeTo(other);
-  return Transform2d(pose.Translation(), pose.Rotation());
-}
-
-bool Pose2d::operator==(const Pose2d& other) const {
-  return m_translation == other.m_translation && m_rotation == other.m_rotation;
-}
-
-bool Pose2d::operator!=(const Pose2d& other) const {
-  return !operator==(other);
-}
-
-Pose2d Pose2d::TransformBy(const Transform2d& other) const {
-  return {m_translation + (other.Translation().RotateBy(m_rotation)),
-          m_rotation + other.Rotation()};
+  return Transform2d{pose.Translation(), pose.Rotation()};
 }
 
 Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
@@ -87,7 +64,7 @@
           {halfThetaByTanOfHalfDtheta, -halfDtheta}) *
       std::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
 
-  return {translationPart.X(), translationPart.Y(), units::radian_t(dtheta)};
+  return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}};
 }
 
 void frc::to_json(wpi::json& json, const Pose2d& pose) {
diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
new file mode 100644
index 0000000..4732c4d
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
@@ -0,0 +1,155 @@
+// 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/Pose3d.h"
+
+#include <cmath>
+
+#include <wpi/json.h>
+
+using namespace frc;
+
+namespace {
+
+/**
+ * Applies the hat operator to a rotation vector.
+ *
+ * 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.
+ */
+Matrixd<3, 3> RotationVectorToMatrix(const Vectord<3>& 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}};
+}
+}  // namespace
+
+Pose3d::Pose3d(Translation3d translation, Rotation3d rotation)
+    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+
+Pose3d::Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
+               Rotation3d rotation)
+    : m_translation(x, y, z), m_rotation(std::move(rotation)) {}
+
+Pose3d::Pose3d(const Pose2d& pose)
+    : m_translation(pose.X(), pose.Y(), 0_m),
+      m_rotation(0_rad, 0_rad, pose.Rotation().Radians()) {}
+
+Pose3d Pose3d::operator+(const Transform3d& other) const {
+  return TransformBy(other);
+}
+
+Transform3d Pose3d::operator-(const Pose3d& other) const {
+  const auto pose = this->RelativeTo(other);
+  return Transform3d{pose.Translation(), pose.Rotation()};
+}
+
+Pose3d Pose3d::operator*(double scalar) const {
+  return Pose3d{m_translation * scalar, m_rotation * scalar};
+}
+
+Pose3d Pose3d::operator/(double scalar) const {
+  return *this * (1.0 / scalar);
+}
+
+Pose3d Pose3d::TransformBy(const Transform3d& other) const {
+  return {m_translation + (other.Translation().RotateBy(m_rotation)),
+          other.Rotation() + m_rotation};
+}
+
+Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
+  const Transform3d transform{other, *this};
+  return {transform.Translation(), transform.Rotation()};
+}
+
+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;
+
+  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;
+  } 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;
+  }
+
+  // Get translation component
+  Vectord<3> translation =
+      J * Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dz.value()};
+
+  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}};
+
+  return *this + transform;
+}
+
+Twist3d Pose3d::Log(const Pose3d& end) const {
+  const auto transform = end.RelativeTo(*this);
+
+  Vectord<3> rotVec = transform.Rotation().GetQuaternion().ToRotationVector();
+
+  Matrixd<3, 3> Omega = RotationVectorToMatrix(rotVec);
+  Matrixd<3, 3> OmegaSq = Omega * Omega;
+
+  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;
+  } 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;
+  }
+
+  // Get dtranslation component
+  Vectord<3> dtranslation =
+      Jinv * Vectord<3>{transform.X().value(), transform.Y().value(),
+                        transform.Z().value()};
+
+  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)}};
+}
+
+Pose2d Pose3d::ToPose2d() const {
+  return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
+}
+
+void frc::to_json(wpi::json& json, const Pose3d& pose) {
+  json = wpi::json{{"translation", pose.Translation()},
+                   {"rotation", pose.Rotation()}};
+}
+
+void frc::from_json(const wpi::json& json, Pose3d& pose) {
+  pose = Pose3d{json.at("translation").get<Translation3d>(),
+                json.at("rotation").get<Rotation3d>()};
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
new file mode 100644
index 0000000..9c2ceda
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
@@ -0,0 +1,94 @@
+// 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/Quaternion.h"
+
+#include <wpi/json.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 {
+  // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
+  const auto& r1 = m_r;
+  const auto& v1 = m_v;
+  const auto& r2 = other.m_r;
+  const auto& v2 = other.m_v;
+
+  // v₁ x v₂
+  Eigen::Vector3d cross{v1(1) * v2(2) - v2(1) * v1(2),
+                        v2(0) * v1(2) - v1(0) * v2(2),
+                        v1(0) * v2(1) - v2(0) * v1(1)};
+
+  // 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)};
+}
+
+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;
+}
+
+Quaternion Quaternion::Inverse() const {
+  return Quaternion{m_r, -m_v(0), -m_v(1), -m_v(2)};
+}
+
+Quaternion Quaternion::Normalize() const {
+  double norm = std::sqrt(W() * W() + X() * X() + Y() * Y() + Z() * Z());
+  if (norm == 0.0) {
+    return Quaternion{};
+  } else {
+    return Quaternion{W() / norm, X() / norm, Y() / norm, Z() / norm};
+  }
+}
+
+double Quaternion::W() const {
+  return m_r;
+}
+
+double Quaternion::X() const {
+  return m_v(0);
+}
+
+double Quaternion::Y() const {
+  return m_v(1);
+}
+
+double Quaternion::Z() const {
+  return m_v(2);
+}
+
+Eigen::Vector3d Quaternion::ToRotationVector() const {
+  // See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
+  // Sound State Representation through Encapsulation of Manifolds"
+  //
+  // https://arxiv.org/pdf/1107.1119.pdf
+  double norm = m_v.norm();
+
+  if (norm < 1e-9) {
+    return (2.0 / W() - 2.0 / 3.0 * norm * norm / (W() * W() * W())) * m_v;
+  } else {
+    if (W() < 0.0) {
+      return 2.0 * std::atan2(-norm, -W()) / norm * m_v;
+    } else {
+      return 2.0 * std::atan2(norm, W()) / norm * m_v;
+    }
+  }
+}
+
+void frc::to_json(wpi::json& json, const Quaternion& quaternion) {
+  json = wpi::json{{"W", quaternion.W()},
+                   {"X", quaternion.X()},
+                   {"Y", quaternion.Y()},
+                   {"Z", quaternion.Z()}};
+}
+
+void frc::from_json(const wpi::json& json, Quaternion& quaternion) {
+  quaternion =
+      Quaternion{json.at("W").get<double>(), json.at("X").get<double>(),
+                 json.at("Y").get<double>(), json.at("Z").get<double>()};
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
index 27af5ed..921e1f8 100644
--- a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
@@ -12,57 +12,6 @@
 
 using namespace frc;
 
-Rotation2d::Rotation2d(units::radian_t value)
-    : m_value(value),
-      m_cos(units::math::cos(value)),
-      m_sin(units::math::sin(value)) {}
-
-Rotation2d::Rotation2d(units::degree_t value)
-    : m_value(value),
-      m_cos(units::math::cos(value)),
-      m_sin(units::math::sin(value)) {}
-
-Rotation2d::Rotation2d(double x, double y) {
-  const auto magnitude = std::hypot(x, y);
-  if (magnitude > 1e-6) {
-    m_sin = y / magnitude;
-    m_cos = x / magnitude;
-  } else {
-    m_sin = 0.0;
-    m_cos = 1.0;
-  }
-  m_value = units::radian_t(std::atan2(m_sin, m_cos));
-}
-
-Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
-  return RotateBy(other);
-}
-
-Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
-  return *this + -other;
-}
-
-Rotation2d Rotation2d::operator-() const {
-  return Rotation2d(-m_value);
-}
-
-Rotation2d Rotation2d::operator*(double scalar) const {
-  return Rotation2d(m_value * scalar);
-}
-
-bool Rotation2d::operator==(const Rotation2d& other) const {
-  return std::hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9;
-}
-
-bool Rotation2d::operator!=(const Rotation2d& other) const {
-  return !operator==(other);
-}
-
-Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
-  return {Cos() * other.Cos() - Sin() * other.Sin(),
-          Cos() * other.Sin() + Sin() * other.Cos()};
-}
-
 void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
   json = wpi::json{{"radians", rotation.Radians().value()}};
 }
diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
new file mode 100644
index 0000000..298f0e6
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
@@ -0,0 +1,242 @@
+// 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/Rotation3d.h"
+
+#include <cmath>
+#include <numbers>
+
+#include <wpi/json.h>
+
+#include "Eigen/Core"
+#include "Eigen/LU"
+#include "Eigen/QR"
+#include "frc/fmt/Eigen.h"
+#include "units/math.h"
+#include "wpimath/MathShared.h"
+
+using namespace frc;
+
+Rotation3d::Rotation3d(const Quaternion& q) {
+  m_q = q.Normalize();
+}
+
+Rotation3d::Rotation3d(units::radian_t roll, units::radian_t pitch,
+                       units::radian_t yaw) {
+  // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
+  double cr = units::math::cos(roll * 0.5);
+  double sr = units::math::sin(roll * 0.5);
+
+  double cp = units::math::cos(pitch * 0.5);
+  double sp = units::math::sin(pitch * 0.5);
+
+  double cy = units::math::cos(yaw * 0.5);
+  double sy = units::math::sin(yaw * 0.5);
+
+  m_q = Quaternion{cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy,
+                   cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy};
+}
+
+Rotation3d::Rotation3d(const Vectord<3>& 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);
+  m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
+}
+
+Rotation3d::Rotation3d(const Matrixd<3, 3>& 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) {
+    std::string msg =
+        fmt::format("Rotation matrix isn't orthogonal\n\nR =\n{}\n", R);
+
+    wpi::math::MathSharedStore::ReportError(msg);
+    throw std::domain_error(msg);
+  }
+  if (std::abs(R.determinant() - 1.0) > 1e-9) {
+    std::string msg = fmt::format(
+        "Rotation matrix is orthogonal but not special orthogonal\n\nR =\n{}\n",
+        R);
+
+    wpi::math::MathSharedStore::ReportError(msg);
+    throw std::domain_error(msg);
+  }
+
+  // Turn rotation matrix into a quaternion
+  // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+  double trace = R(0, 0) + R(1, 1) + R(2, 2);
+  double w;
+  double x;
+  double y;
+  double z;
+
+  if (trace > 0.0) {
+    double s = 0.5 / std::sqrt(trace + 1.0);
+    w = 0.25 / s;
+    x = (R(2, 1) - R(1, 2)) * s;
+    y = (R(0, 2) - R(2, 0)) * s;
+    z = (R(1, 0) - R(0, 1)) * s;
+  } else {
+    if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
+      double s = 2.0 * std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2));
+      w = (R(2, 1) - R(1, 2)) / s;
+      x = 0.25 * s;
+      y = (R(0, 1) + R(1, 0)) / s;
+      z = (R(0, 2) + R(2, 0)) / s;
+    } else if (R(1, 1) > R(2, 2)) {
+      double s = 2.0 * std::sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2));
+      w = (R(0, 2) - R(2, 0)) / s;
+      x = (R(0, 1) + R(1, 0)) / s;
+      y = 0.25 * s;
+      z = (R(1, 2) + R(2, 1)) / s;
+    } else {
+      double s = 2.0 * std::sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1));
+      w = (R(1, 0) - R(0, 1)) / s;
+      x = (R(0, 2) + R(2, 0)) / s;
+      y = (R(1, 2) + R(2, 1)) / s;
+      z = 0.25 * s;
+    }
+  }
+
+  m_q = Quaternion{w, x, y, z};
+}
+
+Rotation3d::Rotation3d(const Vectord<3>& initial, const Vectord<3>& final) {
+  double dot = initial.dot(final);
+  double normProduct = initial.norm() * final.norm();
+  double dotNorm = dot / normProduct;
+
+  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.
+    return;
+  } else if (dotNorm < -1.0 + 1E-9) {
+    // If the dot product is -1, the two vectors point in opposite directions so
+    // a 180 degree rotation is required. Any orthogonal vector can be used for
+    // it. Q in the QR decomposition is an orthonormal basis, so it contains
+    // orthogonal unit vectors.
+    Eigen::Matrix<double, 3, 1> X = initial;
+    Eigen::HouseholderQR<decltype(X)> qr{X};
+    Eigen::Matrix<double, 3, 3> Q = qr.householderQ();
+
+    // w = std::cos(θ/2) = std::cos(90°) = 0
+    //
+    // For x, y, and z, we use the second column of Q because the first is
+    // parallel instead of orthogonal. The third column would also work.
+    m_q = Quaternion{0.0, Q(0, 1), Q(1, 1), Q(2, 1)};
+  } else {
+    // initial x final
+    Eigen::Vector3d axis{initial(1) * final(2) - final(1) * initial(2),
+                         final(0) * initial(2) - initial(0) * final(2),
+                         initial(0) * final(1) - final(0) * initial(1)};
+
+    // https://stackoverflow.com/a/11741520
+    m_q = Quaternion{normProduct + dot, axis(0), axis(1), axis(2)}.Normalize();
+  }
+}
+
+Rotation3d Rotation3d::operator+(const Rotation3d& other) const {
+  return RotateBy(other);
+}
+
+Rotation3d Rotation3d::operator-(const Rotation3d& other) const {
+  return *this + -other;
+}
+
+Rotation3d Rotation3d::operator-() const {
+  return Rotation3d{m_q.Inverse()};
+}
+
+Rotation3d Rotation3d::operator*(double scalar) const {
+  // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
+  if (m_q.W() >= 0.0) {
+    return Rotation3d{{m_q.X(), m_q.Y(), m_q.Z()},
+                      2.0 * units::radian_t{scalar * std::acos(m_q.W())}};
+  } else {
+    return Rotation3d{{-m_q.X(), -m_q.Y(), -m_q.Z()},
+                      2.0 * units::radian_t{scalar * std::acos(-m_q.W())}};
+  }
+}
+
+Rotation3d Rotation3d::operator/(double scalar) const {
+  return *this * (1.0 / scalar);
+}
+
+Rotation3d Rotation3d::RotateBy(const Rotation3d& other) const {
+  return Rotation3d{other.m_q * m_q};
+}
+
+const Quaternion& Rotation3d::GetQuaternion() const {
+  return m_q;
+}
+
+units::radian_t Rotation3d::X() const {
+  double w = m_q.W();
+  double x = m_q.X();
+  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))};
+}
+
+units::radian_t Rotation3d::Y() const {
+  double w = m_q.W();
+  double x = m_q.X();
+  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
+  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)};
+  } else {
+    return units::radian_t{std::asin(ratio)};
+  }
+}
+
+units::radian_t Rotation3d::Z() const {
+  double w = m_q.W();
+  double x = m_q.X();
+  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))};
+}
+
+Vectord<3> 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};
+  } else {
+    return {m_q.X() / norm, m_q.Y() / norm, m_q.Z() / norm};
+  }
+}
+
+units::radian_t Rotation3d::Angle() const {
+  double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
+  return units::radian_t{2.0 * std::atan2(norm, m_q.W())};
+}
+
+Rotation2d Rotation3d::ToRotation2d() const {
+  return Rotation2d{Z()};
+}
+
+void frc::to_json(wpi::json& json, const Rotation3d& rotation) {
+  json = wpi::json{{"quaternion", rotation.GetQuaternion()}};
+}
+
+void frc::from_json(const wpi::json& json, Rotation3d& rotation) {
+  rotation = Rotation3d{json.at("quaternion").get<Quaternion>()};
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
index 0808f35..25b0590 100644
--- a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
@@ -18,24 +18,6 @@
   m_rotation = final.Rotation() - initial.Rotation();
 }
 
-Transform2d::Transform2d(Translation2d translation, Rotation2d rotation)
-    : m_translation(translation), m_rotation(rotation) {}
-
-Transform2d Transform2d::Inverse() const {
-  // We are rotating the difference between the translations
-  // using a clockwise rotation matrix. This transforms the global
-  // delta into a local delta (relative to the initial pose).
-  return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
-}
-
 Transform2d Transform2d::operator+(const Transform2d& other) const {
   return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
 }
-
-bool Transform2d::operator==(const Transform2d& other) const {
-  return m_translation == other.m_translation && m_rotation == other.m_rotation;
-}
-
-bool Transform2d::operator!=(const Transform2d& other) const {
-  return !operator==(other);
-}
diff --git a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
new file mode 100644
index 0000000..1cfabaa
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
@@ -0,0 +1,33 @@
+// 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/Transform3d.h"
+
+#include "frc/geometry/Pose3d.h"
+
+using namespace frc;
+
+Transform3d::Transform3d(Pose3d initial, Pose3d final) {
+  // We are rotating the difference between the translations
+  // using a clockwise rotation matrix. This transforms the global
+  // delta into a local delta (relative to the initial pose).
+  m_translation = (final.Translation() - initial.Translation())
+                      .RotateBy(-initial.Rotation());
+
+  m_rotation = final.Rotation() - initial.Rotation();
+}
+
+Transform3d::Transform3d(Translation3d translation, Rotation3d rotation)
+    : m_translation(std::move(translation)), 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
+  // delta into a local delta (relative to the initial pose).
+  return Transform3d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
+}
+
+Transform3d Transform3d::operator+(const Transform3d& other) const {
+  return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
index 5a30ec2..d463696 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
@@ -10,12 +10,6 @@
 
 using namespace frc;
 
-Translation2d::Translation2d(units::meter_t x, units::meter_t y)
-    : m_x(x), m_y(y) {}
-
-Translation2d::Translation2d(units::meter_t distance, const Rotation2d& angle)
-    : m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {}
-
 units::meter_t Translation2d::Distance(const Translation2d& other) const {
   return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
 }
@@ -24,40 +18,11 @@
   return units::math::hypot(m_x, m_y);
 }
 
-Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
-  return {m_x * other.Cos() - m_y * other.Sin(),
-          m_x * other.Sin() + m_y * other.Cos()};
-}
-
-Translation2d Translation2d::operator+(const Translation2d& other) const {
-  return {X() + other.X(), Y() + other.Y()};
-}
-
-Translation2d Translation2d::operator-(const Translation2d& other) const {
-  return *this + -other;
-}
-
-Translation2d Translation2d::operator-() const {
-  return {-m_x, -m_y};
-}
-
-Translation2d Translation2d::operator*(double scalar) const {
-  return {scalar * m_x, scalar * m_y};
-}
-
-Translation2d Translation2d::operator/(double scalar) const {
-  return *this * (1.0 / scalar);
-}
-
 bool Translation2d::operator==(const Translation2d& other) const {
   return units::math::abs(m_x - other.m_x) < 1E-9_m &&
          units::math::abs(m_y - other.m_y) < 1E-9_m;
 }
 
-bool Translation2d::operator!=(const Translation2d& other) const {
-  return !operator==(other);
-}
-
 void frc::to_json(wpi::json& json, const Translation2d& translation) {
   json =
       wpi::json{{"x", translation.X().value()}, {"y", translation.Y().value()}};
diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
new file mode 100644
index 0000000..2c53791
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Translation3d.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 "frc/geometry/Translation3d.h"
+
+#include <wpi/json.h>
+
+#include "units/length.h"
+#include "units/math.h"
+
+using namespace frc;
+
+Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
+  auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
+  m_x = rectangular.X();
+  m_y = rectangular.Y();
+  m_z = rectangular.Z();
+}
+
+units::meter_t Translation3d::Distance(const Translation3d& other) const {
+  return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) +
+                           units::math::pow<2>(other.m_y - m_y) +
+                           units::math::pow<2>(other.m_z - m_z));
+}
+
+units::meter_t Translation3d::Norm() const {
+  return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
+}
+
+Translation3d Translation3d::RotateBy(const Rotation3d& other) const {
+  Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()};
+  auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse();
+  return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()},
+                       units::meter_t{qprime.Z()}};
+}
+
+bool Translation3d::operator==(const Translation3d& other) const {
+  return units::math::abs(m_x - other.m_x) < 1E-9_m &&
+         units::math::abs(m_y - other.m_y) < 1E-9_m &&
+         units::math::abs(m_z - other.m_z) < 1E-9_m;
+}
+
+void frc::to_json(wpi::json& json, const Translation3d& translation) {
+  json = wpi::json{{"x", translation.X().value()},
+                   {"y", translation.Y().value()},
+                   {"z", translation.Z().value()}};
+}
+
+void frc::from_json(const wpi::json& json, Translation3d& translation) {
+  translation = Translation3d{units::meter_t{json.at("x").get<double>()},
+                              units::meter_t{json.at("y").get<double>()},
+                              units::meter_t{json.at("z").get<double>()}};
+}
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
index b036833..fd8a223 100644
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
@@ -8,6 +8,7 @@
 
 #include <wpi/jni_util.h>
 
+#include "Eigen/Cholesky"
 #include "Eigen/Core"
 #include "Eigen/Eigenvalues"
 #include "Eigen/QR"
@@ -70,7 +71,7 @@
   return elements;
 }
 
-frc::Trajectory CreateTrajectoryFromElements(wpi::span<const double> elements) {
+frc::Trajectory CreateTrajectoryFromElements(std::span<const double> elements) {
   // Make sure that the elements have the correct length.
   assert(elements.size() % 7 == 0);
 
@@ -307,4 +308,60 @@
   }
 }
 
+/*
+ * 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/kinematics/DifferentialDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
index c4a4311..1ff7a8a 100644
--- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
@@ -9,8 +9,11 @@
 using namespace frc;
 
 DifferentialDriveOdometry::DifferentialDriveOdometry(
-    const Rotation2d& gyroAngle, const Pose2d& initialPose)
-    : m_pose(initialPose) {
+    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;
   wpi::math::MathSharedStore::ReportUsage(
diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
index c4a71fd..298dd7f 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
@@ -25,8 +25,7 @@
                                       chassisSpeeds.vy.value(),
                                       chassisSpeeds.omega.value()};
 
-  Eigen::Vector<double, 4> wheelsVector =
-      m_inverseKinematics * chassisSpeedsVector;
+  Vectord<4> wheelsVector = m_inverseKinematics * chassisSpeedsVector;
 
   MecanumDriveWheelSpeeds wheelSpeeds;
   wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsVector(0)};
@@ -38,7 +37,7 @@
 
 ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
     const MecanumDriveWheelSpeeds& wheelSpeeds) const {
-  Eigen::Vector<double, 4> wheelSpeedsVector{
+  Vectord<4> wheelSpeedsVector{
       wheelSpeeds.frontLeft.value(), wheelSpeeds.frontRight.value(),
       wheelSpeeds.rearLeft.value(), wheelSpeeds.rearRight.value()};
 
@@ -50,13 +49,24 @@
           units::radians_per_second_t{chassisSpeedsVector(2)}};
 }
 
+Twist2d MecanumDriveKinematics::ToTwist2d(
+    const MecanumDriveWheelPositions& wheelDeltas) const {
+  Vectord<4> 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)}};
+}
+
 void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
                                                   Translation2d fr,
                                                   Translation2d rl,
                                                   Translation2d rr) const {
-  m_inverseKinematics =
-      Eigen::Matrix<double, 4, 3>{{1, -1, (-(fl.X() + fl.Y())).value()},
-                                  {1, 1, (fr.X() - fr.Y()).value()},
-                                  {1, 1, (rl.X() - rl.Y()).value()},
-                                  {1, -1, (-(rr.X() + rr.Y())).value()}};
+  m_inverseKinematics = Matrixd<4, 3>{{1, -1, (-(fl.X() + fl.Y())).value()},
+                                      {1, 1, (fr.X() - fr.Y()).value()},
+                                      {1, 1, (rl.X() - rl.Y()).value()},
+                                      {1, -1, (-(rr.X() + rr.Y())).value()}};
 }
diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
index bbeee58..394adaf 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
@@ -8,32 +8,37 @@
 
 using namespace frc;
 
-MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
-                                           const Rotation2d& gyroAngle,
-                                           const Pose2d& initialPose)
-    : m_kinematics(kinematics), m_pose(initialPose) {
+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;
   wpi::math::MathSharedStore::ReportUsage(
       wpi::math::MathUsageId::kOdometry_MecanumDrive, 1);
 }
 
-const Pose2d& MecanumDriveOdometry::UpdateWithTime(
-    units::second_t currentTime, const Rotation2d& gyroAngle,
-    MecanumDriveWheelSpeeds wheelSpeeds) {
-  units::second_t deltaTime =
-      (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
-  m_previousTime = currentTime;
-
+const Pose2d& MecanumDriveOdometry::Update(
+    const Rotation2d& gyroAngle,
+    const MecanumDriveWheelPositions& wheelPositions) {
   auto angle = gyroAngle + m_gyroOffset;
 
-  auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
-  static_cast<void>(dtheta);
+  MecanumDriveWheelPositions wheelDeltas{
+      wheelPositions.frontLeft - m_previousWheelPositions.frontLeft,
+      wheelPositions.frontRight - m_previousWheelPositions.frontRight,
+      wheelPositions.rearLeft - m_previousWheelPositions.rearLeft,
+      wheelPositions.rearRight - m_previousWheelPositions.rearRight,
+  };
 
-  auto newPose = m_pose.Exp(
-      {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
+  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/SwerveDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/SwerveDriveKinematics.cpp
new file mode 100644
index 0000000..5e6cf28
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/SwerveDriveKinematics.cpp
@@ -0,0 +1,12 @@
+// 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/SwerveDriveKinematics.h"
+
+namespace frc {
+
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+    SwerveDriveKinematics<4>;
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/cpp/kinematics/SwerveDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/SwerveDriveOdometry.cpp
new file mode 100644
index 0000000..7a9d065
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/SwerveDriveOdometry.cpp
@@ -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.
+
+#include "frc/kinematics/SwerveDriveOdometry.h"
+
+namespace frc {
+
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT) SwerveDriveOdometry<4>;
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
index cec620c..e8bbb46 100644
--- a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
+++ b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
@@ -20,10 +20,10 @@
 
   if (waypoints.size() > 1) {
     waypoints.emplace(waypoints.begin(),
-                      Translation2d{units::meter_t(xInitial[0]),
-                                    units::meter_t(yInitial[0])});
+                      Translation2d{units::meter_t{xInitial[0]},
+                                    units::meter_t{yInitial[0]}});
     waypoints.emplace_back(
-        Translation2d{units::meter_t(xFinal[0]), units::meter_t(yFinal[0])});
+        Translation2d{units::meter_t{xFinal[0]}, units::meter_t{yFinal[0]}});
 
     // Populate tridiagonal system for clamped cubic
     /* See:
diff --git a/wpimath/src/main/native/cpp/system/LinearSystemLoop.cpp b/wpimath/src/main/native/cpp/system/LinearSystemLoop.cpp
new file mode 100644
index 0000000..16979d9
--- /dev/null
+++ b/wpimath/src/main/native/cpp/system/LinearSystemLoop.cpp
@@ -0,0 +1,14 @@
+// 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/system/LinearSystemLoop.h"
+
+namespace frc {
+
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+    LinearSystemLoop<1, 1, 1>;
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+    LinearSystemLoop<2, 1, 1>;
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp b/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp
new file mode 100644
index 0000000..3527092
--- /dev/null
+++ b/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp
@@ -0,0 +1,193 @@
+// 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/system/plant/LinearSystemId.h"
+
+using namespace frc;
+
+LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor,
+                                                     units::kilogram_t mass,
+                                                     units::meter_t radius,
+                                                     double G) {
+  if (mass <= 0_kg) {
+    throw std::domain_error("mass must be greater than zero.");
+  }
+  if (radius <= 0_m) {
+    throw std::domain_error("radius must be greater than zero.");
+  }
+  if (G <= 0.0) {
+    throw std::domain_error("G must be greater than zero.");
+  }
+
+  Matrixd<2, 2> A{
+      {0.0, 1.0},
+      {0.0, (-std::pow(G, 2) * motor.Kt /
+             (motor.R * units::math::pow<2>(radius) * mass * motor.Kv))
+                .value()}};
+  Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * radius * mass)).value()};
+  Matrixd<1, 2> C{1.0, 0.0};
+  Matrixd<1, 1> D{0.0};
+
+  return LinearSystem<2, 1, 1>(A, B, C, D);
+}
+
+LinearSystem<2, 1, 1> LinearSystemId::SingleJointedArmSystem(
+    DCMotor motor, units::kilogram_square_meter_t J, double G) {
+  if (J <= 0_kg_sq_m) {
+    throw std::domain_error("J must be greater than zero.");
+  }
+  if (G <= 0.0) {
+    throw std::domain_error("G must be greater than zero.");
+  }
+
+  Matrixd<2, 2> A{
+      {0.0, 1.0},
+      {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
+  Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
+  Matrixd<1, 2> C{1.0, 0.0};
+  Matrixd<1, 1> D{0.0};
+
+  return LinearSystem<2, 1, 1>(A, B, C, D);
+}
+
+LinearSystem<2, 2, 2> LinearSystemId::IdentifyDrivetrainSystem(
+    decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
+    decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular) {
+  if (kVLinear <= decltype(kVLinear){0}) {
+    throw std::domain_error("Kv,linear must be greater than zero.");
+  }
+  if (kALinear <= decltype(kALinear){0}) {
+    throw std::domain_error("Ka,linear must be greater than zero.");
+  }
+  if (kVAngular <= decltype(kVAngular){0}) {
+    throw std::domain_error("Kv,angular must be greater than zero.");
+  }
+  if (kAAngular <= decltype(kAAngular){0}) {
+    throw std::domain_error("Ka,angular must be greater than zero.");
+  }
+
+  double A1 = -(kVLinear.value() / kALinear.value() +
+                kVAngular.value() / kAAngular.value());
+  double A2 = -(kVLinear.value() / kALinear.value() -
+                kVAngular.value() / kAAngular.value());
+  double B1 = 1.0 / kALinear.value() + 1.0 / kAAngular.value();
+  double B2 = 1.0 / kALinear.value() - 1.0 / kAAngular.value();
+
+  Matrixd<2, 2> A = 0.5 * Matrixd<2, 2>{{A1, A2}, {A2, A1}};
+  Matrixd<2, 2> B = 0.5 * Matrixd<2, 2>{{B1, B2}, {B2, B1}};
+  Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
+  Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
+
+  return LinearSystem<2, 2, 2>(A, B, C, D);
+}
+
+LinearSystem<2, 2, 2> LinearSystemId::IdentifyDrivetrainSystem(
+    decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
+    decltype(1_V / 1_rad_per_s) kVAngular,
+    decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth) {
+  if (kVLinear <= decltype(kVLinear){0}) {
+    throw std::domain_error("Kv,linear must be greater than zero.");
+  }
+  if (kALinear <= decltype(kALinear){0}) {
+    throw std::domain_error("Ka,linear must be greater than zero.");
+  }
+  if (kVAngular <= decltype(kVAngular){0}) {
+    throw std::domain_error("Kv,angular must be greater than zero.");
+  }
+  if (kAAngular <= decltype(kAAngular){0}) {
+    throw std::domain_error("Ka,angular must be greater than zero.");
+  }
+  if (trackwidth <= 0_m) {
+    throw std::domain_error("r must be greater than zero.");
+  }
+
+  // We want to find a factor to include in Kv,angular that will convert
+  // `u = Kv,angular omega` to `u = Kv,angular v`.
+  //
+  // v = omega r
+  // omega = v/r
+  // omega = 1/r v
+  // omega = 1/(trackwidth/2) v
+  // omega = 2/trackwidth v
+  //
+  // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
+  // to V/(m/s).
+  return IdentifyDrivetrainSystem(kVLinear, kALinear,
+                                  kVAngular * 2.0 / trackwidth * 1_rad,
+                                  kAAngular * 2.0 / trackwidth * 1_rad);
+}
+
+LinearSystem<1, 1, 1> LinearSystemId::FlywheelSystem(
+    DCMotor motor, units::kilogram_square_meter_t J, double G) {
+  if (J <= 0_kg_sq_m) {
+    throw std::domain_error("J must be greater than zero.");
+  }
+  if (G <= 0.0) {
+    throw std::domain_error("G must be greater than zero.");
+  }
+
+  Matrixd<1, 1> A{
+      (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
+  Matrixd<1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
+  Matrixd<1, 1> C{1.0};
+  Matrixd<1, 1> D{0.0};
+
+  return LinearSystem<1, 1, 1>(A, B, C, D);
+}
+
+LinearSystem<2, 1, 2> LinearSystemId::DCMotorSystem(
+    DCMotor motor, units::kilogram_square_meter_t J, double G) {
+  if (J <= 0_kg_sq_m) {
+    throw std::domain_error("J must be greater than zero.");
+  }
+  if (G <= 0.0) {
+    throw std::domain_error("G must be greater than zero.");
+  }
+
+  Matrixd<2, 2> A{
+      {0.0, 1.0},
+      {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
+  Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).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);
+}
+
+LinearSystem<2, 2, 2> LinearSystemId::DrivetrainVelocitySystem(
+    const DCMotor& motor, units::kilogram_t mass, units::meter_t r,
+    units::meter_t rb, units::kilogram_square_meter_t J, double G) {
+  if (mass <= 0_kg) {
+    throw std::domain_error("mass must be greater than zero.");
+  }
+  if (r <= 0_m) {
+    throw std::domain_error("r must be greater than zero.");
+  }
+  if (rb <= 0_m) {
+    throw std::domain_error("rb must be greater than zero.");
+  }
+  if (J <= 0_kg_sq_m) {
+    throw std::domain_error("J must be greater than zero.");
+  }
+  if (G <= 0.0) {
+    throw std::domain_error("G must be greater than zero.");
+  }
+
+  auto C1 = -std::pow(G, 2) * motor.Kt /
+            (motor.Kv * motor.R * units::math::pow<2>(r));
+  auto C2 = G * motor.Kt / (motor.R * r);
+
+  Matrixd<2, 2> A{{((1 / mass + units::math::pow<2>(rb) / J) * C1).value(),
+                   ((1 / mass - units::math::pow<2>(rb) / J) * C1).value()},
+                  {((1 / mass - units::math::pow<2>(rb) / J) * C1).value(),
+                   ((1 / mass + units::math::pow<2>(rb) / J) * C1).value()}};
+  Matrixd<2, 2> B{{((1 / mass + units::math::pow<2>(rb) / J) * C2).value(),
+                   ((1 / mass - units::math::pow<2>(rb) / J) * C2).value()},
+                  {((1 / mass - units::math::pow<2>(rb) / J) * C2).value(),
+                   ((1 / mass + units::math::pow<2>(rb) / J) * C2).value()}};
+  Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
+  Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
+
+  return LinearSystem<2, 2, 2>(A, B, C, D);
+}
diff --git a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
index ecb36c2..de47547 100644
--- a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
@@ -13,16 +13,6 @@
 
 using namespace frc;
 
-bool Trajectory::State::operator==(const Trajectory::State& other) const {
-  return t == other.t && velocity == other.velocity &&
-         acceleration == other.acceleration && pose == other.pose &&
-         curvature == other.curvature;
-}
-
-bool Trajectory::State::operator!=(const Trajectory::State& other) const {
-  return !operator==(other);
-}
-
 Trajectory::State Trajectory::State::Interpolate(State endValue,
                                                  double i) const {
   // Find the new [t] value.
@@ -46,7 +36,7 @@
   const units::meters_per_second_t newV = velocity + (acceleration * deltaT);
 
   // Calculate the change in position.
-  // delta_s = v_0 t + 0.5 at^2
+  // delta_s = v_0 t + 0.5at²
   const units::meter_t newS =
       (velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) *
       (reversing ? -1.0 : 1.0);
@@ -163,11 +153,3 @@
       units::meters_per_second_squared_t{json.at("acceleration").get<double>()};
   state.curvature = units::curvature_t{json.at("curvature").get<double>()};
 }
-
-bool Trajectory::operator==(const Trajectory& other) const {
-  return m_states == other.States();
-}
-
-bool Trajectory::operator!=(const Trajectory& other) const {
-  return !operator==(other);
-}
diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
index 1884758..c7a7e9a 100644
--- a/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
@@ -30,7 +30,7 @@
     Spline<3>::ControlVector initial,
     const std::vector<Translation2d>& interiorWaypoints,
     Spline<3>::ControlVector end, const TrajectoryConfig& config) {
-  const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+  const Transform2d flip{Translation2d{}, 180_deg};
 
   // Make theta normal for trajectory generation if path is reversed.
   // Flip the headings.
@@ -76,7 +76,7 @@
 Trajectory TrajectoryGenerator::GenerateTrajectory(
     std::vector<Spline<5>::ControlVector> controlVectors,
     const TrajectoryConfig& config) {
-  const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+  const Transform2d flip{Translation2d{}, 180_deg};
   // Make theta normal for trajectory generation if path is reversed.
   if (config.IsReversed()) {
     for (auto& vector : controlVectors) {
@@ -112,7 +112,7 @@
 Trajectory TrajectoryGenerator::GenerateTrajectory(
     const std::vector<Pose2d>& waypoints, const TrajectoryConfig& config) {
   auto newWaypoints = waypoints;
-  const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+  const Transform2d flip{Translation2d{}, 180_deg};
   if (config.IsReversed()) {
     for (auto& waypoint : newWaypoints) {
       waypoint = waypoint + flip;
diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
index d397d0c..92d52ed 100644
--- a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
@@ -62,7 +62,7 @@
     // acceleration, since acceleration limits may be a function of velocity.
     while (true) {
       // Enforce global max velocity and max reachable velocity by global
-      // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
+      // acceleration limit. v_f = √(v_i² + 2ad).
 
       constrainedState.maxVelocity = units::math::min(
           maxVelocity,
@@ -126,7 +126,7 @@
 
     while (true) {
       // Enforce max velocity limit (reverse)
-      // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
+      // v_f = √(v_i² + 2ad), where v_i = successor.
       units::meters_per_second_t newMaxVelocity =
           units::math::sqrt(successor.maxVelocity * successor.maxVelocity +
                             successor.minAcceleration * ds * 2.0);
@@ -187,10 +187,10 @@
     if (i > 0) {
       states.at(i - 1).acceleration = reversed ? -accel : accel;
       if (units::math::abs(accel) > 1E-6_mps_sq) {
-        // v_f = v_0 + a * t
+        // v_f = v_0 + at
         dt = (state.maxVelocity - v) / accel;
       } else if (units::math::abs(v) > 1E-6_mps) {
-        // delta_x = v * t
+        // delta_x = vt
         dt = ds / v;
       } else {
         throw std::runtime_error(fmt::format(
diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
index 738d243..40913a8 100644
--- a/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
@@ -15,12 +15,12 @@
 units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
     const Pose2d& pose, units::curvature_t curvature,
     units::meters_per_second_t velocity) const {
-  // ac = v^2 / r
-  // k (curvature) = 1 / r
+  // ac = v²/r
+  // k (curvature) = 1/r
 
-  // therefore, ac = v^2 * k
-  // ac / k = v^2
-  // v = std::sqrt(ac / k)
+  // therefore, ac = v²k
+  // ac/k = v²
+  // v = √(ac/k)
 
   // We have to multiply by 1_rad here to get the units to cancel out nicely.
   // The units library defines a unit for radians although it is technically
diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
index 7c10201..d60b4b6 100644
--- a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
@@ -23,7 +23,7 @@
 units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(
     const Pose2d& pose, units::curvature_t curvature,
     units::meters_per_second_t velocity) const {
-  return units::meters_per_second_t(std::numeric_limits<double>::max());
+  return units::meters_per_second_t{std::numeric_limits<double>::max()};
 }
 
 TrajectoryConstraint::MinMax
@@ -33,8 +33,8 @@
   auto wheelSpeeds =
       m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature});
 
-  auto maxWheelSpeed = std::max(wheelSpeeds.left, wheelSpeeds.right);
-  auto minWheelSpeed = std::min(wheelSpeeds.left, wheelSpeeds.right);
+  auto maxWheelSpeed = (std::max)(wheelSpeeds.left, wheelSpeeds.right);
+  auto minWheelSpeed = (std::min)(wheelSpeeds.left, wheelSpeeds.right);
 
   // Calculate maximum/minimum possible accelerations from motor dynamics
   // and max/min wheel speeds
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/AutoDiff b/wpimath/src/main/native/eigeninclude/unsupported/Eigen/AutoDiff
deleted file mode 100644
index 7a4ff46..0000000
--- a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/AutoDiff
+++ /dev/null
@@ -1,46 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr>
-//
-// 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_AUTODIFF_MODULE
-#define EIGEN_AUTODIFF_MODULE
-
-namespace Eigen {
-
-/**
-  * \defgroup AutoDiff_Module Auto Diff module
-  *
-  * This module features forward automatic differentation via a simple
-  * templated scalar type wrapper AutoDiffScalar.
-  *
-  * Warning : this should NOT be confused with numerical differentiation, which
-  * is a different method and has its own module in Eigen : \ref NumericalDiff_Module.
-  *
-  * \code
-  * #include <unsupported/Eigen/AutoDiff>
-  * \endcode
-  */
-//@{
-
-}
-#include "../../Eigen/src/Core/util/DisableStupidWarnings.h"
-
-
-#include "src/AutoDiff/AutoDiffScalar.h"
-// #include "src/AutoDiff/AutoDiffVector.h"
-#include "src/AutoDiff/AutoDiffJacobian.h"
-
-#include "../../Eigen/src/Core/util/ReenableStupidWarnings.h"
-
-
-
-namespace Eigen {
-//@}
-}
-
-#endif // EIGEN_AUTODIFF_MODULE
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h b/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h
deleted file mode 100644
index 33b6c39..0000000
--- a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h
+++ /dev/null
@@ -1,108 +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>
-//
-// 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_AUTODIFF_JACOBIAN_H
-#define EIGEN_AUTODIFF_JACOBIAN_H
-
-namespace Eigen
-{
-
-template<typename Functor> class AutoDiffJacobian : public Functor
-{
-public:
-  AutoDiffJacobian() : Functor() {}
-  AutoDiffJacobian(const Functor& f) : Functor(f) {}
-
-  // forward constructors
-#if EIGEN_HAS_VARIADIC_TEMPLATES
-  template<typename... T>
-  AutoDiffJacobian(const T& ...Values) : Functor(Values...) {}
-#else
-  template<typename T0>
-  AutoDiffJacobian(const T0& a0) : Functor(a0) {}
-  template<typename T0, typename T1>
-  AutoDiffJacobian(const T0& a0, const T1& a1) : Functor(a0, a1) {}
-  template<typename T0, typename T1, typename T2>
-  AutoDiffJacobian(const T0& a0, const T1& a1, const T2& a2) : Functor(a0, a1, a2) {}
-#endif
-
-  typedef typename Functor::InputType InputType;
-  typedef typename Functor::ValueType ValueType;
-  typedef typename ValueType::Scalar Scalar;
-
-  enum {
-    InputsAtCompileTime = InputType::RowsAtCompileTime,
-    ValuesAtCompileTime = ValueType::RowsAtCompileTime
-  };
-
-  typedef Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;
-  typedef typename JacobianType::Index Index;
-
-  typedef Matrix<Scalar, InputsAtCompileTime, 1> DerivativeType;
-  typedef AutoDiffScalar<DerivativeType> ActiveScalar;
-
-  typedef Matrix<ActiveScalar, InputsAtCompileTime, 1> ActiveInput;
-  typedef Matrix<ActiveScalar, ValuesAtCompileTime, 1> ActiveValue;
-
-#if EIGEN_HAS_VARIADIC_TEMPLATES
-  // Some compilers don't accept variadic parameters after a default parameter,
-  // i.e., we can't just write _jac=0 but we need to overload operator():
-  EIGEN_STRONG_INLINE
-  void operator() (const InputType& x, ValueType* v) const
-  {
-      this->operator()(x, v, 0);
-  }
-  template<typename... ParamsType>
-  void operator() (const InputType& x, ValueType* v, JacobianType* _jac,
-                   const ParamsType&... Params) const
-#else
-  void operator() (const InputType& x, ValueType* v, JacobianType* _jac=0) const
-#endif
-  {
-    eigen_assert(v!=0);
-
-    if (!_jac)
-    {
-#if EIGEN_HAS_VARIADIC_TEMPLATES
-      Functor::operator()(x, v, Params...);
-#else
-      Functor::operator()(x, v);
-#endif
-      return;
-    }
-
-    JacobianType& jac = *_jac;
-
-    ActiveInput ax = x.template cast<ActiveScalar>();
-    ActiveValue av(jac.rows());
-
-    if(InputsAtCompileTime==Dynamic)
-      for (Index j=0; j<jac.rows(); j++)
-        av[j].derivatives().resize(x.rows());
-
-    for (Index i=0; i<jac.cols(); i++)
-      ax[i].derivatives() = DerivativeType::Unit(x.rows(),i);
-
-#if EIGEN_HAS_VARIADIC_TEMPLATES
-    Functor::operator()(ax, &av, Params...);
-#else
-    Functor::operator()(ax, &av);
-#endif
-
-    for (Index i=0; i<jac.rows(); i++)
-    {
-      (*v)[i] = av[i].value();
-      jac.row(i) = av[i].derivatives();
-    }
-  }
-};
-
-}
-
-#endif // EIGEN_AUTODIFF_JACOBIAN_H
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h b/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
deleted file mode 100644
index 0f166e3..0000000
--- a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
+++ /dev/null
@@ -1,730 +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>
-//
-// 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_AUTODIFF_SCALAR_H
-#define EIGEN_AUTODIFF_SCALAR_H
-
-namespace Eigen {
-
-namespace internal {
-
-template<typename A, typename B>
-struct make_coherent_impl {
-  static void run(A&, B&) {}
-};
-
-// resize a to match b is a.size()==0, and conversely.
-template<typename A, typename B>
-void make_coherent(const A& a, const B&b)
-{
-  make_coherent_impl<A,B>::run(a.const_cast_derived(), b.const_cast_derived());
-}
-
-template<typename DerivativeType, bool Enable> struct auto_diff_special_op;
-
-} // end namespace internal
-
-template<typename DerivativeType> class AutoDiffScalar;
-
-template<typename NewDerType>
-inline AutoDiffScalar<NewDerType> MakeAutoDiffScalar(const typename NewDerType::Scalar& value, const NewDerType &der) {
-  return AutoDiffScalar<NewDerType>(value,der);
-}
-
-/** \class AutoDiffScalar
-  * \brief A scalar type replacement with automatic differentiation capability
-  *
-  * \param DerivativeType the vector type used to store/represent the derivatives. The base scalar type
-  *                 as well as the number of derivatives to compute are determined from this type.
-  *                 Typical choices include, e.g., \c Vector4f for 4 derivatives, or \c VectorXf
-  *                 if the number of derivatives is not known at compile time, and/or, the number
-  *                 of derivatives is large.
-  *                 Note that DerivativeType can also be a reference (e.g., \c VectorXf&) to wrap a
-  *                 existing vector into an AutoDiffScalar.
-  *                 Finally, DerivativeType can also be any Eigen compatible expression.
-  *
-  * This class represents a scalar value while tracking its respective derivatives using Eigen's expression
-  * template mechanism.
-  *
-  * It supports the following list of global math function:
-  *  - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos,
-  *  - internal::abs, internal::sqrt, numext::pow, internal::exp, internal::log, internal::sin, internal::cos,
-  *  - internal::conj, internal::real, internal::imag, numext::abs2.
-  *
-  * AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However,
-  * in that case, the expression template mechanism only occurs at the top Matrix level,
-  * while derivatives are computed right away.
-  *
-  */
-
-template<typename DerivativeType>
-class AutoDiffScalar
-  : public internal::auto_diff_special_op
-            <DerivativeType, !internal::is_same<typename internal::traits<typename internal::remove_all<DerivativeType>::type>::Scalar,
-                                          typename NumTraits<typename internal::traits<typename internal::remove_all<DerivativeType>::type>::Scalar>::Real>::value>
-{
-  public:
-    typedef internal::auto_diff_special_op
-            <DerivativeType, !internal::is_same<typename internal::traits<typename internal::remove_all<DerivativeType>::type>::Scalar,
-                       typename NumTraits<typename internal::traits<typename internal::remove_all<DerivativeType>::type>::Scalar>::Real>::value> Base;
-    typedef typename internal::remove_all<DerivativeType>::type DerType;
-    typedef typename internal::traits<DerType>::Scalar Scalar;
-    typedef typename NumTraits<Scalar>::Real Real;
-
-    using Base::operator+;
-    using Base::operator*;
-
-    /** Default constructor without any initialization. */
-    AutoDiffScalar() {}
-
-    /** Constructs an active scalar from its \a value,
-        and initializes the \a nbDer derivatives such that it corresponds to the \a derNumber -th variable */
-    AutoDiffScalar(const Scalar& value, int nbDer, int derNumber)
-      : m_value(value), m_derivatives(DerType::Zero(nbDer))
-    {
-      m_derivatives.coeffRef(derNumber) = Scalar(1);
-    }
-
-    /** Conversion from a scalar constant to an active scalar.
-      * The derivatives are set to zero. */
-    /*explicit*/ AutoDiffScalar(const Real& value)
-      : m_value(value)
-    {
-      if(m_derivatives.size()>0)
-        m_derivatives.setZero();
-    }
-
-    /** Constructs an active scalar from its \a value and derivatives \a der */
-    AutoDiffScalar(const Scalar& value, const DerType& der)
-      : m_value(value), m_derivatives(der)
-    {}
-
-    template<typename OtherDerType>
-    AutoDiffScalar(const AutoDiffScalar<OtherDerType>& other
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-    , typename internal::enable_if<
-            internal::is_same<Scalar, typename internal::traits<typename internal::remove_all<OtherDerType>::type>::Scalar>::value
-        &&  internal::is_convertible<OtherDerType,DerType>::value , void*>::type = 0
-#endif
-    )
-      : m_value(other.value()), m_derivatives(other.derivatives())
-    {}
-
-    friend  std::ostream & operator << (std::ostream & s, const AutoDiffScalar& a)
-    {
-      return s << a.value();
-    }
-
-    AutoDiffScalar(const AutoDiffScalar& other)
-      : m_value(other.value()), m_derivatives(other.derivatives())
-    {}
-
-    template<typename OtherDerType>
-    inline AutoDiffScalar& operator=(const AutoDiffScalar<OtherDerType>& other)
-    {
-      m_value = other.value();
-      m_derivatives = other.derivatives();
-      return *this;
-    }
-
-    inline AutoDiffScalar& operator=(const AutoDiffScalar& other)
-    {
-      m_value = other.value();
-      m_derivatives = other.derivatives();
-      return *this;
-    }
-
-    inline AutoDiffScalar& operator=(const Scalar& other)
-    {
-      m_value = other;
-      if(m_derivatives.size()>0)
-        m_derivatives.setZero();
-      return *this;
-    }
-
-//     inline operator const Scalar& () const { return m_value; }
-//     inline operator Scalar& () { return m_value; }
-
-    inline const Scalar& value() const { return m_value; }
-    inline Scalar& value() { return m_value; }
-
-    inline const DerType& derivatives() const { return m_derivatives; }
-    inline DerType& derivatives() { return m_derivatives; }
-
-    inline bool operator< (const Scalar& other) const  { return m_value <  other; }
-    inline bool operator<=(const Scalar& other) const  { return m_value <= other; }
-    inline bool operator> (const Scalar& other) const  { return m_value >  other; }
-    inline bool operator>=(const Scalar& other) const  { return m_value >= other; }
-    inline bool operator==(const Scalar& other) const  { return m_value == other; }
-    inline bool operator!=(const Scalar& other) const  { return m_value != other; }
-
-    friend inline bool operator< (const Scalar& a, const AutoDiffScalar& b) { return a <  b.value(); }
-    friend inline bool operator<=(const Scalar& a, const AutoDiffScalar& b) { return a <= b.value(); }
-    friend inline bool operator> (const Scalar& a, const AutoDiffScalar& b) { return a >  b.value(); }
-    friend inline bool operator>=(const Scalar& a, const AutoDiffScalar& b) { return a >= b.value(); }
-    friend inline bool operator==(const Scalar& a, const AutoDiffScalar& b) { return a == b.value(); }
-    friend inline bool operator!=(const Scalar& a, const AutoDiffScalar& b) { return a != b.value(); }
-
-    template<typename OtherDerType> inline bool operator< (const AutoDiffScalar<OtherDerType>& b) const  { return m_value <  b.value(); }
-    template<typename OtherDerType> inline bool operator<=(const AutoDiffScalar<OtherDerType>& b) const  { return m_value <= b.value(); }
-    template<typename OtherDerType> inline bool operator> (const AutoDiffScalar<OtherDerType>& b) const  { return m_value >  b.value(); }
-    template<typename OtherDerType> inline bool operator>=(const AutoDiffScalar<OtherDerType>& b) const  { return m_value >= b.value(); }
-    template<typename OtherDerType> inline bool operator==(const AutoDiffScalar<OtherDerType>& b) const  { return m_value == b.value(); }
-    template<typename OtherDerType> inline bool operator!=(const AutoDiffScalar<OtherDerType>& b) const  { return m_value != b.value(); }
-
-    inline const AutoDiffScalar<DerType&> operator+(const Scalar& other) const
-    {
-      return AutoDiffScalar<DerType&>(m_value + other, m_derivatives);
-    }
-
-    friend inline const AutoDiffScalar<DerType&> operator+(const Scalar& a, const AutoDiffScalar& b)
-    {
-      return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
-    }
-
-//     inline const AutoDiffScalar<DerType&> operator+(const Real& other) const
-//     {
-//       return AutoDiffScalar<DerType&>(m_value + other, m_derivatives);
-//     }
-
-//     friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar& b)
-//     {
-//       return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
-//     }
-
-    inline AutoDiffScalar& operator+=(const Scalar& other)
-    {
-      value() += other;
-      return *this;
-    }
-
-    template<typename OtherDerType>
-    inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >
-    operator+(const AutoDiffScalar<OtherDerType>& other) const
-    {
-      internal::make_coherent(m_derivatives, other.derivatives());
-      return AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >(
-        m_value + other.value(),
-        m_derivatives + other.derivatives());
-    }
-
-    template<typename OtherDerType>
-    inline AutoDiffScalar&
-    operator+=(const AutoDiffScalar<OtherDerType>& other)
-    {
-      (*this) = (*this) + other;
-      return *this;
-    }
-
-    inline const AutoDiffScalar<DerType&> operator-(const Scalar& b) const
-    {
-      return AutoDiffScalar<DerType&>(m_value - b, m_derivatives);
-    }
-
-    friend inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
-    operator-(const Scalar& a, const AutoDiffScalar& b)
-    {
-      return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
-            (a - b.value(), -b.derivatives());
-    }
-
-    inline AutoDiffScalar& operator-=(const Scalar& other)
-    {
-      value() -= other;
-      return *this;
-    }
-
-    template<typename OtherDerType>
-    inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >
-    operator-(const AutoDiffScalar<OtherDerType>& other) const
-    {
-      internal::make_coherent(m_derivatives, other.derivatives());
-      return AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >(
-        m_value - other.value(),
-        m_derivatives - other.derivatives());
-    }
-
-    template<typename OtherDerType>
-    inline AutoDiffScalar&
-    operator-=(const AutoDiffScalar<OtherDerType>& other)
-    {
-      *this = *this - other;
-      return *this;
-    }
-
-    inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
-    operator-() const
-    {
-      return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >(
-        -m_value,
-        -m_derivatives);
-    }
-
-    inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
-    operator*(const Scalar& other) const
-    {
-      return MakeAutoDiffScalar(m_value * other, m_derivatives * other);
-    }
-
-    friend inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
-    operator*(const Scalar& other, const AutoDiffScalar& a)
-    {
-      return MakeAutoDiffScalar(a.value() * other, a.derivatives() * other);
-    }
-
-//     inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
-//     operator*(const Real& other) const
-//     {
-//       return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
-//         m_value * other,
-//         (m_derivatives * other));
-//     }
-//
-//     friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
-//     operator*(const Real& other, const AutoDiffScalar& a)
-//     {
-//       return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
-//         a.value() * other,
-//         a.derivatives() * other);
-//     }
-
-    inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
-    operator/(const Scalar& other) const
-    {
-      return MakeAutoDiffScalar(m_value / other, (m_derivatives * (Scalar(1)/other)));
-    }
-
-    friend inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
-    operator/(const Scalar& other, const AutoDiffScalar& a)
-    {
-      return MakeAutoDiffScalar(other / a.value(), a.derivatives() * (Scalar(-other) / (a.value()*a.value())));
-    }
-
-//     inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
-//     operator/(const Real& other) const
-//     {
-//       return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
-//         m_value / other,
-//         (m_derivatives * (Real(1)/other)));
-//     }
-//
-//     friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
-//     operator/(const Real& other, const AutoDiffScalar& a)
-//     {
-//       return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
-//         other / a.value(),
-//         a.derivatives() * (-Real(1)/other));
-//     }
-
-    template<typename OtherDerType>
-    inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(
-        CwiseBinaryOp<internal::scalar_difference_op<Scalar> EIGEN_COMMA
-          const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) EIGEN_COMMA
-          const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<OtherDerType>::type,Scalar,product) >,Scalar,product) >
-    operator/(const AutoDiffScalar<OtherDerType>& other) const
-    {
-      internal::make_coherent(m_derivatives, other.derivatives());
-      return MakeAutoDiffScalar(
-        m_value / other.value(),
-          ((m_derivatives * other.value()) - (other.derivatives() * m_value))
-        * (Scalar(1)/(other.value()*other.value())));
-    }
-
-    template<typename OtherDerType>
-    inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
-        const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product),
-        const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<OtherDerType>::type,Scalar,product) > >
-    operator*(const AutoDiffScalar<OtherDerType>& other) const
-    {
-      internal::make_coherent(m_derivatives, other.derivatives());
-      return MakeAutoDiffScalar(
-        m_value * other.value(),
-        (m_derivatives * other.value()) + (other.derivatives() * m_value));
-    }
-
-    inline AutoDiffScalar& operator*=(const Scalar& other)
-    {
-      *this = *this * other;
-      return *this;
-    }
-
-    template<typename OtherDerType>
-    inline AutoDiffScalar& operator*=(const AutoDiffScalar<OtherDerType>& other)
-    {
-      *this = *this * other;
-      return *this;
-    }
-
-    inline AutoDiffScalar& operator/=(const Scalar& other)
-    {
-      *this = *this / other;
-      return *this;
-    }
-
-    template<typename OtherDerType>
-    inline AutoDiffScalar& operator/=(const AutoDiffScalar<OtherDerType>& other)
-    {
-      *this = *this / other;
-      return *this;
-    }
-
-  protected:
-    Scalar m_value;
-    DerType m_derivatives;
-
-};
-
-namespace internal {
-
-template<typename DerivativeType>
-struct auto_diff_special_op<DerivativeType, true>
-//   : auto_diff_scalar_op<DerivativeType, typename NumTraits<Scalar>::Real,
-//                            is_same<Scalar,typename NumTraits<Scalar>::Real>::value>
-{
-  typedef typename remove_all<DerivativeType>::type DerType;
-  typedef typename traits<DerType>::Scalar Scalar;
-  typedef typename NumTraits<Scalar>::Real Real;
-
-//   typedef auto_diff_scalar_op<DerivativeType, typename NumTraits<Scalar>::Real,
-//                            is_same<Scalar,typename NumTraits<Scalar>::Real>::value> Base;
-
-//   using Base::operator+;
-//   using Base::operator+=;
-//   using Base::operator-;
-//   using Base::operator-=;
-//   using Base::operator*;
-//   using Base::operator*=;
-
-  const AutoDiffScalar<DerivativeType>& derived() const { return *static_cast<const AutoDiffScalar<DerivativeType>*>(this); }
-  AutoDiffScalar<DerivativeType>& derived() { return *static_cast<AutoDiffScalar<DerivativeType>*>(this); }
-
-
-  inline const AutoDiffScalar<DerType&> operator+(const Real& other) const
-  {
-    return AutoDiffScalar<DerType&>(derived().value() + other, derived().derivatives());
-  }
-
-  friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar<DerivativeType>& b)
-  {
-    return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
-  }
-
-  inline AutoDiffScalar<DerivativeType>& operator+=(const Real& other)
-  {
-    derived().value() += other;
-    return derived();
-  }
-
-
-  inline const AutoDiffScalar<typename CwiseUnaryOp<bind2nd_op<scalar_product_op<Scalar,Real> >, DerType>::Type >
-  operator*(const Real& other) const
-  {
-    return AutoDiffScalar<typename CwiseUnaryOp<bind2nd_op<scalar_product_op<Scalar,Real> >, DerType>::Type >(
-      derived().value() * other,
-      derived().derivatives() * other);
-  }
-
-  friend inline const AutoDiffScalar<typename CwiseUnaryOp<bind1st_op<scalar_product_op<Real,Scalar> >, DerType>::Type >
-  operator*(const Real& other, const AutoDiffScalar<DerivativeType>& a)
-  {
-    return AutoDiffScalar<typename CwiseUnaryOp<bind1st_op<scalar_product_op<Real,Scalar> >, DerType>::Type >(
-      a.value() * other,
-      a.derivatives() * other);
-  }
-
-  inline AutoDiffScalar<DerivativeType>& operator*=(const Scalar& other)
-  {
-    *this = *this * other;
-    return derived();
-  }
-};
-
-template<typename DerivativeType>
-struct auto_diff_special_op<DerivativeType, false>
-{
-  void operator*() const;
-  void operator-() const;
-  void operator+() const;
-};
-
-template<typename BinOp, typename A, typename B, typename RefType>
-void make_coherent_expression(CwiseBinaryOp<BinOp,A,B> xpr, const RefType &ref)
-{
-  make_coherent(xpr.const_cast_derived().lhs(), ref);
-  make_coherent(xpr.const_cast_derived().rhs(), ref);
-}
-
-template<typename UnaryOp, typename A, typename RefType>
-void make_coherent_expression(const CwiseUnaryOp<UnaryOp,A> &xpr, const RefType &ref)
-{
-  make_coherent(xpr.nestedExpression().const_cast_derived(), ref);
-}
-
-// needed for compilation only
-template<typename UnaryOp, typename A, typename RefType>
-void make_coherent_expression(const CwiseNullaryOp<UnaryOp,A> &, const RefType &)
-{}
-
-template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols, typename B>
-struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>, B> {
-  typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A;
-  static void run(A& a, B& b) {
-    if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0))
-    {
-      a.resize(b.size());
-      a.setZero();
-    }
-    else if (B::SizeAtCompileTime==Dynamic && a.size()!=0 && b.size()==0)
-    {
-      make_coherent_expression(b,a);
-    }
-  }
-};
-
-template<typename A, typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols>
-struct make_coherent_impl<A, Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > {
-  typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B;
-  static void run(A& a, B& b) {
-    if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0))
-    {
-      b.resize(a.size());
-      b.setZero();
-    }
-    else if (A::SizeAtCompileTime==Dynamic && b.size()!=0 && a.size()==0)
-    {
-      make_coherent_expression(a,b);
-    }
-  }
-};
-
-template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols,
-         typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols>
-struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>,
-                          Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > {
-  typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A;
-  typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B;
-  static void run(A& a, B& b) {
-    if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0))
-    {
-      a.resize(b.size());
-      a.setZero();
-    }
-    else if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0))
-    {
-      b.resize(a.size());
-      b.setZero();
-    }
-  }
-};
-
-} // end namespace internal
-
-template<typename DerType, typename BinOp>
-struct ScalarBinaryOpTraits<AutoDiffScalar<DerType>,typename DerType::Scalar,BinOp>
-{
-  typedef AutoDiffScalar<DerType> ReturnType;
-};
-
-template<typename DerType, typename BinOp>
-struct ScalarBinaryOpTraits<typename DerType::Scalar,AutoDiffScalar<DerType>, BinOp>
-{
-  typedef AutoDiffScalar<DerType> ReturnType;
-};
-
-
-// The following is an attempt to let Eigen's known about expression template, but that's more tricky!
-
-// template<typename DerType, typename BinOp>
-// struct ScalarBinaryOpTraits<AutoDiffScalar<DerType>,AutoDiffScalar<DerType>, BinOp>
-// {
-//   enum { Defined = 1 };
-//   typedef AutoDiffScalar<typename DerType::PlainObject> ReturnType;
-// };
-//
-// template<typename DerType1,typename DerType2, typename BinOp>
-// struct ScalarBinaryOpTraits<AutoDiffScalar<DerType1>,AutoDiffScalar<DerType2>, BinOp>
-// {
-//   enum { Defined = 1 };//internal::is_same<typename DerType1::Scalar,typename DerType2::Scalar>::value };
-//   typedef AutoDiffScalar<typename DerType1::PlainObject> ReturnType;
-// };
-
-#define EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(FUNC,CODE) \
-  template<typename DerType> \
-  inline const Eigen::AutoDiffScalar< \
-  EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename Eigen::internal::remove_all<DerType>::type, typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar, product) > \
-  FUNC(const Eigen::AutoDiffScalar<DerType>& x) { \
-    using namespace Eigen; \
-    typedef typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar Scalar; \
-    EIGEN_UNUSED_VARIABLE(sizeof(Scalar)); \
-    CODE; \
-  }
-
-template<typename DerType>
-struct CleanedUpDerType {
-  typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> type;
-};
-
-template<typename DerType>
-inline const AutoDiffScalar<DerType>& conj(const AutoDiffScalar<DerType>& x)  { return x; }
-template<typename DerType>
-inline const AutoDiffScalar<DerType>& real(const AutoDiffScalar<DerType>& x)  { return x; }
-template<typename DerType>
-inline typename DerType::Scalar imag(const AutoDiffScalar<DerType>&)    { return 0.; }
-template<typename DerType, typename T>
-inline typename CleanedUpDerType<DerType>::type (min)(const AutoDiffScalar<DerType>& x, const T& y) {
-  typedef typename CleanedUpDerType<DerType>::type ADS;
-  return (x <= y ? ADS(x) : ADS(y));
-}
-template<typename DerType, typename T>
-inline typename CleanedUpDerType<DerType>::type (max)(const AutoDiffScalar<DerType>& x, const T& y) {
-  typedef typename CleanedUpDerType<DerType>::type ADS;
-  return (x >= y ? ADS(x) : ADS(y));
-}
-template<typename DerType, typename T>
-inline typename CleanedUpDerType<DerType>::type (min)(const T& x, const AutoDiffScalar<DerType>& y) {
-  typedef typename CleanedUpDerType<DerType>::type ADS;
-  return (x < y ? ADS(x) : ADS(y));
-}
-template<typename DerType, typename T>
-inline typename CleanedUpDerType<DerType>::type (max)(const T& x, const AutoDiffScalar<DerType>& y) {
-  typedef typename CleanedUpDerType<DerType>::type ADS;
-  return (x > y ? ADS(x) : ADS(y));
-}
-template<typename DerType>
-inline typename CleanedUpDerType<DerType>::type (min)(const AutoDiffScalar<DerType>& x, const AutoDiffScalar<DerType>& y) {
-  return (x.value() < y.value() ? x : y);
-}
-template<typename DerType>
-inline typename CleanedUpDerType<DerType>::type (max)(const AutoDiffScalar<DerType>& x, const AutoDiffScalar<DerType>& y) {
-  return (x.value() >= y.value() ? x : y);
-}
-
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs,
-  using std::abs;
-  return Eigen::MakeAutoDiffScalar(abs(x.value()), x.derivatives() * (x.value()<0 ? -1 : 1) );)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs2,
-  using numext::abs2;
-  return Eigen::MakeAutoDiffScalar(abs2(x.value()), x.derivatives() * (Scalar(2)*x.value()));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sqrt,
-  using std::sqrt;
-  Scalar sqrtx = sqrt(x.value());
-  return Eigen::MakeAutoDiffScalar(sqrtx,x.derivatives() * (Scalar(0.5) / sqrtx));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cos,
-  using std::cos;
-  using std::sin;
-  return Eigen::MakeAutoDiffScalar(cos(x.value()), x.derivatives() * (-sin(x.value())));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sin,
-  using std::sin;
-  using std::cos;
-  return Eigen::MakeAutoDiffScalar(sin(x.value()),x.derivatives() * cos(x.value()));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(exp,
-  using std::exp;
-  Scalar expx = exp(x.value());
-  return Eigen::MakeAutoDiffScalar(expx,x.derivatives() * expx);)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(log,
-  using std::log;
-  return Eigen::MakeAutoDiffScalar(log(x.value()),x.derivatives() * (Scalar(1)/x.value()));)
-
-template<typename DerType>
-inline const Eigen::AutoDiffScalar<
-EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<DerType>::type,typename internal::traits<typename internal::remove_all<DerType>::type>::Scalar,product) >
-pow(const Eigen::AutoDiffScalar<DerType> &x, const typename internal::traits<typename internal::remove_all<DerType>::type>::Scalar &y)
-{
-  using namespace Eigen;
-  using std::pow;
-  return Eigen::MakeAutoDiffScalar(pow(x.value(),y), x.derivatives() * (y * pow(x.value(),y-1)));
-}
-
-
-template<typename DerTypeA,typename DerTypeB>
-inline const AutoDiffScalar<Matrix<typename internal::traits<typename internal::remove_all<DerTypeA>::type>::Scalar,Dynamic,1> >
-atan2(const AutoDiffScalar<DerTypeA>& a, const AutoDiffScalar<DerTypeB>& b)
-{
-  using std::atan2;
-  typedef typename internal::traits<typename internal::remove_all<DerTypeA>::type>::Scalar Scalar;
-  typedef AutoDiffScalar<Matrix<Scalar,Dynamic,1> > PlainADS;
-  PlainADS ret;
-  ret.value() = atan2(a.value(), b.value());
-  
-  Scalar squared_hypot = a.value() * a.value() + b.value() * b.value();
-  
-  // if (squared_hypot==0) the derivation is undefined and the following results in a NaN:
-  ret.derivatives() = (a.derivatives() * b.value() - a.value() * b.derivatives()) / squared_hypot;
-
-  return ret;
-}
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(tan,
-  using std::tan;
-  using std::cos;
-  return Eigen::MakeAutoDiffScalar(tan(x.value()),x.derivatives() * (Scalar(1)/numext::abs2(cos(x.value()))));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(asin,
-  using std::sqrt;
-  using std::asin;
-  return Eigen::MakeAutoDiffScalar(asin(x.value()),x.derivatives() * (Scalar(1)/sqrt(1-numext::abs2(x.value()))));)
-  
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(acos,
-  using std::sqrt;
-  using std::acos;
-  return Eigen::MakeAutoDiffScalar(acos(x.value()),x.derivatives() * (Scalar(-1)/sqrt(1-numext::abs2(x.value()))));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(tanh,
-  using std::cosh;
-  using std::tanh;
-  return Eigen::MakeAutoDiffScalar(tanh(x.value()),x.derivatives() * (Scalar(1)/numext::abs2(cosh(x.value()))));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sinh,
-  using std::sinh;
-  using std::cosh;
-  return Eigen::MakeAutoDiffScalar(sinh(x.value()),x.derivatives() * cosh(x.value()));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cosh,
-  using std::sinh;
-  using std::cosh;
-  return Eigen::MakeAutoDiffScalar(cosh(x.value()),x.derivatives() * sinh(x.value()));)
-
-#undef EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY
-
-template<typename DerType> struct NumTraits<AutoDiffScalar<DerType> >
-  : NumTraits< typename NumTraits<typename internal::remove_all<DerType>::type::Scalar>::Real >
-{
-  typedef typename internal::remove_all<DerType>::type DerTypeCleaned;
-  typedef AutoDiffScalar<Matrix<typename NumTraits<typename DerTypeCleaned::Scalar>::Real,DerTypeCleaned::RowsAtCompileTime,DerTypeCleaned::ColsAtCompileTime,
-                                0, DerTypeCleaned::MaxRowsAtCompileTime, DerTypeCleaned::MaxColsAtCompileTime> > Real;
-  typedef AutoDiffScalar<DerType> NonInteger;
-  typedef AutoDiffScalar<DerType> Nested;
-  typedef typename NumTraits<typename DerTypeCleaned::Scalar>::Literal Literal;
-  enum{
-    RequireInitialization = 1
-  };
-};
-
-}
-
-namespace std {
-
-template <typename T>
-class numeric_limits<Eigen::AutoDiffScalar<T> >
-  : public numeric_limits<typename T::Scalar> {};
-
-template <typename T>
-class numeric_limits<Eigen::AutoDiffScalar<T&> >
-  : public numeric_limits<typename T::Scalar> {};
-
-}  // namespace std
-
-#endif // EIGEN_AUTODIFF_SCALAR_H
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h b/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h
deleted file mode 100644
index 8c2d048..0000000
--- a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h
+++ /dev/null
@@ -1,220 +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>
-//
-// 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_AUTODIFF_VECTOR_H
-#define EIGEN_AUTODIFF_VECTOR_H
-
-namespace Eigen {
-
-/* \class AutoDiffScalar
-  * \brief A scalar type replacement with automatic differentation capability
-  *
-  * \param DerType the vector type used to store/represent the derivatives (e.g. Vector3f)
-  *
-  * This class represents a scalar value while tracking its respective derivatives.
-  *
-  * It supports the following list of global math function:
-  *  - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos,
-  *  - internal::abs, internal::sqrt, numext::pow, internal::exp, internal::log, internal::sin, internal::cos,
-  *  - internal::conj, internal::real, internal::imag, numext::abs2.
-  *
-  * AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However,
-  * in that case, the expression template mechanism only occurs at the top Matrix level,
-  * while derivatives are computed right away.
-  *
-  */
-template<typename ValueType, typename JacobianType>
-class AutoDiffVector
-{
-  public:
-    //typedef typename internal::traits<ValueType>::Scalar Scalar;
-    typedef typename internal::traits<ValueType>::Scalar BaseScalar;
-    typedef AutoDiffScalar<Matrix<BaseScalar,JacobianType::RowsAtCompileTime,1> > ActiveScalar;
-    typedef ActiveScalar Scalar;
-    typedef AutoDiffScalar<typename JacobianType::ColXpr> CoeffType;
-    typedef typename JacobianType::Index Index;
-
-    inline AutoDiffVector() {}
-
-    inline AutoDiffVector(const ValueType& values)
-      : m_values(values)
-    {
-      m_jacobian.setZero();
-    }
-
-
-    CoeffType operator[] (Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
-    const CoeffType operator[] (Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
-
-    CoeffType operator() (Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
-    const CoeffType operator() (Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
-
-    CoeffType coeffRef(Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
-    const CoeffType coeffRef(Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
-
-    Index size() const { return m_values.size(); }
-
-    // FIXME here we could return an expression of the sum
-    Scalar sum() const { /*std::cerr << "sum \n\n";*/ /*std::cerr << m_jacobian.rowwise().sum() << "\n\n";*/ return Scalar(m_values.sum(), m_jacobian.rowwise().sum()); }
-
-
-    inline AutoDiffVector(const ValueType& values, const JacobianType& jac)
-      : m_values(values), m_jacobian(jac)
-    {}
-
-    template<typename OtherValueType, typename OtherJacobianType>
-    inline AutoDiffVector(const AutoDiffVector<OtherValueType, OtherJacobianType>& other)
-      : m_values(other.values()), m_jacobian(other.jacobian())
-    {}
-
-    inline AutoDiffVector(const AutoDiffVector& other)
-      : m_values(other.values()), m_jacobian(other.jacobian())
-    {}
-
-    template<typename OtherValueType, typename OtherJacobianType>
-    inline AutoDiffVector& operator=(const AutoDiffVector<OtherValueType, OtherJacobianType>& other)
-    {
-      m_values = other.values();
-      m_jacobian = other.jacobian();
-      return *this;
-    }
-
-    inline AutoDiffVector& operator=(const AutoDiffVector& other)
-    {
-      m_values = other.values();
-      m_jacobian = other.jacobian();
-      return *this;
-    }
-
-    inline const ValueType& values() const { return m_values; }
-    inline ValueType& values() { return m_values; }
-
-    inline const JacobianType& jacobian() const { return m_jacobian; }
-    inline JacobianType& jacobian() { return m_jacobian; }
-
-    template<typename OtherValueType,typename OtherJacobianType>
-    inline const AutoDiffVector<
-      typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type,
-      typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type >
-    operator+(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
-    {
-      return AutoDiffVector<
-      typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type,
-      typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type >(
-        m_values + other.values(),
-        m_jacobian + other.jacobian());
-    }
-
-    template<typename OtherValueType, typename OtherJacobianType>
-    inline AutoDiffVector&
-    operator+=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
-    {
-      m_values += other.values();
-      m_jacobian += other.jacobian();
-      return *this;
-    }
-
-    template<typename OtherValueType,typename OtherJacobianType>
-    inline const AutoDiffVector<
-      typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type,
-      typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type >
-    operator-(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
-    {
-      return AutoDiffVector<
-        typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type,
-        typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type >(
-          m_values - other.values(),
-          m_jacobian - other.jacobian());
-    }
-
-    template<typename OtherValueType, typename OtherJacobianType>
-    inline AutoDiffVector&
-    operator-=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
-    {
-      m_values -= other.values();
-      m_jacobian -= other.jacobian();
-      return *this;
-    }
-
-    inline const AutoDiffVector<
-      typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, ValueType>::Type,
-      typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, JacobianType>::Type >
-    operator-() const
-    {
-      return AutoDiffVector<
-        typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, ValueType>::Type,
-        typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, JacobianType>::Type >(
-          -m_values,
-          -m_jacobian);
-    }
-
-    inline const AutoDiffVector<
-      typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
-      typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type>
-    operator*(const BaseScalar& other) const
-    {
-      return AutoDiffVector<
-        typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
-        typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >(
-          m_values * other,
-          m_jacobian * other);
-    }
-
-    friend inline const AutoDiffVector<
-      typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
-      typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >
-    operator*(const Scalar& other, const AutoDiffVector& v)
-    {
-      return AutoDiffVector<
-        typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
-        typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >(
-          v.values() * other,
-          v.jacobian() * other);
-    }
-
-//     template<typename OtherValueType,typename OtherJacobianType>
-//     inline const AutoDiffVector<
-//       CwiseBinaryOp<internal::scalar_multiple_op<Scalar>, ValueType, OtherValueType>
-//       CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
-//         CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>,
-//         CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, OtherJacobianType> > >
-//     operator*(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
-//     {
-//       return AutoDiffVector<
-//         CwiseBinaryOp<internal::scalar_multiple_op<Scalar>, ValueType, OtherValueType>
-//         CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
-//           CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>,
-//           CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, OtherJacobianType> > >(
-//             m_values.cwise() * other.values(),
-//             (m_jacobian * other.values()) + (m_values * other.jacobian()));
-//     }
-
-    inline AutoDiffVector& operator*=(const Scalar& other)
-    {
-      m_values *= other;
-      m_jacobian *= other;
-      return *this;
-    }
-
-    template<typename OtherValueType,typename OtherJacobianType>
-    inline AutoDiffVector& operator*=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
-    {
-      *this = *this * other;
-      return *this;
-    }
-
-  protected:
-    ValueType m_values;
-    JacobianType m_jacobian;
-
-};
-
-}
-
-#endif // EIGEN_AUTODIFF_VECTOR_H
diff --git a/wpimath/src/main/native/include/frc/ComputerVisionUtil.h b/wpimath/src/main/native/include/frc/ComputerVisionUtil.h
new file mode 100644
index 0000000..54f9752
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/ComputerVisionUtil.h
@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Transform3d.h"
+
+namespace frc {
+
+/**
+ * Returns the robot's pose in the field coordinate system given an object's
+ * field-relative pose, the transformation from the camera's pose to the
+ * object's pose (obtained via computer vision), and the transformation from the
+ * robot's pose to the camera's pose.
+ *
+ * The object could be a target or a fiducial marker.
+ *
+ * @param objectInField An object's field-relative pose.
+ * @param cameraToObject The transformation from the camera's pose to the
+ *   object's pose. This comes from computer vision.
+ * @param robotToCamera The transformation from the robot's pose to the camera's
+ *   pose. This can either be a constant for a rigidly mounted camera, or
+ *   variable if the camera is mounted to a turret. If the camera was mounted 3
+ *   inches in front of the "origin" (usually physical center) of the robot,
+ *   this would be frc::Transform3d{3_in, 0_in, 0_in, frc::Rotation3d{}}.
+ * @return The robot's field-relative pose.
+ */
+WPILIB_DLLEXPORT
+frc::Pose3d ObjectToRobotPose(const frc::Pose3d& objectInField,
+                              const frc::Transform3d& cameraToObject,
+                              const frc::Transform3d& robotToCamera);
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/EigenCore.h b/wpimath/src/main/native/include/frc/EigenCore.h
new file mode 100644
index 0000000..b33e9e2
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/EigenCore.h
@@ -0,0 +1,23 @@
+// 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"
+
+namespace frc {
+
+template <int Size>
+using Vectord = Eigen::Vector<double, Size>;
+
+template <int Rows, int Cols,
+          int Options = Eigen::AutoAlign |
+                        ((Rows == 1 && Cols != 1) ? Eigen::RowMajor
+                         : (Cols == 1 && Rows != 1)
+                             ? Eigen::ColMajor
+                             : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION),
+          int MaxRows = Rows, int MaxCols = Cols>
+using Matrixd = Eigen::Matrix<double, Rows, Cols, Options, MaxRows, MaxCols>;
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h
index 54a77af..24bf857 100644
--- a/wpimath/src/main/native/include/frc/MathUtil.h
+++ b/wpimath/src/main/native/include/frc/MathUtil.h
@@ -4,22 +4,84 @@
 
 #pragma once
 
+#include <numbers>
+
 #include <wpi/SymbolExports.h>
-#include <wpi/numbers>
 
 #include "units/angle.h"
+#include "units/base.h"
+#include "units/math.h"
 
 namespace frc {
 
 /**
- * Returns 0.0 if the given value is within the specified range around zero.
- * The remaining range between the deadband and 1.0 is scaled from 0.0 to 1.0.
+ * Returns 0.0 if the given value is within the specified range around zero. The
+ * remaining range between the deadband and the maximum magnitude is scaled from
+ * 0.0 to the maximum magnitude.
  *
- * @param value    Value to clip.
+ * @param value Value to clip.
  * @param deadband Range around zero.
+ * @param maxMagnitude The maximum magnitude of the input (defaults to 1). Can
+ * be infinite.
+ * @return The value after the deadband is applied.
  */
-WPILIB_DLLEXPORT
-double ApplyDeadband(double value, double deadband);
+template <typename T,
+          typename = std::enable_if_t<std::disjunction_v<
+              std::is_floating_point<T>, units::traits::is_unit_t<T>>>>
+T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
+  T magnitude;
+  if constexpr (std::is_floating_point_v<T>) {
+    magnitude = std::abs(value);
+  } else {
+    magnitude = units::math::abs(value);
+  }
+
+  if (magnitude > deadband) {
+    if (maxMagnitude / deadband > 1.0E12) {
+      // If max magnitude is sufficiently large, the implementation encounters
+      // roundoff error.  Implementing the limiting behavior directly avoids
+      // the problem.
+      return value > T{0.0} ? value - deadband : value + deadband;
+    }
+    if (value > T{0.0}) {
+      // Map deadband to 0 and map max to max.
+      //
+      // y - y₁ = m(x - x₁)
+      // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
+      // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
+      //
+      // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max).
+      // x₁ = deadband
+      // y₁ = 0
+      // x₂ = max
+      // y₂ = max
+      //
+      // y = (max - 0)/(max - deadband) (x - deadband) + 0
+      // y = max/(max - deadband) (x - deadband)
+      // y = max (x - deadband)/(max - deadband)
+      return maxMagnitude * (value - deadband) / (maxMagnitude - deadband);
+    } else {
+      // Map -deadband to 0 and map -max to -max.
+      //
+      // y - y₁ = m(x - x₁)
+      // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
+      // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
+      //
+      // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
+      // x₁ = -deadband
+      // y₁ = 0
+      // x₂ = -max
+      // y₂ = -max
+      //
+      // y = (-max - 0)/(-max + deadband) (x + deadband) + 0
+      // y = max/(max - deadband) (x + deadband)
+      // y = max (x + deadband)/(max - deadband)
+      return maxMagnitude * (value + deadband) / (maxMagnitude - deadband);
+    }
+  } else {
+    return T{0.0};
+  }
+}
 
 /**
  * Returns modulus of input.
@@ -51,8 +113,8 @@
 WPILIB_DLLEXPORT
 constexpr units::radian_t AngleModulus(units::radian_t angle) {
   return InputModulus<units::radian_t>(angle,
-                                       units::radian_t{-wpi::numbers::pi},
-                                       units::radian_t{wpi::numbers::pi});
+                                       units::radian_t{-std::numbers::pi},
+                                       units::radian_t{std::numbers::pi});
 }
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
index 5fdd18c..0345b46 100644
--- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h
+++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
@@ -6,15 +6,16 @@
 
 #include <array>
 #include <cmath>
+#include <limits>
 #include <random>
 #include <type_traits>
 
 #include <wpi/SymbolExports.h>
 #include <wpi/deprecated.h>
 
-#include "Eigen/Core"
 #include "Eigen/Eigenvalues"
 #include "Eigen/QR"
+#include "frc/EigenCore.h"
 #include "frc/geometry/Pose2d.h"
 
 namespace frc {
@@ -32,7 +33,11 @@
 
 template <typename Matrix, typename T, typename... Ts>
 void CostMatrixImpl(Matrix& result, T elem, Ts... elems) {
-  result(result.rows() - (sizeof...(Ts) + 1)) = 1.0 / std::pow(elem, 2);
+  if (elem == std::numeric_limits<double>::infinity()) {
+    result(result.rows() - (sizeof...(Ts) + 1)) = 0.0;
+  } else {
+    result(result.rows() - (sizeof...(Ts) + 1)) = 1.0 / std::pow(elem, 2);
+  }
   if constexpr (sizeof...(Ts) > 0) {
     CostMatrixImpl(result, elems...);
   }
@@ -59,9 +64,9 @@
 }
 
 template <int States, int Inputs>
-bool IsStabilizableImpl(const Eigen::Matrix<double, States, States>& A,
-                        const Eigen::Matrix<double, States, Inputs>& B) {
-  Eigen::EigenSolver<Eigen::Matrix<double, States, States>> es{A};
+bool IsStabilizableImpl(const Matrixd<States, States>& A,
+                        const Matrixd<States, Inputs>& B) {
+  Eigen::EigenSolver<Matrixd<States, States>> es{A};
 
   for (int i = 0; i < States; ++i) {
     if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
@@ -89,47 +94,23 @@
 }  // namespace detail
 
 /**
- * Creates a matrix from the given list of elements.
- *
- * The elements of the matrix are filled in in row-major order.
- *
- * @param elems An array of elements in the matrix.
- * @return A matrix containing the given elements.
- * @deprecated Use Eigen::Matrix or Eigen::Vector initializer list constructor.
- */
-template <int Rows, int Cols, typename... Ts,
-          typename =
-              std::enable_if_t<std::conjunction_v<std::is_same<double, Ts>...>>>
-WPI_DEPRECATED(
-    "Use Eigen::Matrix or Eigen::Vector initializer list constructor")
-Eigen::Matrix<double, Rows, Cols> MakeMatrix(Ts... elems) {
-  static_assert(
-      sizeof...(elems) == Rows * Cols,
-      "Number of provided elements doesn't match matrix dimensionality");
-
-  Eigen::Matrix<double, Rows, Cols> result;
-  detail::MatrixImpl<Rows, Cols>(result, elems...);
-  return result;
-}
-
-/**
  * 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 tolerance 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,
- *              its elements are the maximum allowed excursions of the control
- *              inputs from no actuation.
+ * @param tolerances An array. For a Q matrix, its elements are the maximum
+ *                   allowed excursions of the states from the reference. For an
+ *                   R matrix, its elements are the maximum allowed excursions
+ *                   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>...>>>
-Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(
-    Ts... costs) {
+Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances) {
   Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
-  detail::CostMatrixImpl(result.diagonal(), costs...);
+  detail::CostMatrixImpl(result.diagonal(), tolerances...);
   return result;
 }
 
@@ -147,8 +128,7 @@
  */
 template <typename... Ts, typename = std::enable_if_t<
                               std::conjunction_v<std::is_same<double, Ts>...>>>
-Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(
-    Ts... stdDevs) {
+Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
   Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
   detail::CovMatrixImpl(result.diagonal(), stdDevs...);
   return result;
@@ -167,7 +147,7 @@
  * @return State excursion or control effort cost matrix.
  */
 template <size_t N>
-Eigen::Matrix<double, N, N> MakeCostMatrix(const std::array<double, N>& costs) {
+Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& costs) {
   Eigen::DiagonalMatrix<double, N> result;
   auto& diag = result.diagonal();
   for (size_t i = 0; i < N; ++i) {
@@ -189,8 +169,7 @@
  * @return Process noise or measurement noise covariance matrix.
  */
 template <size_t N>
-Eigen::Matrix<double, N, N> MakeCovMatrix(
-    const std::array<double, N>& stdDevs) {
+Matrixd<N, N> MakeCovMatrix(const std::array<double, N>& stdDevs) {
   Eigen::DiagonalMatrix<double, N> result;
   auto& diag = result.diagonal();
   for (size_t i = 0; i < N; ++i) {
@@ -201,8 +180,8 @@
 
 template <typename... Ts, typename = std::enable_if_t<
                               std::conjunction_v<std::is_same<double, Ts>...>>>
-Eigen::Matrix<double, sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
-  Eigen::Matrix<double, sizeof...(Ts), 1> result;
+Matrixd<sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
+  Matrixd<sizeof...(Ts), 1> result;
   detail::WhiteNoiseVectorImpl(result, stdDevs...);
   return result;
 }
@@ -216,12 +195,11 @@
  * @return White noise vector.
  */
 template <int N>
-Eigen::Vector<double, N> MakeWhiteNoiseVector(
-    const std::array<double, N>& stdDevs) {
+Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
   std::random_device rd;
   std::mt19937 gen{rd()};
 
-  Eigen::Vector<double, N> result;
+  Vectord<N> result;
   for (int i = 0; i < N; ++i) {
     // Passing a standard deviation of 0.0 to std::normal_distribution is
     // undefined behavior
@@ -243,7 +221,7 @@
  * @return The vector.
  */
 WPILIB_DLLEXPORT
-Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose);
+Vectord<3> PoseTo3dVector(const Pose2d& pose);
 
 /**
  * Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
@@ -253,7 +231,7 @@
  * @return The vector.
  */
 WPILIB_DLLEXPORT
-Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose);
+Vectord<4> PoseTo4dVector(const Pose2d& pose);
 
 /**
  * Returns true if (A, B) is a stabilizable pair.
@@ -268,8 +246,8 @@
  * @param B Input matrix.
  */
 template <int States, int Inputs>
-bool IsStabilizable(const Eigen::Matrix<double, States, States>& A,
-                    const Eigen::Matrix<double, States, Inputs>& B) {
+bool IsStabilizable(const Matrixd<States, States>& A,
+                    const Matrixd<States, Inputs>& B) {
   return detail::IsStabilizableImpl<States, Inputs>(A, B);
 }
 
@@ -286,8 +264,8 @@
  * @param C Output matrix.
  */
 template <int States, int Outputs>
-bool IsDetectable(const Eigen::Matrix<double, States, States>& A,
-                  const Eigen::Matrix<double, Outputs, States>& C) {
+bool IsDetectable(const Matrixd<States, States>& A,
+                  const Matrixd<Outputs, States>& C) {
   return detail::IsStabilizableImpl<States, Outputs>(A.transpose(),
                                                      C.transpose());
 }
@@ -295,14 +273,14 @@
 // Template specializations are used here to make common state-input pairs
 // compile faster.
 template <>
-WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(
-    const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B);
+WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A,
+                                           const Matrixd<1, 1>& B);
 
 // Template specializations are used here to make common state-input pairs
 // compile faster.
 template <>
-WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(
-    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B);
+WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
+                                           const Matrixd<2, 1>& B);
 
 /**
  * Converts a Pose2d into a vector of [x, y, theta].
@@ -312,7 +290,7 @@
  * @return The vector.
  */
 WPILIB_DLLEXPORT
-Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose);
+Vectord<3> PoseToVector(const Pose2d& pose);
 
 /**
  * Clamps input vector between system's minimum and maximum allowable input.
@@ -324,11 +302,10 @@
  * @return Clamped input vector.
  */
 template <int Inputs>
-Eigen::Vector<double, Inputs> ClampInputMaxMagnitude(
-    const Eigen::Vector<double, Inputs>& u,
-    const Eigen::Vector<double, Inputs>& umin,
-    const Eigen::Vector<double, Inputs>& umax) {
-  Eigen::Vector<double, Inputs> result;
+Vectord<Inputs> ClampInputMaxMagnitude(const Vectord<Inputs>& u,
+                                       const Vectord<Inputs>& umin,
+                                       const Vectord<Inputs>& umax) {
+  Vectord<Inputs> result;
   for (int i = 0; i < Inputs; ++i) {
     result(i) = std::clamp(u(i), umin(i), umax(i));
   }
@@ -345,8 +322,8 @@
  * @return The normalizedInput
  */
 template <int Inputs>
-Eigen::Vector<double, Inputs> DesaturateInputVector(
-    const Eigen::Vector<double, Inputs>& u, double maxMagnitude) {
+Vectord<Inputs> DesaturateInputVector(const Vectord<Inputs>& u,
+                                      double maxMagnitude) {
   double maxValue = u.template lpNorm<Eigen::Infinity>();
 
   if (maxValue > maxMagnitude) {
diff --git a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
index 1f0e407..5621803 100644
--- a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
@@ -37,7 +37,7 @@
    * @param kS   The static gain, in volts.
    * @param kG The gravity gain, in volts.
    * @param kV   The velocity gain, in volt seconds per radian.
-   * @param kA   The acceleration gain, in volt seconds^2 per radian.
+   * @param kA   The acceleration gain, in volt seconds² per radian.
    */
   constexpr ArmFeedforward(
       units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
@@ -47,9 +47,13 @@
   /**
    * Calculates the feedforward from the gains and setpoints.
    *
-   * @param angle        The angle setpoint, in radians.
+   * @param angle        The angle setpoint, in radians. This angle should be
+   *                     measured from the horizontal (i.e. if the provided
+   *                     angle is 0, the arm should be parallel to the floor).
+   *                     If your encoder does not follow this convention, an
+   *                     offset should be added.
    * @param velocity     The velocity setpoint, in radians per second.
-   * @param acceleration The acceleration setpoint, in radians per second^2.
+   * @param acceleration The acceleration setpoint, in radians per second².
    * @return The computed feedforward, in volts.
    */
   units::volt_t Calculate(units::unit_t<Angle> angle,
@@ -70,8 +74,12 @@
    * achievable - enter the acceleration constraint, and this will give you
    * a simultaneously-achievable velocity constraint.
    *
-   * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm
+   * @param maxVoltage   The maximum voltage that can be supplied to the arm.
+   * @param angle        The angle of the arm. This angle should be measured
+   *                     from the horizontal (i.e. if the provided angle is 0,
+   *                     the arm should be parallel to the floor). If your
+   *                     encoder does not follow this convention, an offset
+   *                     should be added.
    * @param acceleration The acceleration of the arm.
    * @return The maximum possible velocity at the given acceleration and angle.
    */
@@ -91,8 +99,12 @@
    * achievable - enter the acceleration constraint, and this will give you
    * a simultaneously-achievable velocity constraint.
    *
-   * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm
+   * @param maxVoltage   The maximum voltage that can be supplied to the arm.
+   * @param angle        The angle of the arm. This angle should be measured
+   *                     from the horizontal (i.e. if the provided angle is 0,
+   *                     the arm should be parallel to the floor). If your
+   *                     encoder does not follow this convention, an offset
+   *                     should be added.
    * @param acceleration The acceleration of the arm.
    * @return The minimum possible velocity at the given acceleration and angle.
    */
@@ -113,8 +125,12 @@
    * a simultaneously-achievable acceleration constraint.
    *
    * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm
-   * @param velocity The velocity of the arm.
+   * @param angle      The angle of the arm. This angle should be measured
+   *                   from the horizontal (i.e. if the provided angle is 0,
+   *                   the arm should be parallel to the floor). If your
+   *                   encoder does not follow this convention, an offset
+   *                   should be added.
+   * @param velocity   The velocity of the arm.
    * @return The maximum possible acceleration at the given velocity and angle.
    */
   units::unit_t<Acceleration> MaxAchievableAcceleration(
@@ -133,8 +149,12 @@
    * a simultaneously-achievable acceleration constraint.
    *
    * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm
-   * @param velocity The velocity of the arm.
+   * @param angle      The angle of the arm. This angle should be measured
+   *                   from the horizontal (i.e. if the provided angle is 0,
+   *                   the arm should be parallel to the floor). If your
+   *                   encoder does not follow this convention, an offset
+   *                   should be added.
+   * @param velocity   The velocity of the arm.
    * @return The minimum possible acceleration at the given velocity and angle.
    */
   units::unit_t<Acceleration> MinAchievableAcceleration(
diff --git a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
index 656f767..21aad25 100644
--- a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
@@ -7,8 +7,8 @@
 #include <array>
 #include <functional>
 
-#include "Eigen/Core"
 #include "Eigen/QR"
+#include "frc/EigenCore.h"
 #include "frc/system/NumericalJacobian.h"
 #include "units/time.h"
 
@@ -39,6 +39,9 @@
 template <int States, int Inputs>
 class ControlAffinePlantInversionFeedforward {
  public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+
   /**
    * Constructs a feedforward with given model dynamics as a function
    * of state and input.
@@ -50,15 +53,11 @@
    * @param dt The timestep between calls of calculate().
    */
   ControlAffinePlantInversionFeedforward(
-      std::function<
-          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                        const Eigen::Vector<double, Inputs>&)>
-          f,
+      std::function<StateVector(const StateVector&, const InputVector&)> f,
       units::second_t dt)
       : m_dt(dt), m_f(f) {
-    m_B = NumericalJacobianU<States, States, Inputs>(
-        f, Eigen::Vector<double, States>::Zero(),
-        Eigen::Vector<double, Inputs>::Zero());
+    m_B = NumericalJacobianU<States, States, Inputs>(f, StateVector::Zero(),
+                                                     InputVector::Zero());
 
     Reset();
   }
@@ -73,14 +72,12 @@
    * @param dt The timestep between calls of calculate().
    */
   ControlAffinePlantInversionFeedforward(
-      std::function<
-          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&)>
-          f,
-      const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
+      std::function<StateVector(const StateVector&)> f,
+      const Matrixd<States, Inputs>& B, units::second_t dt)
       : m_B(B), m_dt(dt) {
-    m_f = [=](const Eigen::Vector<double, States>& x,
-              const Eigen::Vector<double, Inputs>& u)
-        -> Eigen::Vector<double, States> { return f(x); };
+    m_f = [=](const StateVector& x, const InputVector& u) -> StateVector {
+      return f(x);
+    };
 
     Reset();
   }
@@ -95,7 +92,7 @@
    *
    * @return The calculated feedforward.
    */
-  const Eigen::Vector<double, Inputs>& Uff() const { return m_uff; }
+  const InputVector& Uff() const { return m_uff; }
 
   /**
    * Returns an element of the previously calculated feedforward.
@@ -104,14 +101,14 @@
    *
    * @return The row of the calculated feedforward.
    */
-  double Uff(int i) const { return m_uff(i, 0); }
+  double Uff(int i) const { return m_uff(i); }
 
   /**
    * Returns the current reference vector r.
    *
    * @return The current reference vector.
    */
-  const Eigen::Vector<double, States>& R() const { return m_r; }
+  const StateVector& R() const { return m_r; }
 
   /**
    * Returns an element of the reference vector r.
@@ -120,14 +117,14 @@
    *
    * @return The row of the current reference vector.
    */
-  double R(int i) const { return m_r(i, 0); }
+  double R(int i) const { return m_r(i); }
 
   /**
    * Resets the feedforward with a specified initial state vector.
    *
    * @param initialState The initial state vector.
    */
-  void Reset(const Eigen::Vector<double, States>& initialState) {
+  void Reset(const StateVector& initialState) {
     m_r = initialState;
     m_uff.setZero();
   }
@@ -146,15 +143,14 @@
    * reference.
    *
    * If this method is used the initial state of the system is the one set using
-   * Reset(const Eigen::Vector<double, States>&). If the initial state is not
+   * Reset(const StateVector&). If the initial state is not
    * set it defaults to a zero vector.
    *
    * @param nextR The reference state of the future timestep (k + dt).
    *
    * @return The calculated feedforward.
    */
-  Eigen::Vector<double, Inputs> Calculate(
-      const Eigen::Vector<double, States>& nextR) {
+  InputVector Calculate(const StateVector& nextR) {
     return Calculate(m_r, nextR);
   }
 
@@ -166,36 +162,30 @@
    *
    * @return The calculated feedforward.
    */
-  Eigen::Vector<double, Inputs> Calculate(
-      const Eigen::Vector<double, States>& r,
-      const Eigen::Vector<double, States>& nextR) {
-    Eigen::Vector<double, States> rDot = (nextR - r) / m_dt.value();
+  InputVector Calculate(const StateVector& r, const StateVector& nextR) {
+    StateVector rDot = (nextR - r) / m_dt.value();
 
-    m_uff = m_B.householderQr().solve(
-        rDot - m_f(r, Eigen::Vector<double, Inputs>::Zero()));
+    m_uff = m_B.householderQr().solve(rDot - m_f(r, InputVector::Zero()));
 
     m_r = nextR;
     return m_uff;
   }
 
  private:
-  Eigen::Matrix<double, States, Inputs> m_B;
+  Matrixd<States, Inputs> m_B;
 
   units::second_t m_dt;
 
   /**
    * The model dynamics.
    */
-  std::function<Eigen::Vector<double, States>(
-      const Eigen::Vector<double, States>&,
-      const Eigen::Vector<double, Inputs>&)>
-      m_f;
+  std::function<StateVector(const StateVector&, const InputVector&)> m_f;
 
   // Current reference
-  Eigen::Vector<double, States> m_r;
+  StateVector m_r;
 
   // Computed feedforward
-  Eigen::Vector<double, Inputs> m_uff;
+  InputVector m_uff;
 };
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h
new file mode 100644
index 0000000..3a5148a
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h
@@ -0,0 +1,83 @@
+// 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 "Eigen/Core"
+#include "frc/controller/DifferentialDriveWheelVoltages.h"
+#include "frc/system/LinearSystem.h"
+#include "units/acceleration.h"
+#include "units/angular_acceleration.h"
+#include "units/length.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+
+namespace frc {
+
+/**
+ * Filters the provided voltages to limit a differential drive's linear and
+ * angular acceleration.
+ *
+ * The differential drive model can be created via the functions in
+ * LinearSystemId.
+ */
+class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter {
+ public:
+  /**
+   * Constructs a DifferentialDriveAccelerationLimiter.
+   *
+   * @param system The differential drive dynamics.
+   * @param trackwidth The distance between the differential drive's left and
+   *                   right wheels.
+   * @param maxLinearAccel The maximum linear acceleration.
+   * @param maxAngularAccel The maximum angular acceleration.
+   */
+  DifferentialDriveAccelerationLimiter(
+      LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
+      units::meters_per_second_squared_t maxLinearAccel,
+      units::radians_per_second_squared_t maxAngularAccel);
+
+  /**
+   * Constructs a DifferentialDriveAccelerationLimiter.
+   *
+   * @param system The differential drive dynamics.
+   * @param trackwidth The distance between the differential drive's left and
+   *                   right wheels.
+   * @param minLinearAccel The minimum (most negative) linear acceleration.
+   * @param maxLinearAccel The maximum (most positive) linear acceleration.
+   * @param maxAngularAccel The maximum angular acceleration.
+   * @throws std::invalid_argument if minimum linear acceleration is greater
+   * than maximum linear acceleration
+   */
+  DifferentialDriveAccelerationLimiter(
+      LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
+      units::meters_per_second_squared_t minLinearAccel,
+      units::meters_per_second_squared_t maxLinearAccel,
+      units::radians_per_second_squared_t maxAngularAccel);
+
+  /**
+   * Returns the next voltage pair subject to acceleraiton constraints.
+   *
+   * @param leftVelocity The left wheel velocity.
+   * @param rightVelocity The right wheel velocity.
+   * @param leftVoltage The unconstrained left motor voltage.
+   * @param rightVoltage The unconstrained right motor voltage.
+   * @return The constrained wheel voltages.
+   */
+  DifferentialDriveWheelVoltages Calculate(
+      units::meters_per_second_t leftVelocity,
+      units::meters_per_second_t rightVelocity, units::volt_t leftVoltage,
+      units::volt_t rightVoltage);
+
+ private:
+  LinearSystem<2, 2, 2> m_system;
+  units::meter_t m_trackwidth;
+  units::meters_per_second_squared_t m_minLinearAccel;
+  units::meters_per_second_squared_t m_maxLinearAccel;
+  units::radians_per_second_squared_t m_maxAngularAccel;
+};
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h
new file mode 100644
index 0000000..716bc78
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h
@@ -0,0 +1,83 @@
+// 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/controller/DifferentialDriveWheelVoltages.h"
+#include "frc/system/LinearSystem.h"
+#include "units/acceleration.h"
+#include "units/angular_acceleration.h"
+#include "units/angular_velocity.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+
+namespace frc {
+/**
+ * A helper class which computes the feedforward outputs for a differential
+ * drive drivetrain.
+ */
+class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
+  frc::LinearSystem<2, 2, 2> m_plant;
+
+ public:
+  /**
+   * Creates a new DifferentialDriveFeedforward with the specified parameters.
+   *
+   * @param kVLinear The linear velocity gain in volts per (meters per second).
+   * @param kALinear The linear acceleration gain in volts per (meters per
+   * second squared).
+   * @param kVAngular The angular velocity gain in volts per (radians per
+   * second).
+   * @param kAAngular The angular acceleration gain in volts per (radians per
+   * second squared).
+   * @param trackwidth The distance between the differential drive's left and
+   * right wheels, in meters.
+   */
+  DifferentialDriveFeedforward(decltype(1_V / 1_mps) kVLinear,
+                               decltype(1_V / 1_mps_sq) kALinear,
+                               decltype(1_V / 1_rad_per_s) kVAngular,
+                               decltype(1_V / 1_rad_per_s_sq) kAAngular,
+                               units::meter_t trackwidth);
+
+  /**
+   * Creates a new DifferentialDriveFeedforward with the specified parameters.
+   *
+   * @param kVLinear The linear velocity gain in volts per (meters per second).
+   * @param kALinear The linear acceleration gain in volts per (meters per
+   * second squared).
+   * @param kVAngular The angular velocity gain in volts per (meters per
+   * second).
+   * @param kAAngular The angular acceleration gain in volts per (meters per
+   * second squared).
+   */
+  DifferentialDriveFeedforward(decltype(1_V / 1_mps) kVLinear,
+                               decltype(1_V / 1_mps_sq) kALinear,
+                               decltype(1_V / 1_mps) kVAngular,
+                               decltype(1_V / 1_mps_sq) kAAngular);
+
+  /**
+   * Calculates the differential drive feedforward inputs given velocity
+   * setpoints.
+   *
+   * @param currentLeftVelocity The current left velocity of the differential
+   * drive in meters/second.
+   * @param nextLeftVelocity The next left velocity of the differential drive in
+   * meters/second.
+   * @param currentRightVelocity The current right velocity of the differential
+   * drive in meters/second.
+   * @param nextRightVelocity The next right velocity of the differential drive
+   * in meters/second.
+   * @param dt Discretization timestep.
+   */
+  DifferentialDriveWheelVoltages Calculate(
+      units::meters_per_second_t currentLeftVelocity,
+      units::meters_per_second_t nextLeftVelocity,
+      units::meters_per_second_t currentRightVelocity,
+      units::meters_per_second_t nextRightVelocity, units::second_t dt);
+};
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h
new file mode 100644
index 0000000..48f341e
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h
@@ -0,0 +1,19 @@
+// 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/voltage.h"
+
+namespace frc {
+
+/**
+ * Motor voltages for a differential drive.
+ */
+struct DifferentialDriveWheelVoltages {
+  units::volt_t left = 0_V;
+  units::volt_t right = 0_V;
+};
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
index 269a7e6..bbe7720 100644
--- a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
@@ -6,6 +6,7 @@
 
 #include <wpi/MathExtras.h>
 
+#include "units/length.h"
 #include "units/time.h"
 #include "units/voltage.h"
 
@@ -14,9 +15,9 @@
  * A helper class that computes feedforward outputs for a simple elevator
  * (modeled as a motor acting against the force of gravity).
  */
-template <class Distance>
 class ElevatorFeedforward {
  public:
+  using Distance = units::meters;
   using Velocity =
       units::compound_unit<Distance, units::inverse<units::seconds>>;
   using Acceleration =
@@ -33,7 +34,7 @@
    * @param kS The static gain, in volts.
    * @param kG The gravity gain, in volts.
    * @param kV The velocity gain, in volt seconds per distance.
-   * @param kA The acceleration gain, in volt seconds^2 per distance.
+   * @param kA The acceleration gain, in volt seconds² per distance.
    */
   constexpr ElevatorFeedforward(
       units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
@@ -44,7 +45,7 @@
    * Calculates the feedforward from the gains and setpoints.
    *
    * @param velocity     The velocity setpoint, in distance per second.
-   * @param acceleration The acceleration setpoint, in distance per second^2.
+   * @param acceleration The acceleration setpoint, in distance per second².
    * @return The computed feedforward, in volts.
    */
   constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
diff --git a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
index 398a872..9a749c6 100644
--- a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
+++ b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
@@ -45,6 +45,12 @@
       frc2::PIDController xController, frc2::PIDController yController,
       ProfiledPIDController<units::radian> thetaController);
 
+  HolonomicDriveController(const HolonomicDriveController&) = default;
+  HolonomicDriveController& operator=(const HolonomicDriveController&) =
+      default;
+  HolonomicDriveController(HolonomicDriveController&&) = default;
+  HolonomicDriveController& operator=(HolonomicDriveController&&) = default;
+
   /**
    * Returns true if the pose error is within tolerance of the reference.
    */
@@ -61,32 +67,32 @@
   /**
    * Returns the next output of the holonomic drive controller.
    *
-   * The reference pose, linear velocity, and angular velocity should come from
-   * a drivetrain trajectory.
-   *
-   * @param currentPose        The current pose.
-   * @param poseRef            The desired pose.
-   * @param linearVelocityRef  The desired linear velocity.
-   * @param angleRef           The desired ending angle.
+   * @param currentPose The current pose, as measured by odometry or pose
+   * estimator.
+   * @param trajectoryPose The desired trajectory pose, as sampled for the
+   * current timestep.
+   * @param desiredLinearVelocity The desired linear velocity.
+   * @param desiredHeading The desired heading.
+   * @return The next output of the holonomic drive controller.
    */
-  ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
-                          units::meters_per_second_t linearVelocityRef,
-                          const Rotation2d& angleRef);
+  ChassisSpeeds Calculate(const Pose2d& currentPose,
+                          const Pose2d& trajectoryPose,
+                          units::meters_per_second_t desiredLinearVelocity,
+                          const Rotation2d& desiredHeading);
 
   /**
    * Returns the next output of the holonomic drive controller.
    *
-   * The reference pose, linear velocity, and angular velocity should come from
-   * a drivetrain trajectory.
-   *
-   * @param currentPose  The current pose.
-   * @param desiredState The desired pose, linear velocity, and angular velocity
-   *                     from a trajectory.
-   * @param angleRef     The desired ending angle.
+   * @param currentPose The current pose, as measured by odometry or pose
+   * estimator.
+   * @param desiredState The desired trajectory pose, as sampled for the current
+   * timestep.
+   * @param desiredHeading The desired heading.
+   * @return The next output of the holonomic drive controller.
    */
   ChassisSpeeds Calculate(const Pose2d& currentPose,
                           const Trajectory::State& desiredState,
-                          const Rotation2d& angleRef);
+                          const Rotation2d& desiredHeading);
 
   /**
    * Enables and disables the controller for troubleshooting purposes. When
diff --git a/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
new file mode 100644
index 0000000..eef1fc0
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
@@ -0,0 +1,123 @@
+// 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/system/LinearSystem.h>
+
+#include "Eigen/QR"
+#include "frc/EigenCore.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Contains the controller coefficients and logic for an implicit model
+ * follower.
+ *
+ * Implicit model following lets us design a feedback controller that erases the
+ * dynamics of our system and makes it behave like some other system. This can
+ * be used to make a drivetrain more controllable during teleop driving by
+ * making it behave like a slower or more benign drivetrain.
+ *
+ * For more on the underlying math, read appendix B.3 in
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+template <int States, int Inputs>
+class ImplicitModelFollower {
+ public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param plant    The plant being controlled.
+   * @param plantRef The plant whose dynamics should be followed.
+   */
+  template <int Outputs>
+  ImplicitModelFollower(const LinearSystem<States, Inputs, Outputs>& plant,
+                        const LinearSystem<States, Inputs, Outputs>& plantRef)
+      : ImplicitModelFollower<States, Inputs>(plant.A(), plant.B(),
+                                              plantRef.A(), plantRef.B()) {}
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A    Continuous system matrix of the plant being controlled.
+   * @param B    Continuous input matrix of the plant being controlled.
+   * @param Aref Continuous system matrix whose dynamics should be followed.
+   * @param Bref Continuous input matrix whose dynamics should be followed.
+   */
+  ImplicitModelFollower(const Matrixd<States, States>& A,
+                        const Matrixd<States, Inputs>& B,
+                        const Matrixd<States, States>& Aref,
+                        const Matrixd<States, Inputs>& Bref) {
+    // Find u_imf that makes real model match reference model.
+    //
+    // dx/dt = Ax + Bu_imf
+    // dz/dt = A_ref z + B_ref u
+    //
+    // Let x = z.
+    //
+    // dx/dt = dz/dt
+    // Ax + Bu_imf = Aref x + B_ref u
+    // Bu_imf = A_ref x - Ax + B_ref u
+    // Bu_imf = (A_ref - A)x + B_ref u
+    // u_imf = B⁻¹((A_ref - A)x + Bref u)
+    // u_imf = -B⁻¹(A - A_ref)x + B⁻¹B_ref u
+
+    // The first term makes the open-loop poles that of the reference
+    // system, and the second term makes the input behave like that of the
+    // reference system.
+    m_A = -B.householderQr().solve(A - Aref);
+    m_B = B.householderQr().solve(Bref);
+
+    Reset();
+  }
+
+  /**
+   * Returns the control input vector u.
+   *
+   * @return The control input.
+   */
+  const InputVector& U() const { return m_u; }
+
+  /**
+   * Returns an element of the control input vector u.
+   *
+   * @param i Row of u.
+   *
+   * @return The row of the control input vector.
+   */
+  double U(int i) const { return m_u(i); }
+
+  /**
+   * Resets the controller.
+   */
+  void Reset() { m_u.setZero(); }
+
+  /**
+   * Returns the next output of the controller.
+   *
+   * @param x The current state x.
+   * @param u The current input for the original model.
+   */
+  InputVector Calculate(const StateVector& x, const InputVector& u) {
+    m_u = m_A * x + m_B * u;
+    return m_u;
+  }
+
+ private:
+  // Computed controller output
+  InputVector m_u;
+
+  // State space conversion gain
+  Matrixd<Inputs, States> m_A;
+
+  // Input space conversion gain
+  Matrixd<Inputs, Inputs> m_B;
+};
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h
new file mode 100644
index 0000000..0a336aa
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h
@@ -0,0 +1,125 @@
+// 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 <wpi/array.h>
+#include <wpi/interpolating_map.h>
+
+#include "frc/EigenCore.h"
+#include "frc/controller/DifferentialDriveWheelVoltages.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/trajectory/Trajectory.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+
+namespace frc {
+
+/**
+ * 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.
+ *
+ * See section 8.7 in Controls Engineering in FRC for a derivation of the
+ * control law we used shown in theorem 8.7.4.
+ */
+class WPILIB_DLLEXPORT LTVDifferentialDriveController {
+ public:
+  /**
+   * Constructs a linear time-varying differential drive controller.
+   *
+   * @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.
+   */
+  LTVDifferentialDriveController(const frc::LinearSystem<2, 2, 2>& plant,
+                                 units::meter_t trackwidth,
+                                 const wpi::array<double, 5>& Qelems,
+                                 const wpi::array<double, 2>& Relems,
+                                 units::second_t dt);
+
+  /**
+   * Move constructor.
+   */
+  LTVDifferentialDriveController(LTVDifferentialDriveController&&) = default;
+
+  /**
+   * Move assignment operator.
+   */
+  LTVDifferentialDriveController& operator=(LTVDifferentialDriveController&&) =
+      default;
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   */
+  bool AtReference() const;
+
+  /**
+   * Sets the pose error which is considered tolerable for use with
+   * AtReference().
+   *
+   * @param poseTolerance Pose error which is tolerable.
+   * @param leftVelocityTolerance Left velocity error which is tolerable.
+   * @param rightVelocityTolerance Right velocity error which is tolerable.
+   */
+  void SetTolerance(const Pose2d& poseTolerance,
+                    units::meters_per_second_t leftVelocityTolerance,
+                    units::meters_per_second_t rightVelocityTolerance);
+
+  /**
+   * Returns the left and right output voltages of the LTV controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose      The current pose.
+   * @param leftVelocity     The current left velocity.
+   * @param rightVelocity    The current right velocity.
+   * @param poseRef          The desired pose.
+   * @param leftVelocityRef  The desired left velocity.
+   * @param rightVelocityRef The desired right velocity.
+   */
+  DifferentialDriveWheelVoltages Calculate(
+      const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
+      units::meters_per_second_t rightVelocity, const Pose2d& poseRef,
+      units::meters_per_second_t leftVelocityRef,
+      units::meters_per_second_t rightVelocityRef);
+
+  /**
+   * Returns the left and right output voltages of the LTV controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose  The current pose.
+   * @param leftVelocity The left velocity.
+   * @param rightVelocity The right velocity.
+   * @param desiredState The desired pose, linear velocity, and angular velocity
+   *                     from a trajectory.
+   */
+  DifferentialDriveWheelVoltages Calculate(
+      const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
+      units::meters_per_second_t rightVelocity,
+      const Trajectory::State& desiredState);
+
+ private:
+  units::meter_t m_trackwidth;
+
+  // LUT from drivetrain linear velocity to LQR gain
+  wpi::interpolating_map<units::meters_per_second_t, Matrixd<2, 5>> m_table;
+
+  Vectord<5> m_error;
+  Vectord<5> m_tolerance;
+};
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h
new file mode 100644
index 0000000..83cfe4b
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h
@@ -0,0 +1,123 @@
+// 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 <wpi/array.h>
+#include <wpi/interpolating_map.h>
+
+#include "frc/EigenCore.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/Trajectory.h"
+#include "units/angular_velocity.h"
+#include "units/time.h"
+#include "units/velocity.h"
+
+namespace frc {
+
+/**
+ * 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.
+ *
+ * See section 8.9 in Controls Engineering in FRC for a derivation of the
+ * control law we used shown in theorem 8.9.1.
+ */
+class WPILIB_DLLEXPORT LTVUnicycleController {
+ public:
+  /**
+   * Constructs a linear time-varying unicycle controller with default maximum
+   * desired error tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum
+   * desired control effort of (1 m/s, 2 rad/s).
+   *
+   * @param dt Discretization timestep.
+   * @param maxVelocity The maximum velocity for the controller gain lookup
+   *                    table.
+   */
+  explicit LTVUnicycleController(
+      units::second_t dt, units::meters_per_second_t maxVelocity = 9_mps);
+
+  /**
+   * Constructs a linear time-varying unicycle controller.
+   *
+   * @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.
+   */
+  LTVUnicycleController(const wpi::array<double, 3>& Qelems,
+                        const wpi::array<double, 2>& Relems, units::second_t dt,
+                        units::meters_per_second_t maxVelocity = 9_mps);
+
+  /**
+   * Move constructor.
+   */
+  LTVUnicycleController(LTVUnicycleController&&) = default;
+
+  /**
+   * Move assignment operator.
+   */
+  LTVUnicycleController& operator=(LTVUnicycleController&&) = default;
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   */
+  bool AtReference() const;
+
+  /**
+   * Sets the pose error which is considered tolerable for use with
+   * AtReference().
+   *
+   * @param poseTolerance Pose error which is tolerable.
+   */
+  void SetTolerance(const Pose2d& poseTolerance);
+
+  /**
+   * Returns the linear and angular velocity outputs of the LTV controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose        The current pose.
+   * @param poseRef            The desired pose.
+   * @param linearVelocityRef  The desired linear velocity.
+   * @param angularVelocityRef The desired angular velocity.
+   */
+  ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
+                          units::meters_per_second_t linearVelocityRef,
+                          units::radians_per_second_t angularVelocityRef);
+
+  /**
+   * Returns the linear and angular velocity outputs of the LTV controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose  The current pose.
+   * @param desiredState The desired pose, linear velocity, and angular velocity
+   *                     from a trajectory.
+   */
+  ChassisSpeeds Calculate(const Pose2d& currentPose,
+                          const Trajectory::State& desiredState);
+
+  /**
+   * Enables and disables the controller for troubleshooting purposes.
+   *
+   * @param enabled If the controller is enabled or not.
+   */
+  void SetEnabled(bool enabled);
+
+ private:
+  // LUT from drivetrain linear velocity to LQR gain
+  wpi::interpolating_map<units::meters_per_second_t, Matrixd<2, 3>> m_table;
+
+  Pose2d m_poseError;
+  Pose2d m_poseTolerance;
+  bool m_enabled = true;
+};
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
index 519368d..d4cc3c4 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
@@ -7,8 +7,8 @@
 #include <array>
 #include <functional>
 
-#include "Eigen/Core"
 #include "Eigen/QR"
+#include "frc/EigenCore.h"
 #include "frc/system/Discretization.h"
 #include "frc/system/LinearSystem.h"
 #include "units/time.h"
@@ -31,6 +31,9 @@
 template <int States, int Inputs>
 class LinearPlantInversionFeedforward {
  public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+
   /**
    * Constructs a feedforward with the given plant.
    *
@@ -50,9 +53,9 @@
    * @param B  Continuous input matrix of the plant being controlled.
    * @param dt Discretization timestep.
    */
-  LinearPlantInversionFeedforward(
-      const Eigen::Matrix<double, States, States>& A,
-      const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
+  LinearPlantInversionFeedforward(const Matrixd<States, States>& A,
+                                  const Matrixd<States, Inputs>& B,
+                                  units::second_t dt)
       : m_dt(dt) {
     DiscretizeAB<States, Inputs>(A, B, dt, &m_A, &m_B);
     Reset();
@@ -63,7 +66,7 @@
    *
    * @return The calculated feedforward.
    */
-  const Eigen::Vector<double, Inputs>& Uff() const { return m_uff; }
+  const InputVector& Uff() const { return m_uff; }
 
   /**
    * Returns an element of the previously calculated feedforward.
@@ -72,14 +75,14 @@
    *
    * @return The row of the calculated feedforward.
    */
-  double Uff(int i) const { return m_uff(i, 0); }
+  double Uff(int i) const { return m_uff(i); }
 
   /**
    * Returns the current reference vector r.
    *
    * @return The current reference vector.
    */
-  const Eigen::Vector<double, States>& R() const { return m_r; }
+  const StateVector& R() const { return m_r; }
 
   /**
    * Returns an element of the reference vector r.
@@ -88,14 +91,14 @@
    *
    * @return The row of the current reference vector.
    */
-  double R(int i) const { return m_r(i, 0); }
+  double R(int i) const { return m_r(i); }
 
   /**
    * Resets the feedforward with a specified initial state vector.
    *
    * @param initialState The initial state vector.
    */
-  void Reset(const Eigen::Vector<double, States>& initialState) {
+  void Reset(const StateVector& initialState) {
     m_r = initialState;
     m_uff.setZero();
   }
@@ -114,16 +117,15 @@
    * reference.
    *
    * If this method is used the initial state of the system is the one set using
-   * Reset(const Eigen::Vector<double, States>&). If the initial state is not
+   * Reset(const StateVector&). If the initial state is not
    * set it defaults to a zero vector.
    *
    * @param nextR The reference state of the future timestep (k + dt).
    *
    * @return The calculated feedforward.
    */
-  Eigen::Vector<double, Inputs> Calculate(
-      const Eigen::Vector<double, States>& nextR) {
-    return Calculate(m_r, nextR);  // NOLINT
+  InputVector Calculate(const StateVector& nextR) {
+    return Calculate(m_r, nextR);
   }
 
   /**
@@ -134,25 +136,23 @@
    *
    * @return The calculated feedforward.
    */
-  Eigen::Vector<double, Inputs> Calculate(
-      const Eigen::Vector<double, States>& r,
-      const Eigen::Vector<double, States>& nextR) {
+  InputVector Calculate(const StateVector& r, const StateVector& nextR) {
     m_uff = m_B.householderQr().solve(nextR - (m_A * r));
     m_r = nextR;
     return m_uff;
   }
 
  private:
-  Eigen::Matrix<double, States, States> m_A;
-  Eigen::Matrix<double, States, Inputs> m_B;
+  Matrixd<States, States> m_A;
+  Matrixd<States, Inputs> m_B;
 
   units::second_t m_dt;
 
   // Current reference
-  Eigen::Vector<double, States> m_r;
+  StateVector m_r;
 
   // Computed feedforward
-  Eigen::Vector<double, Inputs> m_uff;
+  InputVector m_uff;
 };
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
index 44a8501..50d6566 100644
--- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
+++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
@@ -4,26 +4,14 @@
 
 #pragma once
 
-#include <frc/fmt/Eigen.h>
-
-#include <string>
-
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "Eigen/Cholesky"
-#include "Eigen/Core"
-#include "Eigen/Eigenvalues"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-#include "frc/StateSpaceUtil.h"
-#include "frc/system/Discretization.h"
+#include "frc/EigenCore.h"
 #include "frc/system/LinearSystem.h"
 #include "units/time.h"
-#include "unsupported/Eigen/MatrixFunctions"
-#include "wpimath/MathShared.h"
 
 namespace frc {
-namespace detail {
 
 /**
  * Contains the controller coefficients and logic for a linear-quadratic
@@ -37,8 +25,14 @@
  * @tparam Inputs Number of inputs.
  */
 template <int States, int Inputs>
-class LinearQuadraticRegulatorImpl {
+class LinearQuadraticRegulator {
  public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+
+  using StateArray = wpi::array<double, States>;
+  using InputArray = wpi::array<double, Inputs>;
+
   /**
    * Constructs a controller with the given coefficients and plant.
    *
@@ -46,14 +40,12 @@
    * @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::invalid_argument If the system is uncontrollable.
    */
   template <int Outputs>
-  LinearQuadraticRegulatorImpl(
-      const LinearSystem<States, Inputs, Outputs>& plant,
-      const wpi::array<double, States>& Qelems,
-      const wpi::array<double, Inputs>& Relems, units::second_t dt)
-      : LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, Relems, dt) {
-  }
+  LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
+                           const StateArray& Qelems, const InputArray& Relems,
+                           units::second_t dt);
 
   /**
    * Constructs a controller with the given coefficients and plant.
@@ -63,14 +55,12 @@
    * @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::invalid_argument If the system is uncontrollable.
    */
-  LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
-                               const Eigen::Matrix<double, States, Inputs>& B,
-                               const wpi::array<double, States>& Qelems,
-                               const wpi::array<double, Inputs>& Relems,
-                               units::second_t dt)
-      : LinearQuadraticRegulatorImpl(A, B, MakeCostMatrix(Qelems),
-                                     MakeCostMatrix(Relems), dt) {}
+  LinearQuadraticRegulator(const Matrixd<States, States>& A,
+                           const Matrixd<States, Inputs>& B,
+                           const StateArray& Qelems, const InputArray& Relems,
+                           units::second_t dt);
 
   /**
    * Constructs a controller with the given coefficients and plant.
@@ -80,36 +70,13 @@
    * @param Q  The state cost matrix.
    * @param R  The input cost matrix.
    * @param dt Discretization timestep.
+   * @throws std::invalid_argument If the system is uncontrollable.
    */
-  LinearQuadraticRegulatorImpl(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,
-                               units::second_t dt) {
-    Eigen::Matrix<double, States, States> discA;
-    Eigen::Matrix<double, States, Inputs> discB;
-    DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
-
-    if (!IsStabilizable<States, Inputs>(discA, discB)) {
-      std::string msg = fmt::format(
-          "The system passed to the LQR is uncontrollable!\n\nA =\n{}\nB "
-          "=\n{}\n",
-          discA, discB);
-
-      wpi::math::MathSharedStore::ReportError(msg);
-      throw std::invalid_argument(msg);
-    }
-
-    Eigen::Matrix<double, States, States> S =
-        drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
-
-    // K = (BᵀSB + R)⁻¹BᵀSA
-    m_K = (discB.transpose() * S * discB + R)
-              .llt()
-              .solve(discB.transpose() * S * discA);
-
-    Reset();
-  }
+  LinearQuadraticRegulator(const Matrixd<States, States>& A,
+                           const Matrixd<States, Inputs>& B,
+                           const Matrixd<States, States>& Q,
+                           const Matrixd<Inputs, Inputs>& R,
+                           units::second_t dt);
 
   /**
    * Constructs a controller with the given coefficients and plant.
@@ -120,36 +87,22 @@
    * @param R  The input cost matrix.
    * @param N  The state-input cross-term cost matrix.
    * @param dt Discretization timestep.
+   * @throws std::invalid_argument If the system is uncontrollable.
    */
-  LinearQuadraticRegulatorImpl(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,
-                               units::second_t dt) {
-    Eigen::Matrix<double, States, States> discA;
-    Eigen::Matrix<double, States, Inputs> discB;
-    DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+  LinearQuadraticRegulator(const Matrixd<States, States>& A,
+                           const Matrixd<States, Inputs>& B,
+                           const Matrixd<States, States>& Q,
+                           const Matrixd<Inputs, Inputs>& R,
+                           const Matrixd<States, Inputs>& N,
+                           units::second_t dt);
 
-    Eigen::Matrix<double, States, States> S =
-        drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
-
-    // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
-    m_K = (discB.transpose() * S * discB + R)
-              .llt()
-              .solve(discB.transpose() * S * discA + N.transpose());
-
-    Reset();
-  }
-
-  LinearQuadraticRegulatorImpl(LinearQuadraticRegulatorImpl&&) = default;
-  LinearQuadraticRegulatorImpl& operator=(LinearQuadraticRegulatorImpl&&) =
-      default;
+  LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
+  LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
 
   /**
    * Returns the controller matrix K.
    */
-  const Eigen::Matrix<double, Inputs, States>& K() const { return m_K; }
+  const Matrixd<Inputs, States>& K() const { return m_K; }
 
   /**
    * Returns an element of the controller matrix K.
@@ -164,7 +117,7 @@
    *
    * @return The reference vector.
    */
-  const Eigen::Vector<double, States>& R() const { return m_r; }
+  const StateVector& R() const { return m_r; }
 
   /**
    * Returns an element of the reference vector r.
@@ -173,14 +126,14 @@
    *
    * @return The row of the reference vector.
    */
-  double R(int i) const { return m_r(i, 0); }
+  double R(int i) const { return m_r(i); }
 
   /**
    * Returns the control input vector u.
    *
    * @return The control input.
    */
-  const Eigen::Vector<double, Inputs>& U() const { return m_u; }
+  const InputVector& U() const { return m_u; }
 
   /**
    * Returns an element of the control input vector u.
@@ -189,7 +142,7 @@
    *
    * @return The row of the control input vector.
    */
-  double U(int i) const { return m_u(i, 0); }
+  double U(int i) const { return m_u(i); }
 
   /**
    * Resets the controller.
@@ -204,11 +157,7 @@
    *
    * @param x The current state x.
    */
-  Eigen::Vector<double, Inputs> Calculate(
-      const Eigen::Vector<double, States>& x) {
-    m_u = m_K * (m_r - x);
-    return m_u;
-  }
+  InputVector Calculate(const StateVector& x);
 
   /**
    * Returns the next output of the controller.
@@ -216,12 +165,7 @@
    * @param x     The current state x.
    * @param nextR The next reference vector r.
    */
-  Eigen::Vector<double, Inputs> Calculate(
-      const Eigen::Vector<double, States>& x,
-      const Eigen::Vector<double, States>& nextR) {
-    m_r = nextR;
-    return Calculate(x);
-  }
+  InputVector Calculate(const StateVector& x, const StateVector& nextR);
 
   /**
    * Adjusts LQR controller gain to compensate for a pure time delay in the
@@ -241,209 +185,26 @@
    */
   template <int Outputs>
   void LatencyCompensate(const LinearSystem<States, Inputs, Outputs>& plant,
-                         units::second_t dt, units::second_t inputDelay) {
-    Eigen::Matrix<double, States, States> discA;
-    Eigen::Matrix<double, States, Inputs> discB;
-    DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
-
-    m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
-  }
+                         units::second_t dt, units::second_t inputDelay);
 
  private:
   // Current reference
-  Eigen::Vector<double, States> m_r;
+  StateVector m_r;
 
   // Computed controller output
-  Eigen::Vector<double, Inputs> m_u;
+  InputVector m_u;
 
   // Controller gain
-  Eigen::Matrix<double, Inputs, States> m_K;
+  Matrixd<Inputs, States> m_K;
 };
 
-}  // namespace detail
-
-template <int States, int Inputs>
-class LinearQuadraticRegulator
-    : public detail::LinearQuadraticRegulatorImpl<States, Inputs> {
- public:
-  /**
-   * Constructs a controller with the given coefficients and plant.
-   *
-   * @tparam Outputs The number of outputs.
-   * @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.
-   * @param dt     Discretization timestep.
-   */
-  template <int Outputs>
-  LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
-                           const wpi::array<double, States>& Qelems,
-                           const wpi::array<double, Inputs>& Relems,
-                           units::second_t dt)
-      : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
-
-  /**
-   * Constructs a controller with the given coefficients and plant.
-   *
-   * @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.
-   * @param Relems The maximum desired control effort for each input.
-   * @param dt     Discretization timestep.
-   */
-  LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
-                           const Eigen::Matrix<double, States, Inputs>& B,
-                           const wpi::array<double, States>& Qelems,
-                           const wpi::array<double, Inputs>& Relems,
-                           units::second_t dt)
-      : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
-                                 MakeCostMatrix(Relems), dt) {}
-
-  /**
-   * Constructs a controller with the given coefficients and plant.
-   *
-   * @param A  Continuous system matrix of the plant being controlled.
-   * @param B  Continuous input matrix of the plant being controlled.
-   * @param Q  The state cost matrix.
-   * @param R  The input cost matrix.
-   * @param dt Discretization timestep.
-   */
-  LinearQuadraticRegulator(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,
-                           units::second_t dt)
-      : detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q, R, dt} {}
-
-  /**
-   * Constructs a controller with the given coefficients and plant.
-   *
-   * @param A  Continuous system matrix of the plant being controlled.
-   * @param B  Continuous input matrix of the plant being controlled.
-   * @param Q  The state cost matrix.
-   * @param R  The input cost matrix.
-   * @param N  The state-input cross-term cost matrix.
-   * @param dt Discretization timestep.
-   */
-  LinearQuadraticRegulator(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,
-                           units::second_t dt)
-      : detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q,
-                                                             R, N, dt} {}
-
-  LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
-  LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
-};
-
-// Template specializations are used here to make common state-input pairs
-// compile faster.
-template <>
-class WPILIB_DLLEXPORT LinearQuadraticRegulator<1, 1>
-    : public detail::LinearQuadraticRegulatorImpl<1, 1> {
- public:
-  template <int Outputs>
-  LinearQuadraticRegulator(const LinearSystem<1, 1, Outputs>& plant,
-                           const wpi::array<double, 1>& Qelems,
-                           const wpi::array<double, 1>& Relems,
-                           units::second_t dt)
-      : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
-
-  LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
-                           const Eigen::Matrix<double, 1, 1>& B,
-                           const wpi::array<double, 1>& Qelems,
-                           const wpi::array<double, 1>& Relems,
-                           units::second_t dt);
-
-  LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
-                           const Eigen::Matrix<double, 1, 1>& B,
-                           const Eigen::Matrix<double, 1, 1>& Q,
-                           const Eigen::Matrix<double, 1, 1>& R,
-                           units::second_t dt);
-
-  LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
-                           const Eigen::Matrix<double, 1, 1>& B,
-                           const Eigen::Matrix<double, 1, 1>& Q,
-                           const Eigen::Matrix<double, 1, 1>& R,
-                           const Eigen::Matrix<double, 1, 1>& N,
-                           units::second_t dt);
-
-  LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
-  LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
-};
-
-// Template specializations are used here to make common state-input pairs
-// compile faster.
-template <>
-class WPILIB_DLLEXPORT LinearQuadraticRegulator<2, 1>
-    : public detail::LinearQuadraticRegulatorImpl<2, 1> {
- public:
-  template <int Outputs>
-  LinearQuadraticRegulator(const LinearSystem<2, 1, Outputs>& plant,
-                           const wpi::array<double, 2>& Qelems,
-                           const wpi::array<double, 1>& Relems,
-                           units::second_t dt)
-      : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
-
-  LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
-                           const Eigen::Matrix<double, 2, 1>& B,
-                           const wpi::array<double, 2>& Qelems,
-                           const wpi::array<double, 1>& Relems,
-                           units::second_t dt);
-
-  LinearQuadraticRegulator(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,
-                           units::second_t dt);
-
-  LinearQuadraticRegulator(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,
-                           units::second_t dt);
-
-  LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
-  LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
-};
-
-// Template specializations are used here to make common state-input pairs
-// compile faster.
-template <>
-class WPILIB_DLLEXPORT LinearQuadraticRegulator<2, 2>
-    : public detail::LinearQuadraticRegulatorImpl<2, 2> {
- public:
-  template <int Outputs>
-  LinearQuadraticRegulator(const LinearSystem<2, 2, Outputs>& plant,
-                           const wpi::array<double, 2>& Qelems,
-                           const wpi::array<double, 2>& Relems,
-                           units::second_t dt)
-      : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
-
-  LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
-                           const Eigen::Matrix<double, 2, 2>& B,
-                           const wpi::array<double, 2>& Qelems,
-                           const wpi::array<double, 2>& Relems,
-                           units::second_t dt);
-
-  LinearQuadraticRegulator(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,
-                           units::second_t dt);
-
-  LinearQuadraticRegulator(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,
-                           units::second_t dt);
-
-  LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
-  LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
-};
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    LinearQuadraticRegulator<1, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    LinearQuadraticRegulator<2, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    LinearQuadraticRegulator<2, 2>;
 
 }  // namespace frc
+
+#include "LinearQuadraticRegulator.inc"
diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
new file mode 100644
index 0000000..87d37ec
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
@@ -0,0 +1,113 @@
+// 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/Eigenvalues"
+#include "drake/math/discrete_algebraic_riccati_equation.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 {
+
+template <int States, int Inputs>
+template <int Outputs>
+LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
+    const LinearSystem<States, Inputs, Outputs>& plant,
+    const StateArray& Qelems, const InputArray& Relems, units::second_t dt)
+    : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
+
+template <int States, int Inputs>
+LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
+    const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
+    const StateArray& Qelems, const InputArray& Relems, units::second_t dt)
+    : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
+                               MakeCostMatrix(Relems), dt) {}
+
+template <int States, int Inputs>
+LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
+    const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
+    const Matrixd<States, States>& Q, const Matrixd<Inputs, Inputs>& R,
+    units::second_t dt) {
+  Matrixd<States, States> discA;
+  Matrixd<States, Inputs> discB;
+  DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+
+  if (!IsStabilizable<States, Inputs>(discA, discB)) {
+    std::string msg = fmt::format(
+        "The system passed to the LQR is uncontrollable!\n\nA =\n{}\nB "
+        "=\n{}\n",
+        discA, discB);
+
+    wpi::math::MathSharedStore::ReportError(msg);
+    throw std::invalid_argument(msg);
+  }
+
+  Matrixd<States, States> S =
+      drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
+
+  // K = (BᵀSB + R)⁻¹BᵀSA
+  m_K = (discB.transpose() * S * discB + R)
+            .llt()
+            .solve(discB.transpose() * S * discA);
+
+  Reset();
+}
+
+template <int States, int Inputs>
+LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
+    const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
+    const Matrixd<States, States>& Q, const Matrixd<Inputs, Inputs>& R,
+    const Matrixd<States, Inputs>& N, units::second_t dt) {
+  Matrixd<States, States> discA;
+  Matrixd<States, Inputs> discB;
+  DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+
+  Matrixd<States, States> S =
+      drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
+
+  // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
+  m_K = (discB.transpose() * S * discB + R)
+            .llt()
+            .solve(discB.transpose() * S * discA + N.transpose());
+
+  Reset();
+}
+
+template <int States, int Inputs>
+typename LinearQuadraticRegulator<States, Inputs>::InputVector
+LinearQuadraticRegulator<States, Inputs>::Calculate(const StateVector& x) {
+  m_u = m_K * (m_r - x);
+  return m_u;
+}
+
+template <int States, int Inputs>
+typename LinearQuadraticRegulator<States, Inputs>::InputVector
+LinearQuadraticRegulator<States, Inputs>::Calculate(const StateVector& x,
+                                                    const StateVector& nextR) {
+  m_r = nextR;
+  return Calculate(x);
+}
+
+template <int States, int Inputs>
+template <int Outputs>
+void LinearQuadraticRegulator<States, Inputs>::LatencyCompensate(
+    const LinearSystem<States, Inputs, Outputs>& plant, units::second_t dt,
+    units::second_t inputDelay) {
+  Matrixd<States, States> discA;
+  Matrixd<States, Inputs> discB;
+  DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
+
+  m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
+}
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/controller/PIDController.h b/wpimath/src/main/native/include/frc/controller/PIDController.h
index 98625f8..d6a41d1 100644
--- a/wpimath/src/main/native/include/frc/controller/PIDController.h
+++ b/wpimath/src/main/native/include/frc/controller/PIDController.h
@@ -102,6 +102,20 @@
   units::second_t GetPeriod() const;
 
   /**
+   * Gets the position tolerance of this controller.
+   *
+   * @return The position tolerance of the controller.
+   */
+  double GetPositionTolerance() const;
+
+  /**
+   * Gets the velocity tolerance of this controller.
+   *
+   * @return The velocity tolerance of the controller.
+   */
+  double GetVelocityTolerance() const;
+
+  /**
    * Sets the setpoint for the PIDController.
    *
    * @param setpoint The desired setpoint.
diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
index e0b10c7..8491118 100644
--- a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -22,7 +22,7 @@
 namespace frc {
 namespace detail {
 WPILIB_DLLEXPORT
-void ReportProfiledPIDController();
+int IncrementAndGetProfiledPIDControllerInstances();
 }  // namespace detail
 
 /**
@@ -59,7 +59,10 @@
   ProfiledPIDController(double Kp, double Ki, double Kd,
                         Constraints constraints, units::second_t period = 20_ms)
       : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
-    detail::ReportProfiledPIDController();
+    int instances = detail::IncrementAndGetProfiledPIDControllerInstances();
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kController_ProfiledPIDController, instances);
+    wpi::SendableRegistry::Add(this, "ProfiledPIDController", instances);
   }
 
   ~ProfiledPIDController() override = default;
@@ -132,6 +135,24 @@
   units::second_t GetPeriod() const { return m_controller.GetPeriod(); }
 
   /**
+   * Gets the position tolerance of this controller.
+   *
+   * @return The position tolerance of the controller.
+   */
+  double GetPositionTolerance() const {
+    return m_controller.GetPositionTolerance();
+  }
+
+  /**
+   * Gets the velocity tolerance of this controller.
+   *
+   * @return The velocity tolerance of the controller.
+   */
+  double GetVelocityTolerance() const {
+    return m_controller.GetVelocityTolerance();
+  }
+
+  /**
    * Sets the goal for the ProfiledPIDController.
    *
    * @param goal The desired unprofiled setpoint.
@@ -143,7 +164,7 @@
    *
    * @param goal The desired unprofiled setpoint.
    */
-  void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t(0)}; }
+  void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t{0}}; }
 
   /**
    * Gets the goal for the ProfiledPIDController.
@@ -224,9 +245,9 @@
    * @param positionTolerance Position error which is tolerable.
    * @param velocityTolerance Velocity error which is tolerable.
    */
-  void SetTolerance(
-      Distance_t positionTolerance,
-      Velocity_t velocityTolerance = std::numeric_limits<double>::infinity()) {
+  void SetTolerance(Distance_t positionTolerance,
+                    Velocity_t velocityTolerance = Velocity_t{
+                        std::numeric_limits<double>::infinity()}) {
     m_controller.SetTolerance(positionTolerance.value(),
                               velocityTolerance.value());
   }
@@ -237,14 +258,14 @@
    * @return The error.
    */
   Distance_t GetPositionError() const {
-    return Distance_t(m_controller.GetPositionError());
+    return Distance_t{m_controller.GetPositionError()};
   }
 
   /**
    * Returns the change in error per second.
    */
   Velocity_t GetVelocityError() const {
-    return Velocity_t(m_controller.GetVelocityError());
+    return Velocity_t{m_controller.GetVelocityError()};
   }
 
   /**
@@ -339,7 +360,7 @@
    * velocity is assumed to be zero.
    */
   void Reset(Distance_t measuredPosition) {
-    Reset(measuredPosition, Velocity_t(0));
+    Reset(measuredPosition, Velocity_t{0});
   }
 
   void InitSendable(wpi::SendableBuilder& builder) override {
diff --git a/wpimath/src/main/native/include/frc/controller/RamseteController.h b/wpimath/src/main/native/include/frc/controller/RamseteController.h
index bb23f46..d031101 100644
--- a/wpimath/src/main/native/include/frc/controller/RamseteController.h
+++ b/wpimath/src/main/native/include/frc/controller/RamseteController.h
@@ -5,7 +5,6 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
-#include <wpi/deprecated.h>
 
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
@@ -58,17 +57,6 @@
    * @param zeta Tuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger
    *             values provide more damping in response.
    */
-  WPI_DEPRECATED("Use unit-safe constructor instead")
-  RamseteController(double b, double zeta);
-
-  /**
-   * Construct a Ramsete unicycle controller.
-   *
-   * @param b    Tuning parameter (b > 0 rad²/m²) for which larger values make
-   *             convergence more aggressive like a proportional term.
-   * @param zeta Tuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger
-   *             values provide more damping in response.
-   */
   RamseteController(units::unit_t<b_unit> b, units::unit_t<zeta_unit> zeta);
 
   /**
diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
index df1a52c..86b40ea 100644
--- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
+++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
@@ -6,7 +6,7 @@
 
 #include <wpi/MathExtras.h>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/controller/LinearPlantInversionFeedforward.h"
 #include "frc/system/plant/LinearSystemId.h"
 #include "units/time.h"
@@ -35,7 +35,7 @@
    *
    * @param kS The static gain, in volts.
    * @param kV The velocity gain, in volt seconds per distance.
-   * @param kA The acceleration gain, in volt seconds^2 per distance.
+   * @param kA The acceleration gain, in volt seconds² per distance.
    */
   constexpr SimpleMotorFeedforward(
       units::volt_t kS, units::unit_t<kv_unit> kV,
@@ -46,7 +46,7 @@
    * Calculates the feedforward from the gains and setpoints.
    *
    * @param velocity     The velocity setpoint, in distance per second.
-   * @param acceleration The acceleration setpoint, in distance per second^2.
+   * @param acceleration The acceleration setpoint, in distance per second².
    * @return The computed feedforward, in volts.
    */
   constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
@@ -70,8 +70,8 @@
     auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
     LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
 
-    Eigen::Vector<double, 1> r{currentVelocity.value()};
-    Eigen::Vector<double, 1> nextR{nextVelocity.value()};
+    Vectord<1> r{currentVelocity.value()};
+    Vectord<1> nextR{nextVelocity.value()};
 
     return kS * wpi::sgn(currentVelocity.value()) +
            units::volt_t{feedforward.Calculate(r, nextR)(0)};
diff --git a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
index 3ddabc1..0512c68c 100644
--- a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
+++ b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
@@ -4,9 +4,9 @@
 
 #pragma once
 
-#include <wpi/numbers>
+#include <numbers>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/MathUtil.h"
 
 namespace frc {
@@ -21,10 +21,9 @@
  * @param angleStateIdx The row containing angles to be normalized.
  */
 template <int States>
-Eigen::Vector<double, States> AngleResidual(
-    const Eigen::Vector<double, States>& a,
-    const Eigen::Vector<double, States>& b, int angleStateIdx) {
-  Eigen::Vector<double, States> ret = a - b;
+Vectord<States> AngleResidual(const Vectord<States>& a,
+                              const Vectord<States>& b, int angleStateIdx) {
+  Vectord<States> ret = a - b;
   ret[angleStateIdx] =
       AngleModulus(units::radian_t{ret[angleStateIdx]}).value();
   return ret;
@@ -38,8 +37,7 @@
  * @param angleStateIdx The row containing angles to be normalized.
  */
 template <int States>
-std::function<Eigen::Vector<double, States>(
-    const Eigen::Vector<double, States>&, const Eigen::Vector<double, States>&)>
+std::function<Vectord<States>(const Vectord<States>&, const Vectord<States>&)>
 AngleResidual(int angleStateIdx) {
   return [=](auto a, auto b) {
     return AngleResidual<States>(a, b, angleStateIdx);
@@ -56,12 +54,11 @@
  * @param angleStateIdx The row containing angles to be normalized.
  */
 template <int States>
-Eigen::Vector<double, States> AngleAdd(const Eigen::Vector<double, States>& a,
-                                       const Eigen::Vector<double, States>& b,
-                                       int angleStateIdx) {
-  Eigen::Vector<double, States> ret = a + b;
+Vectord<States> AngleAdd(const Vectord<States>& a, const Vectord<States>& b,
+                         int angleStateIdx) {
+  Vectord<States> ret = a + b;
   ret[angleStateIdx] =
-      InputModulus(ret[angleStateIdx], -wpi::numbers::pi, wpi::numbers::pi);
+      InputModulus(ret[angleStateIdx], -std::numbers::pi, std::numbers::pi);
   return ret;
 }
 
@@ -73,8 +70,7 @@
  * @param angleStateIdx The row containing angles to be normalized.
  */
 template <int States>
-std::function<Eigen::Vector<double, States>(
-    const Eigen::Vector<double, States>&, const Eigen::Vector<double, States>&)>
+std::function<Vectord<States>(const Vectord<States>&, const Vectord<States>&)>
 AngleAdd(int angleStateIdx) {
   return [=](auto a, auto b) { return AngleAdd<States>(a, b, angleStateIdx); };
 }
@@ -91,17 +87,21 @@
  * @param angleStatesIdx The row containing the angles.
  */
 template <int CovDim, int States>
-Eigen::Vector<double, CovDim> AngleMean(
-    const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
-    const Eigen::Vector<double, 2 * States + 1>& Wm, int angleStatesIdx) {
-  double sumSin = sigmas.row(angleStatesIdx)
-                      .unaryExpr([](auto it) { return std::sin(it); })
+Vectord<CovDim> AngleMean(const Matrixd<CovDim, 2 * States + 1>& sigmas,
+                          const Vectord<2 * States + 1>& Wm,
+                          int angleStatesIdx) {
+  double sumSin = (sigmas.row(angleStatesIdx).unaryExpr([](auto it) {
+                    return std::sin(it);
+                  }) *
+                   Wm)
                       .sum();
-  double sumCos = sigmas.row(angleStatesIdx)
-                      .unaryExpr([](auto it) { return std::cos(it); })
+  double sumCos = (sigmas.row(angleStatesIdx).unaryExpr([](auto it) {
+                    return std::cos(it);
+                  }) *
+                   Wm)
                       .sum();
 
-  Eigen::Vector<double, CovDim> ret = sigmas * Wm;
+  Vectord<CovDim> ret = sigmas * Wm;
   ret[angleStatesIdx] = std::atan2(sumSin, sumCos);
   return ret;
 }
@@ -116,9 +116,8 @@
  * @param angleStateIdx The row containing the angles.
  */
 template <int CovDim, int States>
-std::function<Eigen::Vector<double, CovDim>(
-    const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
-    const Eigen::Vector<double, 2 * States + 1>&)>
+std::function<Vectord<CovDim>(const Matrixd<CovDim, 2 * States + 1>&,
+                              const Vectord<2 * States + 1>&)>
 AngleMean(int angleStateIdx) {
   return [=](auto sigmas, auto Wm) {
     return AngleMean<CovDim, States>(sigmas, Wm, angleStateIdx);
diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
index 12810fa..339ccc9 100644
--- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
@@ -7,17 +7,18 @@
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "Eigen/Core"
-#include "frc/estimator/KalmanFilterLatencyCompensator.h"
-#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/EigenCore.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 {
 /**
- * This class wraps an Unscented Kalman Filter to fuse latency-compensated
+ * This class wraps Differential Drive Odometry to fuse latency-compensated
  * vision measurements with differential drive encoder measurements. It will
  * correct for noisy vision measurements and encoder drift. It is intended to be
  * an easy drop-in for DifferentialDriveOdometry. In fact, if you never call
@@ -25,77 +26,68 @@
  * same as DifferentialDriveOdometry.
  *
  * Update() should be called every robot loop (if your robot loops are faster or
- * slower than the default, then you should change the nominal delta time via
- * the constructor).
+ * slower than the default of 20 ms, then you should change the nominal delta
+ * time via the constructor).
  *
  * AddVisionMeasurement() can be called as infrequently as you want; if you
  * never call it, then this class will behave like regular encoder odometry.
- *
- * The state-space system used internally has the following states (x), inputs
- * (u), and outputs (y):
- *
- * <strong> x = [x, y, theta, dist_l, dist_r]ᵀ </strong> in the field coordinate
- * system containing x position, y position, heading, left encoder distance,
- * and right encoder distance.
- *
- * <strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity,
- * and angular velocity in the field coordinate system.
- *
- * NB: Using velocities make things considerably easier, because it means that
- * teams don't have to worry about getting an accurate model. Basically, we
- * suspect that it's easier for teams to get good encoder data than it is for
- * them to perform system identification well enough to get a good model.
- *
- * <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
- * position, and heading; or <strong>y = [dist_l, dist_r, theta] </strong>
- * containing left encoder position, right encoder position, and gyro heading.
  */
 class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
  public:
   /**
-   * Constructs a DifferentialDrivePoseEstimator.
+   * Constructs a DifferentialDrivePoseEstimator with default standard
+   * deviations for the model and vision measurements.
    *
-   * @param gyroAngle                The gyro angle of the robot.
-   * @param initialPose              The estimated initial pose.
-   * @param stateStdDevs             Standard deviations of model states.
-   *                                 Increase these numbers to trust your
-   *                                 model's state estimates less. This matrix
-   *                                 is in the form
-   *                                 [x, y, theta, dist_l, dist_r]ᵀ,
-   *                                 with units in meters and radians.
-   * @param localMeasurementStdDevs  Standard deviations of the encoder and gyro
-   *                                 measurements. Increase these numbers to
-   *                                 trust sensor readings from
-   *                                 encoders and gyros less.
-   *                                 This matrix is in the form
-   *                                 [dist_l, dist_r, theta]ᵀ, with units in
-   *                                 meters and radians.
-   * @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.
-   * @param nominalDt                The period of the loop calling Update().
+   * The default standard deviations of the model states are
+   * 0.02 meters for x, 0.02 meters for y, and 0.01 radians for heading.
+   * The default standard deviations of the vision measurements are
+   * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The gyro angle of the robot.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @param initialPose The estimated initial pose.
    */
-  DifferentialDrivePoseEstimator(
-      const Rotation2d& gyroAngle, const Pose2d& initialPose,
-      const wpi::array<double, 5>& stateStdDevs,
-      const wpi::array<double, 3>& localMeasurementStdDevs,
-      const wpi::array<double, 3>& visionMeasurementStdDevs,
-      units::second_t nominalDt = 0.02_s);
+  DifferentialDrivePoseEstimator(DifferentialDriveKinematics& kinematics,
+                                 const Rotation2d& gyroAngle,
+                                 units::meter_t leftDistance,
+                                 units::meter_t rightDistance,
+                                 const Pose2d& initialPose);
 
   /**
-   * Sets the pose estimator's trust of global measurements. This might be used
+   * Constructs a DifferentialDrivePoseEstimator.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The gyro angle of the robot.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @param initialPose The estimated initial pose.
+   * @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.
+   */
+  DifferentialDrivePoseEstimator(
+      DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+      units::meter_t leftDistance, units::meter_t rightDistance,
+      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
-   *                                 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.
+   * @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);
@@ -103,74 +95,71 @@
   /**
    * Resets the robot's position on the field.
    *
-   * You NEED to reset your encoders to zero when calling this method. 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 pose The estimated pose of the robot on the field.
    * @param gyroAngle The current gyro angle.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @param pose The estimated pose of the robot on the field.
    */
-  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle);
+  void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+                     units::meter_t rightDistance, const Pose2d& pose);
 
   /**
-   * Returns the pose of the robot at the current time as estimated by the
-   * Unscented Kalman Filter.
+   * Gets the estimated robot pose.
    *
    * @return The estimated robot pose.
    */
   Pose2d GetEstimatedPosition() const;
 
   /**
-   * Adds a vision measurement to the Unscented Kalman Filter. This will correct
+   * 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.
+   *     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 Unscented Kalman Filter. This will correct
+   * 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
-   *                                 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.
+   * @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,
@@ -180,27 +169,24 @@
   }
 
   /**
-   * Updates the Unscented Kalman Filter using only wheel encoder information.
-   * Note that this should be called every loop iteration.
+   * Updates the pose estimator with wheel encoder and gyro information. This
+   * should be called every loop.
    *
    * @param gyroAngle     The current gyro angle.
-   * @param wheelSpeeds   The velocities of the wheels in meters per second.
    * @param leftDistance  The distance traveled by the left encoder.
    * @param rightDistance The distance traveled by the right encoder.
    *
    * @return The estimated pose of the robot.
    */
-  Pose2d Update(const Rotation2d& gyroAngle,
-                const DifferentialDriveWheelSpeeds& wheelSpeeds,
-                units::meter_t leftDistance, units::meter_t rightDistance);
+  Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+                units::meter_t rightDistance);
 
   /**
-   * Updates the Unscented Kalman Filter using only wheel encoder information.
-   * Note that this should be called every loop iteration.
+   * 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 wheelSpeeds   The velocities of the wheels in meters per second.
    * @param leftDistance  The distance traveled by the left encoder.
    * @param rightDistance The distance traveled by the right encoder.
    *
@@ -208,35 +194,62 @@
    */
   Pose2d UpdateWithTime(units::second_t currentTime,
                         const Rotation2d& gyroAngle,
-                        const DifferentialDriveWheelSpeeds& wheelSpeeds,
                         units::meter_t leftDistance,
                         units::meter_t rightDistance);
 
  private:
-  UnscentedKalmanFilter<5, 3, 3> m_observer;
-  KalmanFilterLatencyCompensator<5, 3, 3, UnscentedKalmanFilter<5, 3, 3>>
-      m_latencyCompensator;
-  std::function<void(const Eigen::Vector<double, 3>& u,
-                     const Eigen::Vector<double, 3>& y)>
-      m_visionCorrect;
+  struct InterpolationRecord {
+    // The pose observed given the current sensor inputs and the previous pose.
+    Pose2d pose;
 
-  Eigen::Matrix<double, 3, 3> m_visionContR;
+    // The current gyro angle.
+    Rotation2d gyroAngle;
 
-  units::second_t m_nominalDt;
-  units::second_t m_prevTime = -1_s;
+    // The distance traveled by the left encoder.
+    units::meter_t leftDistance;
 
-  Rotation2d m_gyroOffset;
-  Rotation2d m_previousAngle;
+    // The distance traveled by the right encoder.
+    units::meter_t rightDistance;
 
-  template <int Dim>
-  static wpi::array<double, Dim> StdDevMatrixToArray(
-      const Eigen::Vector<double, Dim>& stdDevs);
+    /**
+     * 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;
 
-  static Eigen::Vector<double, 5> F(const Eigen::Vector<double, 5>& x,
-                                    const Eigen::Vector<double, 3>& u);
-  static Eigen::Vector<double, 5> FillStateVector(const Pose2d& pose,
-                                                  units::meter_t leftDistance,
-                                                  units::meter_t rightDistance);
+    /**
+     * 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);
+      }};
 };
 
 }  // 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 3e5edb8..b09e8d9 100644
--- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
@@ -8,13 +8,7 @@
 
 #include <wpi/array.h>
 
-#include "Eigen/Cholesky"
-#include "Eigen/Core"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-#include "frc/StateSpaceUtil.h"
-#include "frc/system/Discretization.h"
-#include "frc/system/NumericalIntegration.h"
-#include "frc/system/NumericalJacobian.h"
+#include "frc/EigenCore.h"
 #include "units/time.h"
 
 namespace frc {
@@ -46,6 +40,15 @@
 template <int States, int Inputs, int Outputs>
 class ExtendedKalmanFilter {
  public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+  using OutputVector = Vectord<Outputs>;
+
+  using StateArray = wpi::array<double, States>;
+  using OutputArray = wpi::array<double, Outputs>;
+
+  using StateMatrix = Matrixd<States, States>;
+
   /**
    * Constructs an extended Kalman filter.
    *
@@ -57,52 +60,11 @@
    * @param measurementStdDevs Standard deviations of measurements.
    * @param dt                 Nominal discretization timestep.
    */
-  ExtendedKalmanFilter(std::function<Eigen::Vector<double, States>(
-                           const Eigen::Vector<double, States>&,
-                           const Eigen::Vector<double, Inputs>&)>
-                           f,
-                       std::function<Eigen::Vector<double, Outputs>(
-                           const Eigen::Vector<double, States>&,
-                           const Eigen::Vector<double, Inputs>&)>
-                           h,
-                       const wpi::array<double, States>& stateStdDevs,
-                       const wpi::array<double, Outputs>& measurementStdDevs,
-                       units::second_t dt)
-      : m_f(f), m_h(h) {
-    m_contQ = MakeCovMatrix(stateStdDevs);
-    m_contR = MakeCovMatrix(measurementStdDevs);
-    m_residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Outputs> {
-      return a - b;
-    };
-    m_addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
-      return a + b;
-    };
-    m_dt = dt;
-
-    Reset();
-
-    Eigen::Matrix<double, States, States> contA =
-        NumericalJacobianX<States, States, Inputs>(
-            m_f, m_xHat, Eigen::Vector<double, Inputs>::Zero());
-    Eigen::Matrix<double, Outputs, States> C =
-        NumericalJacobianX<Outputs, States, Inputs>(
-            m_h, m_xHat, Eigen::Vector<double, Inputs>::Zero());
-
-    Eigen::Matrix<double, States, States> discA;
-    Eigen::Matrix<double, States, States> discQ;
-    DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
-
-    Eigen::Matrix<double, 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);
-    } else {
-      m_initP = Eigen::Matrix<double, States, States>::Zero();
-    }
-    m_P = m_initP;
-  }
+  ExtendedKalmanFilter(
+      std::function<StateVector(const StateVector&, const InputVector&)> f,
+      std::function<OutputVector(const StateVector&, const InputVector&)> h,
+      const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+      units::second_t dt);
 
   /**
    * Constructs an extended Kalman filter.
@@ -118,59 +80,20 @@
    * @param addFuncX           A function that adds two state vectors.
    * @param dt                 Nominal discretization timestep.
    */
-  ExtendedKalmanFilter(std::function<Eigen::Vector<double, States>(
-                           const Eigen::Vector<double, States>&,
-                           const Eigen::Vector<double, Inputs>&)>
-                           f,
-                       std::function<Eigen::Vector<double, Outputs>(
-                           const Eigen::Vector<double, States>&,
-                           const Eigen::Vector<double, Inputs>&)>
-                           h,
-                       const wpi::array<double, States>& stateStdDevs,
-                       const wpi::array<double, Outputs>& measurementStdDevs,
-                       std::function<Eigen::Vector<double, Outputs>(
-                           const Eigen::Vector<double, Outputs>&,
-                           const Eigen::Vector<double, Outputs>&)>
-                           residualFuncY,
-                       std::function<Eigen::Vector<double, States>(
-                           const Eigen::Vector<double, States>&,
-                           const Eigen::Vector<double, States>&)>
-                           addFuncX,
-                       units::second_t dt)
-      : m_f(f), m_h(h), m_residualFuncY(residualFuncY), m_addFuncX(addFuncX) {
-    m_contQ = MakeCovMatrix(stateStdDevs);
-    m_contR = MakeCovMatrix(measurementStdDevs);
-    m_dt = dt;
-
-    Reset();
-
-    Eigen::Matrix<double, States, States> contA =
-        NumericalJacobianX<States, States, Inputs>(
-            m_f, m_xHat, Eigen::Vector<double, Inputs>::Zero());
-    Eigen::Matrix<double, Outputs, States> C =
-        NumericalJacobianX<Outputs, States, Inputs>(
-            m_h, m_xHat, Eigen::Vector<double, Inputs>::Zero());
-
-    Eigen::Matrix<double, States, States> discA;
-    Eigen::Matrix<double, States, States> discQ;
-    DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
-
-    Eigen::Matrix<double, 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);
-    } else {
-      m_initP = Eigen::Matrix<double, States, States>::Zero();
-    }
-    m_P = m_initP;
-  }
+  ExtendedKalmanFilter(
+      std::function<StateVector(const StateVector&, const InputVector&)> f,
+      std::function<OutputVector(const StateVector&, const InputVector&)> h,
+      const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+      std::function<OutputVector(const OutputVector&, const OutputVector&)>
+          residualFuncY,
+      std::function<StateVector(const StateVector&, const StateVector&)>
+          addFuncX,
+      units::second_t dt);
 
   /**
    * Returns the error covariance matrix P.
    */
-  const Eigen::Matrix<double, States, States>& P() const { return m_P; }
+  const StateMatrix& P() const { return m_P; }
 
   /**
    * Returns an element of the error covariance matrix P.
@@ -185,26 +108,26 @@
    *
    * @param P The error covariance matrix P.
    */
-  void SetP(const Eigen::Matrix<double, States, States>& P) { m_P = P; }
+  void SetP(const StateMatrix& P) { m_P = P; }
 
   /**
    * Returns the state estimate x-hat.
    */
-  const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
+  const StateVector& Xhat() const { return m_xHat; }
 
   /**
    * Returns an element of the state estimate x-hat.
    *
    * @param i Row of x-hat.
    */
-  double Xhat(int i) const { return m_xHat(i, 0); }
+  double Xhat(int i) const { return m_xHat(i); }
 
   /**
    * Set initial state estimate x-hat.
    *
    * @param xHat The state estimate x-hat.
    */
-  void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
+  void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
 
   /**
    * Set an element of the initial state estimate x-hat.
@@ -212,7 +135,7 @@
    * @param i     Row of x-hat.
    * @param value Value for element of x-hat.
    */
-  void SetXhat(int i, double value) { m_xHat(i, 0) = value; }
+  void SetXhat(int i, double value) { m_xHat(i) = value; }
 
   /**
    * Resets the observer.
@@ -228,23 +151,7 @@
    * @param u  New control input from controller.
    * @param dt Timestep for prediction.
    */
-  void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
-    // Find continuous A
-    Eigen::Matrix<double, States, States> contA =
-        NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
-
-    // Find discrete A and Q
-    Eigen::Matrix<double, States, States> discA;
-    Eigen::Matrix<double, States, States> discQ;
-    DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
-
-    m_xHat = RK4(m_f, m_xHat, u, dt);
-
-    // Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
-    m_P = discA * m_P * discA.transpose() + discQ;
-
-    m_dt = dt;
-  }
+  void Predict(const InputVector& u, units::second_t dt);
 
   /**
    * Correct the state estimate x-hat using the measurements in y.
@@ -252,27 +159,15 @@
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    */
-  void Correct(const Eigen::Vector<double, Inputs>& u,
-               const Eigen::Vector<double, Outputs>& y) {
+  void Correct(const InputVector& u, const OutputVector& y) {
     Correct<Outputs>(u, y, m_h, m_contR, m_residualFuncY, m_addFuncX);
   }
 
   template <int Rows>
-  void Correct(const Eigen::Vector<double, Inputs>& u,
-               const Eigen::Vector<double, Rows>& y,
-               std::function<Eigen::Vector<double, Rows>(
-                   const Eigen::Vector<double, States>&,
-                   const Eigen::Vector<double, Inputs>&)>
-                   h,
-               const Eigen::Matrix<double, Rows, Rows>& R) {
-    auto residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Rows> {
-      return a - b;
-    };
-    auto addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
-      return a + b;
-    };
-    Correct<Rows>(u, y, h, R, residualFuncY, addFuncX);
-  }
+  void Correct(
+      const InputVector& u, const Vectord<Rows>& y,
+      std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+      const Matrixd<Rows, Rows>& R);
 
   /**
    * Correct the state estimate x-hat using the measurements in y.
@@ -291,73 +186,30 @@
    * @param addFuncX      A function that adds two state vectors.
    */
   template <int Rows>
-  void Correct(const Eigen::Vector<double, Inputs>& u,
-               const Eigen::Vector<double, Rows>& y,
-               std::function<Eigen::Vector<double, Rows>(
-                   const Eigen::Vector<double, States>&,
-                   const Eigen::Vector<double, Inputs>&)>
-                   h,
-               const Eigen::Matrix<double, Rows, Rows>& R,
-               std::function<Eigen::Vector<double, Rows>(
-                   const Eigen::Vector<double, Rows>&,
-                   const Eigen::Vector<double, Rows>&)>
-                   residualFuncY,
-               std::function<Eigen::Vector<double, States>(
-                   const Eigen::Vector<double, States>&,
-                   const Eigen::Vector<double, States>)>
-                   addFuncX) {
-    const Eigen::Matrix<double, Rows, States> C =
-        NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
-    const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
-
-    Eigen::Matrix<double, Rows, Rows> S = C * m_P * C.transpose() + discR;
-
-    // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
-    // efficiently.
-    //
-    // K = PCᵀS⁻¹
-    // KS = PCᵀ
-    // (KS)ᵀ = (PCᵀ)ᵀ
-    // SᵀKᵀ = CPᵀ
-    //
-    // The solution of Ax = b can be found via x = A.solve(b).
-    //
-    // Kᵀ = Sᵀ.solve(CPᵀ)
-    // K = (Sᵀ.solve(CPᵀ))ᵀ
-    Eigen::Matrix<double, States, Rows> K =
-        S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
-
-    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
-    m_xHat = addFuncX(m_xHat, K * residualFuncY(y, h(m_xHat, u)));
-
-    // Pₖ₊₁⁺ = (I − KC)Pₖ₊₁⁻
-    m_P = (Eigen::Matrix<double, States, States>::Identity() - K * C) * m_P;
-  }
+  void Correct(
+      const InputVector& u, const Vectord<Rows>& y,
+      std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+      const Matrixd<Rows, Rows>& R,
+      std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
+          residualFuncY,
+      std::function<StateVector(const StateVector&, const StateVector&)>
+          addFuncX);
 
  private:
-  std::function<Eigen::Vector<double, States>(
-      const Eigen::Vector<double, States>&,
-      const Eigen::Vector<double, Inputs>&)>
-      m_f;
-  std::function<Eigen::Vector<double, Outputs>(
-      const Eigen::Vector<double, States>&,
-      const Eigen::Vector<double, Inputs>&)>
-      m_h;
-  std::function<Eigen::Vector<double, Outputs>(
-      const Eigen::Vector<double, Outputs>&,
-      const Eigen::Vector<double, Outputs>)>
+  std::function<StateVector(const StateVector&, const InputVector&)> m_f;
+  std::function<OutputVector(const StateVector&, const InputVector&)> m_h;
+  std::function<OutputVector(const OutputVector&, const OutputVector&)>
       m_residualFuncY;
-  std::function<Eigen::Vector<double, States>(
-      const Eigen::Vector<double, States>&,
-      const Eigen::Vector<double, States>)>
-      m_addFuncX;
-  Eigen::Vector<double, States> m_xHat;
-  Eigen::Matrix<double, States, States> m_P;
-  Eigen::Matrix<double, States, States> m_contQ;
-  Eigen::Matrix<double, Outputs, Outputs> m_contR;
+  std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
+  StateVector m_xHat;
+  StateMatrix m_P;
+  StateMatrix m_contQ;
+  Matrixd<Outputs, Outputs> m_contR;
   units::second_t m_dt;
 
-  Eigen::Matrix<double, States, States> m_initP;
+  StateMatrix m_initP;
 };
 
 }  // namespace frc
+
+#include "ExtendedKalmanFilter.inc"
diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
new file mode 100644
index 0000000..a56b8b5
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
@@ -0,0 +1,160 @@
+// 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/Cholesky"
+#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/ExtendedKalmanFilter.h"
+#include "frc/system/Discretization.h"
+#include "frc/system/NumericalIntegration.h"
+#include "frc/system/NumericalJacobian.h"
+
+namespace frc {
+
+template <int States, int Inputs, int Outputs>
+ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
+    std::function<StateVector(const StateVector&, const InputVector&)> f,
+    std::function<OutputVector(const StateVector&, const InputVector&)> h,
+    const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+    units::second_t dt)
+    : m_f(f), m_h(h) {
+  m_contQ = MakeCovMatrix(stateStdDevs);
+  m_contR = MakeCovMatrix(measurementStdDevs);
+  m_residualFuncY = [](auto a, auto b) -> OutputVector { return a - b; };
+  m_addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
+  m_dt = dt;
+
+  Reset();
+
+  StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
+      m_f, m_xHat, InputVector::Zero());
+  Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
+      m_h, m_xHat, InputVector::Zero());
+
+  StateMatrix discA;
+  StateMatrix discQ;
+  DiscretizeAQTaylor<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);
+  } else {
+    m_initP = StateMatrix::Zero();
+  }
+  m_P = m_initP;
+}
+
+template <int States, int Inputs, int Outputs>
+ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
+    std::function<StateVector(const StateVector&, const InputVector&)> f,
+    std::function<OutputVector(const StateVector&, const InputVector&)> h,
+    const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+    std::function<OutputVector(const OutputVector&, const OutputVector&)>
+        residualFuncY,
+    std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
+    units::second_t dt)
+    : m_f(f), m_h(h), m_residualFuncY(residualFuncY), m_addFuncX(addFuncX) {
+  m_contQ = MakeCovMatrix(stateStdDevs);
+  m_contR = MakeCovMatrix(measurementStdDevs);
+  m_dt = dt;
+
+  Reset();
+
+  StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
+      m_f, m_xHat, InputVector::Zero());
+  Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
+      m_h, m_xHat, InputVector::Zero());
+
+  StateMatrix discA;
+  StateMatrix discQ;
+  DiscretizeAQTaylor<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);
+  } else {
+    m_initP = StateMatrix::Zero();
+  }
+  m_P = m_initP;
+}
+
+template <int States, int Inputs, int Outputs>
+void ExtendedKalmanFilter<States, Inputs, Outputs>::Predict(
+    const InputVector& u, units::second_t dt) {
+  // Find continuous A
+  StateMatrix contA =
+      NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
+
+  // Find discrete A and Q
+  StateMatrix discA;
+  StateMatrix discQ;
+  DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+
+  m_xHat = RK4(m_f, m_xHat, u, dt);
+
+  // Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
+  m_P = discA * m_P * discA.transpose() + discQ;
+
+  m_dt = dt;
+}
+
+template <int States, int Inputs, int Outputs>
+template <int Rows>
+void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
+    const InputVector& u, const Vectord<Rows>& y,
+    std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+    const Matrixd<Rows, Rows>& R) {
+  auto residualFuncY = [](auto a, auto b) -> Vectord<Rows> { return a - b; };
+  auto addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
+  Correct<Rows>(u, y, h, R, residualFuncY, addFuncX);
+}
+
+template <int States, int Inputs, int Outputs>
+template <int Rows>
+void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
+    const InputVector& u, const Vectord<Rows>& y,
+    std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+    const Matrixd<Rows, Rows>& R,
+    std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
+        residualFuncY,
+    std::function<StateVector(const StateVector&, const StateVector&)>
+        addFuncX) {
+  const Matrixd<Rows, States> C =
+      NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
+  const Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
+
+  Matrixd<Rows, Rows> S = C * m_P * C.transpose() + discR;
+
+  // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
+  // efficiently.
+  //
+  // K = PCᵀS⁻¹
+  // KS = PCᵀ
+  // (KS)ᵀ = (PCᵀ)ᵀ
+  // SᵀKᵀ = CPᵀ
+  //
+  // The solution of Ax = b can be found via x = A.solve(b).
+  //
+  // Kᵀ = Sᵀ.solve(CPᵀ)
+  // K = (Sᵀ.solve(CPᵀ))ᵀ
+  Matrixd<States, Rows> K =
+      S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
+
+  // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + Kₖ₊₁(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
+  m_xHat = addFuncX(m_xHat, K * residualFuncY(y, h(m_xHat, u)));
+
+  // Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
+  // Use Joseph form for numerical stability
+  m_P = (StateMatrix::Identity() - K * C) * m_P *
+            (StateMatrix::Identity() - K * C).transpose() +
+        K * discR * K.transpose();
+}
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
index 3aa4dbd..2121284 100644
--- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
@@ -4,25 +4,14 @@
 
 #pragma once
 
-#include <frc/fmt/Eigen.h>
-
-#include <cmath>
-#include <string>
-
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "Eigen/Cholesky"
-#include "Eigen/Core"
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-#include "frc/StateSpaceUtil.h"
-#include "frc/system/Discretization.h"
+#include "frc/EigenCore.h"
 #include "frc/system/LinearSystem.h"
 #include "units/time.h"
-#include "wpimath/MathShared.h"
 
 namespace frc {
-namespace detail {
 
 /**
  * A Kalman filter combines predictions from a model and measurements to give an
@@ -45,8 +34,15 @@
  * @tparam Outputs The number of outputs.
  */
 template <int States, int Inputs, int Outputs>
-class KalmanFilterImpl {
+class KalmanFilter {
  public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+  using OutputVector = Vectord<Outputs>;
+
+  using StateArray = wpi::array<double, States>;
+  using OutputArray = wpi::array<double, Outputs>;
+
   /**
    * Constructs a state-space observer with the given plant.
    *
@@ -54,65 +50,19 @@
    * @param stateStdDevs       Standard deviations of model states.
    * @param measurementStdDevs Standard deviations of measurements.
    * @param dt                 Nominal discretization timestep.
+   * @throws std::invalid_argument If the system is unobservable.
    */
-  KalmanFilterImpl(LinearSystem<States, Inputs, Outputs>& plant,
-                   const wpi::array<double, States>& stateStdDevs,
-                   const wpi::array<double, Outputs>& measurementStdDevs,
-                   units::second_t dt) {
-    m_plant = &plant;
+  KalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
+               const StateArray& stateStdDevs,
+               const OutputArray& measurementStdDevs, units::second_t dt);
 
-    auto contQ = MakeCovMatrix(stateStdDevs);
-    auto contR = MakeCovMatrix(measurementStdDevs);
-
-    Eigen::Matrix<double, States, States> discA;
-    Eigen::Matrix<double, States, States> discQ;
-    DiscretizeAQTaylor<States>(plant.A(), contQ, dt, &discA, &discQ);
-
-    auto discR = DiscretizeR<Outputs>(contR, dt);
-
-    const auto& C = plant.C();
-
-    if (!IsDetectable<States, Outputs>(discA, C)) {
-      std::string msg = fmt::format(
-          "The system passed to the Kalman filter is "
-          "unobservable!\n\nA =\n{}\nC =\n{}\n",
-          discA, C);
-
-      wpi::math::MathSharedStore::ReportError(msg);
-      throw std::invalid_argument(msg);
-    }
-
-    Eigen::Matrix<double, States, States> P =
-        drake::math::DiscreteAlgebraicRiccatiEquation(
-            discA.transpose(), C.transpose(), discQ, discR);
-
-    // S = CPCᵀ + R
-    Eigen::Matrix<double, Outputs, Outputs> S = C * P * C.transpose() + discR;
-
-    // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
-    // efficiently.
-    //
-    // K = PCᵀS⁻¹
-    // KS = PCᵀ
-    // (KS)ᵀ = (PCᵀ)ᵀ
-    // SᵀKᵀ = CPᵀ
-    //
-    // The solution of Ax = b can be found via x = A.solve(b).
-    //
-    // Kᵀ = Sᵀ.solve(CPᵀ)
-    // K = (Sᵀ.solve(CPᵀ))ᵀ
-    m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose();
-
-    Reset();
-  }
-
-  KalmanFilterImpl(KalmanFilterImpl&&) = default;
-  KalmanFilterImpl& operator=(KalmanFilterImpl&&) = default;
+  KalmanFilter(KalmanFilter&&) = default;
+  KalmanFilter& operator=(KalmanFilter&&) = default;
 
   /**
    * Returns the steady-state Kalman gain matrix K.
    */
-  const Eigen::Matrix<double, States, Outputs>& K() const { return m_K; }
+  const Matrixd<States, Outputs>& K() const { return m_K; }
 
   /**
    * Returns an element of the steady-state Kalman gain matrix K.
@@ -125,7 +75,7 @@
   /**
    * Returns the state estimate x-hat.
    */
-  const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
+  const StateVector& Xhat() const { return m_xHat; }
 
   /**
    * Returns an element of the state estimate x-hat.
@@ -139,7 +89,7 @@
    *
    * @param xHat The state estimate x-hat.
    */
-  void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
+  void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
 
   /**
    * Set an element of the initial state estimate x-hat.
@@ -160,9 +110,7 @@
    * @param u  New control input from controller.
    * @param dt Timestep for prediction.
    */
-  void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
-    m_xHat = m_plant->CalculateX(m_xHat, u, dt);
-  }
+  void Predict(const InputVector& u, units::second_t dt);
 
   /**
    * Correct the state estimate x-hat using the measurements in y.
@@ -170,11 +118,7 @@
    * @param u Same control input used in the last predict step.
    * @param y Measurement vector.
    */
-  void Correct(const Eigen::Vector<double, Inputs>& u,
-               const Eigen::Vector<double, Outputs>& y) {
-    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
-    m_xHat += m_K * (y - (m_plant->C() * m_xHat + m_plant->D() * u));
-  }
+  void Correct(const InputVector& u, const OutputVector& y);
 
  private:
   LinearSystem<States, Inputs, Outputs>* m_plant;
@@ -182,66 +126,19 @@
   /**
    * The steady-state Kalman gain matrix.
    */
-  Eigen::Matrix<double, States, Outputs> m_K;
+  Matrixd<States, Outputs> m_K;
 
   /**
    * The state estimate.
    */
-  Eigen::Vector<double, States> m_xHat;
+  StateVector m_xHat;
 };
 
-}  // namespace detail
-
-template <int States, int Inputs, int Outputs>
-class KalmanFilter : public detail::KalmanFilterImpl<States, Inputs, Outputs> {
- public:
-  /**
-   * Constructs a state-space observer with the given plant.
-   *
-   * @param plant              The plant used for the prediction step.
-   * @param stateStdDevs       Standard deviations of model states.
-   * @param measurementStdDevs Standard deviations of measurements.
-   * @param dt                 Nominal discretization timestep.
-   */
-  KalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
-               const wpi::array<double, States>& stateStdDevs,
-               const wpi::array<double, Outputs>& measurementStdDevs,
-               units::second_t dt)
-      : detail::KalmanFilterImpl<States, Inputs, Outputs>{
-            plant, stateStdDevs, measurementStdDevs, dt} {}
-
-  KalmanFilter(KalmanFilter&&) = default;
-  KalmanFilter& operator=(KalmanFilter&&) = default;
-};
-
-// Template specializations are used here to make common state-input-output
-// triplets compile faster.
-template <>
-class WPILIB_DLLEXPORT KalmanFilter<1, 1, 1>
-    : public detail::KalmanFilterImpl<1, 1, 1> {
- public:
-  KalmanFilter(LinearSystem<1, 1, 1>& plant,
-               const wpi::array<double, 1>& stateStdDevs,
-               const wpi::array<double, 1>& measurementStdDevs,
-               units::second_t dt);
-
-  KalmanFilter(KalmanFilter&&) = default;
-  KalmanFilter& operator=(KalmanFilter&&) = default;
-};
-
-// Template specializations are used here to make common state-input-output
-// triplets compile faster.
-template <>
-class WPILIB_DLLEXPORT KalmanFilter<2, 1, 1>
-    : public detail::KalmanFilterImpl<2, 1, 1> {
- public:
-  KalmanFilter(LinearSystem<2, 1, 1>& plant,
-               const wpi::array<double, 2>& stateStdDevs,
-               const wpi::array<double, 1>& measurementStdDevs,
-               units::second_t dt);
-
-  KalmanFilter(KalmanFilter&&) = default;
-  KalmanFilter& operator=(KalmanFilter&&) = default;
-};
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    KalmanFilter<1, 1, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    KalmanFilter<2, 1, 1>;
 
 }  // namespace frc
+
+#include "KalmanFilter.inc"
diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
new file mode 100644
index 0000000..ca4f37c
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
@@ -0,0 +1,86 @@
+// 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/fmt/Eigen.h>
+
+#include <cmath>
+#include <stdexcept>
+#include <string>
+
+#include "Eigen/Cholesky"
+#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/KalmanFilter.h"
+#include "frc/system/Discretization.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+template <int States, int Inputs, int Outputs>
+KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
+    LinearSystem<States, Inputs, Outputs>& plant,
+    const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+    units::second_t dt) {
+  m_plant = &plant;
+
+  auto contQ = MakeCovMatrix(stateStdDevs);
+  auto contR = MakeCovMatrix(measurementStdDevs);
+
+  Matrixd<States, States> discA;
+  Matrixd<States, States> discQ;
+  DiscretizeAQTaylor<States>(plant.A(), contQ, dt, &discA, &discQ);
+
+  auto discR = DiscretizeR<Outputs>(contR, dt);
+
+  const auto& C = plant.C();
+
+  if (!IsDetectable<States, Outputs>(discA, C)) {
+    std::string msg = fmt::format(
+        "The system passed to the Kalman filter is "
+        "unobservable!\n\nA =\n{}\nC =\n{}\n",
+        discA, C);
+
+    wpi::math::MathSharedStore::ReportError(msg);
+    throw std::invalid_argument(msg);
+  }
+
+  Matrixd<States, States> P = drake::math::DiscreteAlgebraicRiccatiEquation(
+      discA.transpose(), C.transpose(), discQ, discR);
+
+  // S = CPCᵀ + R
+  Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
+
+  // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
+  // efficiently.
+  //
+  // K = PCᵀS⁻¹
+  // KS = PCᵀ
+  // (KS)ᵀ = (PCᵀ)ᵀ
+  // SᵀKᵀ = CPᵀ
+  //
+  // The solution of Ax = b can be found via x = A.solve(b).
+  //
+  // Kᵀ = Sᵀ.solve(CPᵀ)
+  // K = (Sᵀ.solve(CPᵀ))ᵀ
+  m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose();
+
+  Reset();
+}
+
+template <int States, int Inputs, int Outputs>
+void KalmanFilter<States, Inputs, Outputs>::Predict(const InputVector& u,
+                                                    units::second_t dt) {
+  m_xHat = m_plant->CalculateX(m_xHat, u, dt);
+}
+
+template <int States, int Inputs, int Outputs>
+void KalmanFilter<States, Inputs, Outputs>::Correct(const InputVector& u,
+                                                    const OutputVector& y) {
+  // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
+  m_xHat += m_K * (y - (m_plant->C() * m_xHat + m_plant->D() * u));
+}
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
index 5cb8eb2..d6e4127 100644
--- a/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
@@ -10,7 +10,7 @@
 #include <utility>
 #include <vector>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "units/math.h"
 #include "units/time.h"
 
@@ -20,16 +20,15 @@
 class KalmanFilterLatencyCompensator {
  public:
   struct ObserverSnapshot {
-    Eigen::Vector<double, States> xHat;
-    Eigen::Matrix<double, States, States> errorCovariances;
-    Eigen::Vector<double, Inputs> inputs;
-    Eigen::Vector<double, Outputs> localMeasurements;
+    Vectord<States> xHat;
+    Matrixd<States, States> squareRootErrorCovariances;
+    Vectord<Inputs> inputs;
+    Vectord<Outputs> localMeasurements;
 
-    ObserverSnapshot(const KalmanFilterType& observer,
-                     const Eigen::Vector<double, Inputs>& u,
-                     const Eigen::Vector<double, Outputs>& localY)
+    ObserverSnapshot(const KalmanFilterType& observer, const Vectord<Inputs>& u,
+                     const Vectord<Outputs>& localY)
         : xHat(observer.Xhat()),
-          errorCovariances(observer.P()),
+          squareRootErrorCovariances(observer.S()),
           inputs(u),
           localMeasurements(localY) {}
   };
@@ -47,10 +46,8 @@
    * @param localY    The local output at the timestamp
    * @param timestamp The timesnap of the state.
    */
-  void AddObserverState(const KalmanFilterType& observer,
-                        Eigen::Vector<double, Inputs> u,
-                        Eigen::Vector<double, Outputs> localY,
-                        units::second_t timestamp) {
+  void AddObserverState(const KalmanFilterType& observer, Vectord<Inputs> u,
+                        Vectord<Outputs> localY, units::second_t timestamp) {
     // Add the new state into the vector.
     m_pastObserverSnapshots.emplace_back(timestamp,
                                          ObserverSnapshot{observer, u, localY});
@@ -74,10 +71,8 @@
    */
   template <int Rows>
   void ApplyPastGlobalMeasurement(
-      KalmanFilterType* observer, units::second_t nominalDt,
-      Eigen::Vector<double, Rows> y,
-      std::function<void(const Eigen::Vector<double, Inputs>& u,
-                         const Eigen::Vector<double, Rows>& y)>
+      KalmanFilterType* observer, units::second_t nominalDt, Vectord<Rows> y,
+      std::function<void(const Vectord<Inputs>& u, const Vectord<Rows>& y)>
           globalMeasurementCorrect,
       units::second_t timestamp) {
     if (m_pastObserverSnapshots.size() == 0) {
@@ -140,7 +135,7 @@
       auto& [key, snapshot] = m_pastObserverSnapshots[i];
 
       if (i == indexOfClosestEntry) {
-        observer->SetP(snapshot.errorCovariances);
+        observer->SetS(snapshot.squareRootErrorCovariances);
         observer->SetXhat(snapshot.xHat);
       }
 
diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
index e9817ef..d1967e9 100644
--- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
@@ -9,89 +9,82 @@
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "Eigen/Core"
-#include "frc/estimator/KalmanFilterLatencyCompensator.h"
-#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/EigenCore.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
+#include "frc/interpolation/TimeInterpolatableBuffer.h"
 #include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveOdometry.h"
 #include "units/time.h"
 
 namespace frc {
 /**
- * This class wraps an Unscented Kalman Filter to fuse latency-compensated
+ * This class wraps Mecanum Drive Odometry to fuse latency-compensated
  * vision measurements with mecanum drive encoder velocity measurements. It will
  * correct for noisy measurements and encoder drift. It is intended to be an
- * easy but more accurate drop-in for MecanumDriveOdometry.
+ * easy drop-in for MecanumDriveOdometry.
  *
  * Update() should be called every robot loop. If your loops are faster or
- * slower than the default of 0.02s, then you should change the nominal delta
+ * slower than the default of 20 ms, then you should change the nominal delta
  * time by specifying it in the constructor.
  *
  * AddVisionMeasurement() can be called as infrequently as you want; if you
  * never call it, then this class will behave mostly like regular encoder
  * odometry.
- *
- * The state-space system used internally has the following states (x), inputs
- * (u), and outputs (y):
- *
- * <strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system
- * containing x position, y position, and heading.
- *
- * <strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity,
- * and angular velocity in the field coordinate system.
- *
- * <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
- * position, and heading; or <strong> y = [theta]ᵀ </strong> containing gyro
- * heading.
  */
 class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
  public:
   /**
-   * Constructs a MecanumDrivePoseEstimator.
+   * Constructs a MecanumDrivePoseEstimator with default standard deviations
+   * for the model and vision measurements.
    *
-   * @param gyroAngle                The current gyro angle.
-   * @param initialPose              The starting pose estimate.
-   * @param kinematics               A correctly-configured kinematics object
-   *                                 for your drivetrain.
-   * @param stateStdDevs             Standard deviations of model states.
-   *                                 Increase these numbers to trust your
-   *                                 model's state estimates less. This matrix
-   *                                 is in the form [x, y, theta]ᵀ, with units
-   *                                 in meters and radians.
-   * @param localMeasurementStdDevs  Standard deviations of the encoder and gyro
-   *                                 measurements. Increase these numbers to
-   *                                 trust sensor readings from encoders
-   *                                 and gyros less. This matrix is in the form
-   *                                 [theta], with units in radians.
-   * @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.
-   * @param nominalDt                The time in seconds between each robot
-   *                                 loop.
+   * The default standard deviations of the model states are
+   * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.
+   * The default standard deviations of the vision measurements are
+   * 0.45 meters for x, 0.45 meters for y, and 0.45 radians for heading.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distance measured by each wheel.
+   * @param initialPose The starting pose estimate.
    */
-  MecanumDrivePoseEstimator(
-      const Rotation2d& gyroAngle, const Pose2d& initialPose,
-      MecanumDriveKinematics kinematics,
-      const wpi::array<double, 3>& stateStdDevs,
-      const wpi::array<double, 1>& localMeasurementStdDevs,
-      const wpi::array<double, 3>& visionMeasurementStdDevs,
-      units::second_t nominalDt = 0.02_s);
+  MecanumDrivePoseEstimator(MecanumDriveKinematics& kinematics,
+                            const Rotation2d& gyroAngle,
+                            const MecanumDriveWheelPositions& wheelPositions,
+                            const Pose2d& initialPose);
 
   /**
-   * Sets the pose estimator's trust of global measurements. This might be used
+   * Constructs a MecanumDrivePoseEstimator.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distance measured by each wheel.
+   * @param initialPose The starting pose estimate.
+   * @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.
+   */
+  MecanumDrivePoseEstimator(
+      MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+      const MecanumDriveWheelPositions& wheelPositions,
+      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
-   *                                 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.
+   * @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);
@@ -99,75 +92,74 @@
   /**
    * Resets the robot's position on the field.
    *
-   * <p>You NEED to reset your encoders (to zero) when calling this method.
-   *
-   * <p>The gyroscope angle does not need to be reset in the user's robot code.
+   * 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 pose      The position on the field that your robot is at.
    * @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 Pose2d& pose, const Rotation2d& gyroAngle);
+  void ResetPosition(const Rotation2d& gyroAngle,
+                     const MecanumDriveWheelPositions& wheelPositions,
+                     const Pose2d& pose);
 
   /**
-   * Gets the pose of the robot at the current time as estimated by the Extended
-   * Kalman Filter.
+   * Gets the estimated robot pose.
    *
    * @return The estimated robot pose in meters.
    */
   Pose2d GetEstimatedPosition() const;
 
   /**
-   * Add a vision measurement to the Unscented Kalman Filter. This will correct
+   * 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 Timer#GetFPGATimestamp.) This means
-   *                        that you should use Timer#GetFPGATimestamp as your
-   *                        time source or sync the epochs.
+   *     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 Unscented Kalman Filter. This will correct
+   * 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
-   *                                 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.
+   * @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,
@@ -177,57 +169,79 @@
   }
 
   /**
-   * Updates the the Unscented Kalman Filter using only wheel encoder
-   * information. This should be called every loop, and the correct loop period
-   * must be passed into the constructor of this class.
+   * Updates the pose estimator with wheel encoder and gyro information. This
+   * should be called every loop.
    *
    * @param gyroAngle   The current gyro angle.
-   * @param wheelSpeeds The current speeds of the mecanum drive wheels.
+   * @param wheelPositions The distances measured at each wheel.
    * @return The estimated pose of the robot in meters.
    */
   Pose2d Update(const Rotation2d& gyroAngle,
-                const MecanumDriveWheelSpeeds& wheelSpeeds);
+                const MecanumDriveWheelPositions& wheelPositions);
 
   /**
-   * Updates the the Unscented Kalman Filter using only wheel encoder
-   * information. This should be called every loop, and the correct loop period
-   * must be passed into the constructor of this class.
+   * 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 wheelSpeeds The current speeds of the mecanum drive wheels.
+   * @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 MecanumDriveWheelSpeeds& wheelSpeeds);
+                        const MecanumDriveWheelPositions& wheelPositions);
 
  private:
-  UnscentedKalmanFilter<3, 3, 1> m_observer;
-  MecanumDriveKinematics m_kinematics;
-  KalmanFilterLatencyCompensator<3, 3, 1, UnscentedKalmanFilter<3, 3, 1>>
-      m_latencyCompensator;
-  std::function<void(const Eigen::Vector<double, 3>& u,
-                     const Eigen::Vector<double, 3>& y)>
-      m_visionCorrect;
+  struct InterpolationRecord {
+    // The pose observed given the current sensor inputs and the previous pose.
+    Pose2d pose;
 
-  Eigen::Matrix3d m_visionContR;
+    // The current gyroscope angle.
+    Rotation2d gyroAngle;
 
-  units::second_t m_nominalDt;
-  units::second_t m_prevTime = -1_s;
+    // The distances measured at each wheel.
+    MecanumDriveWheelPositions wheelPositions;
 
-  Rotation2d m_gyroOffset;
-  Rotation2d m_previousAngle;
+    /**
+     * 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;
 
-  template <int Dim>
-  static wpi::array<double, Dim> StdDevMatrixToArray(
-      const Eigen::Vector<double, Dim>& vector) {
-    wpi::array<double, Dim> array;
-    for (size_t i = 0; i < Dim; ++i) {
-      array[i] = vector(i);
-    }
-    return array;
-  }
+    /**
+     * 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);
+      }};
 };
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
index 42f5593..b2e0e45 100644
--- a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
+++ b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
@@ -6,8 +6,7 @@
 
 #include <cmath>
 
-#include "Eigen/Cholesky"
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 
 namespace frc {
 
@@ -52,24 +51,23 @@
 
   /**
    * Computes the sigma points for an unscented Kalman filter given the mean
-   * (x) and covariance(P) of the filter.
+   * (x) and square-root covariance(S) of the filter.
    *
    * @param x An array of the means.
-   * @param P Covariance of the filter.
+   * @param S Square-root covariance of the filter.
    *
    * @return Two dimensional array of sigma points. Each column contains all of
    *         the sigmas for one dimension in the problem space. Ordered by
    *         Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
    *
    */
-  Eigen::Matrix<double, States, 2 * States + 1> SigmaPoints(
-      const Eigen::Vector<double, States>& x,
-      const Eigen::Matrix<double, States, States>& P) {
+  Matrixd<States, 2 * States + 1> SquareRootSigmaPoints(
+      const Vectord<States>& x, const Matrixd<States, States>& S) {
     double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
-    Eigen::Matrix<double, States, States> U =
-        ((lambda + States) * P).llt().matrixL();
+    double eta = std::sqrt(lambda + States);
+    Matrixd<States, States> U = eta * S;
 
-    Eigen::Matrix<double, States, 2 * States + 1> sigmas;
+    Matrixd<States, 2 * States + 1> sigmas;
     sigmas.template block<States, 1>(0, 0) = x;
     for (int k = 0; k < States; ++k) {
       sigmas.template block<States, 1>(0, k + 1) =
@@ -84,7 +82,7 @@
   /**
    * Returns the weight for each sigma point for the mean.
    */
-  const Eigen::Vector<double, 2 * States + 1>& Wm() const { return m_Wm; }
+  const Vectord<2 * States + 1>& Wm() const { return m_Wm; }
 
   /**
    * Returns an element of the weight for each sigma point for the mean.
@@ -96,7 +94,7 @@
   /**
    * Returns the weight for each sigma point for the covariance.
    */
-  const Eigen::Vector<double, 2 * States + 1>& Wc() const { return m_Wc; }
+  const Vectord<2 * States + 1>& Wc() const { return m_Wc; }
 
   /**
    * Returns an element of the weight for each sigma point for the covariance.
@@ -106,8 +104,8 @@
   double Wc(int i) const { return m_Wc(i, 0); }
 
  private:
-  Eigen::Vector<double, 2 * States + 1> m_Wm;
-  Eigen::Vector<double, 2 * States + 1> m_Wc;
+  Vectord<2 * States + 1> m_Wm;
+  Vectord<2 * States + 1> m_Wc;
   double m_alpha;
   int m_kappa;
 
@@ -120,8 +118,8 @@
     double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
 
     double c = 0.5 / (States + lambda);
-    m_Wm = Eigen::Vector<double, 2 * States + 1>::Constant(c);
-    m_Wc = Eigen::Vector<double, 2 * States + 1>::Constant(c);
+    m_Wm = Vectord<2 * States + 1>::Constant(c);
+    m_Wc = Vectord<2 * States + 1>::Constant(c);
 
     m_Wm(0) = lambda / (States + lambda);
     m_Wc(0) = lambda / (States + lambda) + (1 - std::pow(m_alpha, 2) + beta);
diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
index 7422a3c..d07bafe 100644
--- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
@@ -4,224 +4,240 @@
 
 #pragma once
 
-#include <limits>
+#include <cmath>
 
+#include <fmt/format.h>
+#include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 #include <wpi/timestamp.h>
 
-#include "Eigen/Core"
-#include "frc/StateSpaceUtil.h"
-#include "frc/estimator/AngleStatistics.h"
-#include "frc/estimator/KalmanFilterLatencyCompensator.h"
-#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/EigenCore.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 "units/time.h"
 
 namespace frc {
+
 /**
- * This class wraps an Unscented Kalman Filter to fuse latency-compensated
- * vision measurements with swerve drive encoder velocity measurements. It will
- * correct for noisy measurements and encoder drift. It is intended to be an
- * easy but more accurate drop-in for SwerveDriveOdometry.
+ * This class wraps Swerve Drive Odometry to fuse latency-compensated
+ * vision measurements with swerve drive encoder distance measurements. It is
+ * intended to be a drop-in for SwerveDriveOdometry.
  *
- * Update() should be called every robot loop. If your loops are faster or
- * slower than the default of 0.02s, then you should change the nominal delta
- * time by specifying it in the constructor.
+ * 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 mostly like regular encoder
+ * never call it, then this class will behave as regular encoder
  * odometry.
- *
- * The state-space system used internally has the following states (x), inputs
- * (u), and outputs (y):
- *
- * <strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system
- * containing x position, y position, and heading.
- *
- * <strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity,
- * and angular velocity in the field coordinate system.
- *
- * <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
- * position, and heading; or <strong> y = [theta]ᵀ </strong> containing gyro
- * heading.
  */
 template <size_t NumModules>
 class SwerveDrivePoseEstimator {
  public:
   /**
-   * Constructs a SwerveDrivePoseEstimator.
+   * Constructs a SwerveDrivePoseEstimator with default standard deviations
+   * for the model and vision measurements.
    *
-   * @param gyroAngle                The current gyro angle.
-   * @param initialPose              The starting pose estimate.
-   * @param kinematics               A correctly-configured kinematics object
-   *                                 for your drivetrain.
-   * @param stateStdDevs             Standard deviations of model states.
-   *                                 Increase these numbers to trust your
-   *                                 model's state estimates less. This matrix
-   *                                 is in the form [x, y, theta]ᵀ, with units
-   *                                 in meters and radians.
-   * @param localMeasurementStdDevs  Standard deviations of the encoder and gyro
-   *                                 measurements. Increase these numbers to
-   *                                 trust sensor readings from encoders
-   *                                 and gyros less. This matrix is in the form
-   *                                 [theta], with units in radians.
-   * @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.
-   * @param nominalDt                The time in seconds between each robot
-   *                                 loop.
+   * The default standard deviations of the model states are
+   * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.
+   * The default standard deviations of the vision measurements are
+   * 0.9 meters for x, 0.9 meters for y, and 0.9 radians for heading.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param modulePositions The current distance and rotation measurements of
+   *     the swerve modules.
+   * @param initialPose The starting pose estimate.
    */
   SwerveDrivePoseEstimator(
-      const Rotation2d& gyroAngle, const Pose2d& initialPose,
       SwerveDriveKinematics<NumModules>& kinematics,
-      const wpi::array<double, 3>& stateStdDevs,
-      const wpi::array<double, 1>& localMeasurementStdDevs,
-      const wpi::array<double, 3>& visionMeasurementStdDevs,
-      units::second_t nominalDt = 0.02_s)
-      : m_observer([](const Eigen::Vector<double, 3>& x,
-                      const Eigen::Vector<double, 3>& u) { return u; },
-                   [](const Eigen::Vector<double, 3>& x,
-                      const Eigen::Vector<double, 3>& u) {
-                     return x.block<1, 1>(2, 0);
-                   },
-                   stateStdDevs, localMeasurementStdDevs,
-                   frc::AngleMean<3, 3>(2), frc::AngleMean<1, 3>(0),
-                   frc::AngleResidual<3>(2), frc::AngleResidual<1>(0),
-                   frc::AngleAdd<3>(2), nominalDt),
-        m_kinematics(kinematics),
-        m_nominalDt(nominalDt) {
+      const Rotation2d& gyroAngle,
+      const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
+      const Pose2d& initialPose)
+      : SwerveDrivePoseEstimator{kinematics,      gyroAngle,
+                                 modulePositions, initialPose,
+                                 {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}} {}
+
+  /**
+   * Constructs a SwerveDrivePoseEstimator.
+   *
+   * @param kinematics A correctly-configured kinematics object for your
+   *     drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param modulePositions The current distance and rotation measurements of
+   *     the swerve modules.
+   * @param initialPose The starting pose estimate.
+   * @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.
+   */
+  SwerveDrivePoseEstimator(
+      SwerveDriveKinematics<NumModules>& kinematics,
+      const Rotation2d& gyroAngle,
+      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);
-
-    // Create correction mechanism for vision measurements.
-    m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
-                          const Eigen::Vector<double, 3>& y) {
-      m_observer.Correct<3>(
-          u, y,
-          [](const Eigen::Vector<double, 3>& x,
-             const Eigen::Vector<double, 3>& u) { return x; },
-          m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
-          frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
-    };
-
-    // Set initial state.
-    m_observer.SetXhat(PoseTo3dVector(initialPose));
-
-    // Calculate offsets.
-    m_gyroOffset = initialPose.Rotation() - gyroAngle;
-    m_previousAngle = initialPose.Rotation();
   }
 
   /**
    * Resets the robot's position on the field.
    *
-   * You NEED to reset your encoders (to zero) when calling this method.
-   *
    * 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 pose      The position on the field that your robot is at.
-   * @param gyroAngle The angle reported by the gyroscope.
+   * @param gyroAngle       The angle reported by the gyroscope.
+   * @param modulePositions The current distance and rotation measurements of
+   *                        the swerve modules.
+   * @param pose            The position on the field that your robot is at.
    */
-  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
+  void ResetPosition(
+      const Rotation2d& gyroAngle,
+      const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
+      const Pose2d& pose) {
     // Reset state estimate and error covariance
-    m_observer.Reset();
-    m_latencyCompensator.Reset();
-
-    m_observer.SetXhat(PoseTo3dVector(pose));
-
-    m_gyroOffset = pose.Rotation() - gyroAngle;
-    m_previousAngle = pose.Rotation();
+    m_odometry.ResetPosition(gyroAngle, modulePositions, pose);
+    m_poseBuffer.Clear();
   }
 
   /**
-   * Gets the pose of the robot at the current time as estimated by the Extended
-   * Kalman Filter.
+   * Gets the estimated robot pose.
    *
    * @return The estimated robot pose in meters.
    */
-  Pose2d GetEstimatedPosition() const {
-    return Pose2d(m_observer.Xhat(0) * 1_m, m_observer.Xhat(1) * 1_m,
-                  Rotation2d(units::radian_t{m_observer.Xhat(2)}));
-  }
+  Pose2d GetEstimatedPosition() const { return m_odometry.GetPose(); }
 
   /**
-   * Sets the pose estimator's trust of global measurements. This might be used
+   * 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
-   *                                 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.
+   * @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) {
-    // Create R (covariances) for vision measurements.
-    m_visionContR = frc::MakeCovMatrix(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]));
+      }
+    }
   }
 
   /**
-   * Add a vision measurement to the Unscented Kalman Filter. This will correct
-   * the odometry pose estimate while still accounting for measurement noise.
+   * 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 Timer#GetFPGATimestamp.) This means
-   *                        that you should use Timer#GetFPGATimestamp as your
-   *                        time source or sync the epochs.
+   *    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) {
-    m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
-        &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
-        m_visionCorrect, 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 Unscented Kalman Filter. This will correct
-   * the odometry pose estimate while still accounting for measurement noise.
+   * 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
-   *                                 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.
+   * @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,
@@ -231,87 +247,140 @@
   }
 
   /**
-   * Updates the the Unscented Kalman Filter using only wheel encoder
-   * information. This should be called every loop, and the correct loop period
-   * must be passed into the constructor of this class.
+   * Updates the pose estimator with wheel encoder and gyro information. This
+   * should be called every loop.
    *
-   * @param gyroAngle    The current gyro angle.
-   * @param moduleStates The current velocities and rotations of the swerve
-   *                     modules.
-   * @return The estimated pose of the robot in meters.
+   * @param gyroAngle       The current gyro angle.
+   * @param modulePositions The current distance and rotation measurements of
+   *                        the swerve modules.
+   * @return The estimated robot pose in meters.
    */
-  template <typename... ModuleState>
-  Pose2d Update(const Rotation2d& gyroAngle, ModuleState&&... moduleStates) {
+  Pose2d Update(
+      const Rotation2d& gyroAngle,
+      const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
     return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
-                          moduleStates...);
+                          modulePositions);
   }
 
   /**
-   * Updates the the Unscented Kalman Filter using only wheel encoder
-   * information. This should be called every loop, and the correct loop period
-   * must be passed into the constructor of this class.
+   * 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 moduleStates The current velocities and rotations of the swerve
-   *                     modules.
-   * @return The estimated pose of the robot in meters.
+   * @param currentTime     Time at which this method was called, in seconds.
+   * @param gyroAngle       The current gyro angle.
+   * @param modulePositions The current distance traveled and rotations of
+   *                        the swerve modules.
+   * @return The estimated robot pose in meters.
    */
-  template <typename... ModuleState>
-  Pose2d UpdateWithTime(units::second_t currentTime,
-                        const Rotation2d& gyroAngle,
-                        ModuleState&&... moduleStates) {
-    auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
-    m_prevTime = currentTime;
+  Pose2d UpdateWithTime(
+      units::second_t currentTime, const Rotation2d& gyroAngle,
+      const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
+    m_odometry.Update(gyroAngle, modulePositions);
 
-    auto angle = gyroAngle + m_gyroOffset;
-    auto omega = (angle - m_previousAngle).Radians() / dt;
+    wpi::array<SwerveModulePosition, NumModules> internalModulePositions{
+        wpi::empty_array};
 
-    auto chassisSpeeds = m_kinematics.ToChassisSpeeds(moduleStates...);
-    auto fieldRelativeSpeeds =
-        Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
-            .RotateBy(angle);
+    for (size_t i = 0; i < NumModules; i++) {
+      internalModulePositions[i].distance = modulePositions[i].distance;
+      internalModulePositions[i].angle = modulePositions[i].angle;
+    }
 
-    Eigen::Vector<double, 3> u{fieldRelativeSpeeds.X().value(),
-                               fieldRelativeSpeeds.Y().value(), omega.value()};
-
-    Eigen::Vector<double, 1> localY{angle.Radians().value()};
-    m_previousAngle = angle;
-
-    m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
-
-    m_observer.Predict(u, dt);
-    m_observer.Correct(u, localY);
+    m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle,
+                                         internalModulePositions});
 
     return GetEstimatedPosition();
   }
 
  private:
-  UnscentedKalmanFilter<3, 3, 1> m_observer;
-  SwerveDriveKinematics<NumModules>& m_kinematics;
-  KalmanFilterLatencyCompensator<3, 3, 1, UnscentedKalmanFilter<3, 3, 1>>
-      m_latencyCompensator;
-  std::function<void(const Eigen::Vector<double, 3>& u,
-                     const Eigen::Vector<double, 3>& y)>
-      m_visionCorrect;
+  struct InterpolationRecord {
+    // The pose observed given the current sensor inputs and the previous pose.
+    Pose2d pose;
 
-  Eigen::Matrix3d m_visionContR;
+    // The current gyroscope angle.
+    Rotation2d gyroAngle;
 
-  units::second_t m_nominalDt;
-  units::second_t m_prevTime = -1_s;
+    // The distances traveled and rotations meaured at each module.
+    wpi::array<SwerveModulePosition, NumModules> modulePostions;
 
-  Rotation2d m_gyroOffset;
-  Rotation2d m_previousAngle;
+    /**
+     * 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;
 
-  template <int Dim>
-  static wpi::array<double, Dim> StdDevMatrixToArray(
-      const Eigen::Vector<double, Dim>& vector) {
-    wpi::array<double, Dim> array;
-    for (size_t i = 0; i < Dim; ++i) {
-      array[i] = vector(i);
+    /**
+     * 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};
+      }
     }
-    return array;
-  }
+  };
+
+  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);
+      }};
 };
 
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    SwerveDrivePoseEstimator<4>;
+
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
index 3aa3e59..39ce615 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
@@ -6,16 +6,11 @@
 
 #include <functional>
 
+#include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "Eigen/Cholesky"
-#include "Eigen/Core"
-#include "frc/StateSpaceUtil.h"
+#include "frc/EigenCore.h"
 #include "frc/estimator/MerweScaledSigmaPoints.h"
-#include "frc/estimator/UnscentedTransform.h"
-#include "frc/system/Discretization.h"
-#include "frc/system/NumericalIntegration.h"
-#include "frc/system/NumericalJacobian.h"
 #include "units/time.h"
 
 namespace frc {
@@ -40,6 +35,10 @@
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
  * "Stochastic control theory".
  *
+ * <p> This class implements a square-root-form unscented Kalman filter
+ * (SR-UKF). For more information about the SR-UKF, see
+ * https://www.researchgate.net/publication/3908304.
+ *
  * @tparam States The number of states.
  * @tparam Inputs The number of inputs.
  * @tparam Outputs The number of outputs.
@@ -47,6 +46,15 @@
 template <int States, int Inputs, int Outputs>
 class UnscentedKalmanFilter {
  public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+  using OutputVector = Vectord<Outputs>;
+
+  using StateArray = wpi::array<double, States>;
+  using OutputArray = wpi::array<double, Outputs>;
+
+  using StateMatrix = Matrixd<States, States>;
+
   /**
    * Constructs an unscented Kalman filter.
    *
@@ -58,39 +66,11 @@
    * @param measurementStdDevs Standard deviations of measurements.
    * @param dt                 Nominal discretization timestep.
    */
-  UnscentedKalmanFilter(std::function<Eigen::Vector<double, States>(
-                            const Eigen::Vector<double, States>&,
-                            const Eigen::Vector<double, Inputs>&)>
-                            f,
-                        std::function<Eigen::Vector<double, Outputs>(
-                            const Eigen::Vector<double, States>&,
-                            const Eigen::Vector<double, Inputs>&)>
-                            h,
-                        const wpi::array<double, States>& stateStdDevs,
-                        const wpi::array<double, Outputs>& measurementStdDevs,
-                        units::second_t dt)
-      : m_f(f), m_h(h) {
-    m_contQ = MakeCovMatrix(stateStdDevs);
-    m_contR = MakeCovMatrix(measurementStdDevs);
-    m_meanFuncX = [](auto sigmas, auto Wm) -> Eigen::Vector<double, States> {
-      return sigmas * Wm;
-    };
-    m_meanFuncY = [](auto sigmas, auto Wc) -> Eigen::Vector<double, Outputs> {
-      return sigmas * Wc;
-    };
-    m_residualFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
-      return a - b;
-    };
-    m_residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Outputs> {
-      return a - b;
-    };
-    m_addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
-      return a + b;
-    };
-    m_dt = dt;
-
-    Reset();
-  }
+  UnscentedKalmanFilter(
+      std::function<StateVector(const StateVector&, const InputVector&)> f,
+      std::function<OutputVector(const StateVector&, const InputVector&)> h,
+      const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+      units::second_t dt);
 
   /**
    * Constructs an unscented Kalman filter with custom mean, residual, and
@@ -117,89 +97,74 @@
    * @param dt                 Nominal discretization timestep.
    */
   UnscentedKalmanFilter(
-      std::function<
-          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                        const Eigen::Vector<double, Inputs>&)>
-          f,
-      std::function<
-          Eigen::Vector<double, Outputs>(const Eigen::Vector<double, States>&,
-                                         const Eigen::Vector<double, Inputs>&)>
-          h,
-      const wpi::array<double, States>& stateStdDevs,
-      const wpi::array<double, Outputs>& measurementStdDevs,
-      std::function<Eigen::Vector<double, States>(
-          const Eigen::Matrix<double, States, 2 * States + 1>&,
-          const Eigen::Vector<double, 2 * States + 1>&)>
+      std::function<StateVector(const StateVector&, const InputVector&)> f,
+      std::function<OutputVector(const StateVector&, const InputVector&)> h,
+      const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+      std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
+                                const Vectord<2 * States + 1>&)>
           meanFuncX,
-      std::function<Eigen::Vector<double, Outputs>(
-          const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
-          const Eigen::Vector<double, 2 * States + 1>&)>
+      std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
+                                 const Vectord<2 * States + 1>&)>
           meanFuncY,
-      std::function<
-          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                        const Eigen::Vector<double, States>&)>
+      std::function<StateVector(const StateVector&, const StateVector&)>
           residualFuncX,
-      std::function<
-          Eigen::Vector<double, Outputs>(const Eigen::Vector<double, Outputs>&,
-                                         const Eigen::Vector<double, Outputs>&)>
+      std::function<OutputVector(const OutputVector&, const OutputVector&)>
           residualFuncY,
-      std::function<
-          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
-                                        const Eigen::Vector<double, States>&)>
+      std::function<StateVector(const StateVector&, const StateVector&)>
           addFuncX,
-      units::second_t dt)
-      : m_f(f),
-        m_h(h),
-        m_meanFuncX(meanFuncX),
-        m_meanFuncY(meanFuncY),
-        m_residualFuncX(residualFuncX),
-        m_residualFuncY(residualFuncY),
-        m_addFuncX(addFuncX) {
-    m_contQ = MakeCovMatrix(stateStdDevs);
-    m_contR = MakeCovMatrix(measurementStdDevs);
-    m_dt = dt;
-
-    Reset();
-  }
+      units::second_t dt);
 
   /**
-   * Returns the error covariance matrix P.
+   * Returns the square-root error covariance matrix S.
    */
-  const Eigen::Matrix<double, States, States>& P() const { return m_P; }
+  const StateMatrix& S() const { return m_S; }
 
   /**
-   * Returns an element of the error covariance matrix P.
+   * Returns an element of the square-root error covariance matrix S.
    *
-   * @param i Row of P.
-   * @param j Column of P.
+   * @param i Row of S.
+   * @param j Column of S.
    */
-  double P(int i, int j) const { return m_P(i, j); }
+  double S(int i, int j) const { return m_S(i, j); }
 
   /**
-   * Set the current error covariance matrix P.
+   * Set the current square-root error covariance matrix S.
+   *
+   * @param S The square-root error covariance matrix S.
+   */
+  void SetS(const StateMatrix& S) { m_S = S; }
+
+  /**
+   * Returns the reconstructed error covariance matrix P.
+   */
+  StateMatrix P() const { return m_S.transpose() * m_S; }
+
+  /**
+   * Set the current square-root error covariance matrix S by taking the square
+   * root of P.
    *
    * @param P The error covariance matrix P.
    */
-  void SetP(const Eigen::Matrix<double, States, States>& P) { m_P = P; }
+  void SetP(const StateMatrix& P) { m_S = P.llt().matrixU(); }
 
   /**
    * Returns the state estimate x-hat.
    */
-  const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
+  const StateVector& Xhat() const { return m_xHat; }
 
   /**
    * Returns an element of the state estimate x-hat.
    *
    * @param i Row of x-hat.
    */
-  double Xhat(int i) const { return m_xHat(i, 0); }
+  double Xhat(int i) const { return m_xHat(i); }
 
   /**
    * Set initial state estimate x-hat.
    *
    * @param xHat The state estimate x-hat.
    */
-  void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
+  void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
 
   /**
    * Set an element of the initial state estimate x-hat.
@@ -207,14 +172,14 @@
    * @param i     Row of x-hat.
    * @param value Value for element of x-hat.
    */
-  void SetXhat(int i, double value) { m_xHat(i, 0) = value; }
+  void SetXhat(int i, double value) { m_xHat(i) = value; }
 
   /**
    * Resets the observer.
    */
   void Reset() {
     m_xHat.setZero();
-    m_P.setZero();
+    m_S.setZero();
     m_sigmasF.setZero();
   }
 
@@ -224,31 +189,7 @@
    * @param u  New control input from controller.
    * @param dt Timestep for prediction.
    */
-  void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
-    m_dt = dt;
-
-    // Discretize Q before projecting mean and covariance forward
-    Eigen::Matrix<double, States, States> contA =
-        NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
-    Eigen::Matrix<double, States, States> discA;
-    Eigen::Matrix<double, States, States> discQ;
-    DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
-
-    Eigen::Matrix<double, States, 2 * States + 1> sigmas =
-        m_pts.SigmaPoints(m_xHat, m_P);
-
-    for (int i = 0; i < m_pts.NumSigmas(); ++i) {
-      Eigen::Vector<double, States> x = sigmas.template block<States, 1>(0, i);
-      m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
-    }
-
-    auto ret = UnscentedTransform<States, States>(
-        m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX);
-    m_xHat = std::get<0>(ret);
-    m_P = std::get<1>(ret);
-
-    m_P += discQ;
-  }
+  void Predict(const InputVector& u, units::second_t dt);
 
   /**
    * Correct the state estimate x-hat using the measurements in y.
@@ -256,8 +197,7 @@
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    */
-  void Correct(const Eigen::Vector<double, Inputs>& u,
-               const Eigen::Vector<double, Outputs>& y) {
+  void Correct(const InputVector& u, const OutputVector& y) {
     Correct<Outputs>(u, y, m_h, m_contR, m_meanFuncY, m_residualFuncY,
                      m_residualFuncX, m_addFuncX);
   }
@@ -276,28 +216,10 @@
    * @param R Measurement noise covariance matrix (continuous-time).
    */
   template <int Rows>
-  void Correct(const Eigen::Vector<double, Inputs>& u,
-               const Eigen::Vector<double, Rows>& y,
-               std::function<Eigen::Vector<double, Rows>(
-                   const Eigen::Vector<double, States>&,
-                   const Eigen::Vector<double, Inputs>&)>
-                   h,
-               const Eigen::Matrix<double, Rows, Rows>& R) {
-    auto meanFuncY = [](auto sigmas, auto Wc) -> Eigen::Vector<double, Rows> {
-      return sigmas * Wc;
-    };
-    auto residualFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
-      return a - b;
-    };
-    auto residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Rows> {
-      return a - b;
-    };
-    auto addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
-      return a + b;
-    };
-    Correct<Rows>(u, y, h, R, meanFuncY, residualFuncY, residualFuncX,
-                  addFuncX);
-  }
+  void Correct(
+      const InputVector& u, const Vectord<Rows>& y,
+      std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+      const Matrixd<Rows, Rows>& R);
 
   /**
    * Correct the state estimate x-hat using the measurements in y.
@@ -320,109 +242,49 @@
    * @param addFuncX      A function that adds two state vectors.
    */
   template <int Rows>
-  void Correct(const Eigen::Vector<double, Inputs>& u,
-               const Eigen::Vector<double, Rows>& y,
-               std::function<Eigen::Vector<double, Rows>(
-                   const Eigen::Vector<double, States>&,
-                   const Eigen::Vector<double, Inputs>&)>
-                   h,
-               const Eigen::Matrix<double, Rows, Rows>& R,
-               std::function<Eigen::Vector<double, Rows>(
-                   const Eigen::Matrix<double, Rows, 2 * States + 1>&,
-                   const Eigen::Vector<double, 2 * States + 1>&)>
-                   meanFuncY,
-               std::function<Eigen::Vector<double, Rows>(
-                   const Eigen::Vector<double, Rows>&,
-                   const Eigen::Vector<double, Rows>&)>
-                   residualFuncY,
-               std::function<Eigen::Vector<double, States>(
-                   const Eigen::Vector<double, States>&,
-                   const Eigen::Vector<double, States>&)>
-                   residualFuncX,
-               std::function<Eigen::Vector<double, States>(
-                   const Eigen::Vector<double, States>&,
-                   const Eigen::Vector<double, States>)>
-                   addFuncX) {
-    const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
-
-    // Transform sigma points into measurement space
-    Eigen::Matrix<double, Rows, 2 * States + 1> sigmasH;
-    Eigen::Matrix<double, States, 2 * States + 1> sigmas =
-        m_pts.SigmaPoints(m_xHat, m_P);
-    for (int i = 0; i < m_pts.NumSigmas(); ++i) {
-      sigmasH.template block<Rows, 1>(0, i) =
-          h(sigmas.template block<States, 1>(0, i), u);
-    }
-
-    // Mean and covariance of prediction passed through UT
-    auto [yHat, Py] = UnscentedTransform<Rows, States>(
-        sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY);
-    Py += discR;
-
-    // Compute cross covariance of the state and the measurements
-    Eigen::Matrix<double, States, Rows> Pxy;
-    Pxy.setZero();
-    for (int i = 0; i < m_pts.NumSigmas(); ++i) {
-      // Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
-      Pxy +=
-          m_pts.Wc(i) *
-          (residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
-          (residualFuncY(sigmasH.template block<Rows, 1>(0, i), yHat))
-              .transpose();
-    }
-
-    // K = P_{xy} P_y⁻¹
-    // Kᵀ = P_yᵀ⁻¹ P_{xy}ᵀ
-    // P_yᵀKᵀ = P_{xy}ᵀ
-    // Kᵀ = P_yᵀ.solve(P_{xy}ᵀ)
-    // K = (P_yᵀ.solve(P_{xy}ᵀ)ᵀ
-    Eigen::Matrix<double, States, Rows> K =
-        Py.transpose().ldlt().solve(Pxy.transpose()).transpose();
-
-    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
-    m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
-
-    // Pₖ₊₁⁺ = Pₖ₊₁⁻ − KP_yKᵀ
-    m_P -= K * Py * K.transpose();
-  }
+  void Correct(
+      const InputVector& u, const Vectord<Rows>& y,
+      std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+      const Matrixd<Rows, Rows>& R,
+      std::function<Vectord<Rows>(const Matrixd<Rows, 2 * States + 1>&,
+                                  const Vectord<2 * States + 1>&)>
+          meanFuncY,
+      std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
+          residualFuncY,
+      std::function<StateVector(const StateVector&, const StateVector&)>
+          residualFuncX,
+      std::function<StateVector(const StateVector&, const StateVector&)>
+          addFuncX);
 
  private:
-  std::function<Eigen::Vector<double, States>(
-      const Eigen::Vector<double, States>&,
-      const Eigen::Vector<double, Inputs>&)>
-      m_f;
-  std::function<Eigen::Vector<double, Outputs>(
-      const Eigen::Vector<double, States>&,
-      const Eigen::Vector<double, Inputs>&)>
-      m_h;
-  std::function<Eigen::Vector<double, States>(
-      const Eigen::Matrix<double, States, 2 * States + 1>&,
-      const Eigen::Vector<double, 2 * States + 1>&)>
+  std::function<StateVector(const StateVector&, const InputVector&)> m_f;
+  std::function<OutputVector(const StateVector&, const InputVector&)> m_h;
+  std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
+                            const Vectord<2 * States + 1>&)>
       m_meanFuncX;
-  std::function<Eigen::Vector<double, Outputs>(
-      const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
-      const Eigen::Vector<double, 2 * States + 1>&)>
+  std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
+                             const Vectord<2 * States + 1>&)>
       m_meanFuncY;
-  std::function<Eigen::Vector<double, States>(
-      const Eigen::Vector<double, States>&,
-      const Eigen::Vector<double, States>&)>
+  std::function<StateVector(const StateVector&, const StateVector&)>
       m_residualFuncX;
-  std::function<Eigen::Vector<double, Outputs>(
-      const Eigen::Vector<double, Outputs>&,
-      const Eigen::Vector<double, Outputs>)>
+  std::function<OutputVector(const OutputVector&, const OutputVector&)>
       m_residualFuncY;
-  std::function<Eigen::Vector<double, States>(
-      const Eigen::Vector<double, States>&,
-      const Eigen::Vector<double, States>)>
-      m_addFuncX;
-  Eigen::Vector<double, States> m_xHat;
-  Eigen::Matrix<double, States, States> m_P;
-  Eigen::Matrix<double, States, States> m_contQ;
-  Eigen::Matrix<double, Outputs, Outputs> m_contR;
-  Eigen::Matrix<double, States, 2 * States + 1> m_sigmasF;
+  std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
+  StateVector m_xHat;
+  StateMatrix m_S;
+  StateMatrix m_contQ;
+  Matrixd<Outputs, Outputs> m_contR;
+  Matrixd<States, 2 * States + 1> m_sigmasF;
   units::second_t m_dt;
 
   MerweScaledSigmaPoints<States> m_pts;
 };
 
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    UnscentedKalmanFilter<3, 3, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    UnscentedKalmanFilter<5, 3, 3>;
+
 }  // namespace frc
+
+#include "UnscentedKalmanFilter.inc"
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
new file mode 100644
index 0000000..a6744bf
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
@@ -0,0 +1,174 @@
+// 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/Cholesky"
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/estimator/UnscentedTransform.h"
+#include "frc/system/Discretization.h"
+#include "frc/system/NumericalIntegration.h"
+#include "frc/system/NumericalJacobian.h"
+
+namespace frc {
+
+template <int States, int Inputs, int Outputs>
+UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
+    std::function<StateVector(const StateVector&, const InputVector&)> f,
+    std::function<OutputVector(const StateVector&, const InputVector&)> h,
+    const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+    units::second_t dt)
+    : m_f(f), m_h(h) {
+  m_contQ = MakeCovMatrix(stateStdDevs);
+  m_contR = MakeCovMatrix(measurementStdDevs);
+  m_meanFuncX = [](auto sigmas, auto Wm) -> StateVector { return sigmas * Wm; };
+  m_meanFuncY = [](auto sigmas, auto Wc) -> OutputVector {
+    return sigmas * Wc;
+  };
+  m_residualFuncX = [](auto a, auto b) -> StateVector { return a - b; };
+  m_residualFuncY = [](auto a, auto b) -> OutputVector { return a - b; };
+  m_addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
+  m_dt = dt;
+
+  Reset();
+}
+
+template <int States, int Inputs, int Outputs>
+UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
+    std::function<StateVector(const StateVector&, const InputVector&)> f,
+    std::function<OutputVector(const StateVector&, const InputVector&)> h,
+    const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
+    std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
+                              const Vectord<2 * States + 1>&)>
+        meanFuncX,
+    std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
+                               const Vectord<2 * States + 1>&)>
+        meanFuncY,
+    std::function<StateVector(const StateVector&, const StateVector&)>
+        residualFuncX,
+    std::function<OutputVector(const OutputVector&, const OutputVector&)>
+        residualFuncY,
+    std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
+    units::second_t dt)
+    : m_f(f),
+      m_h(h),
+      m_meanFuncX(meanFuncX),
+      m_meanFuncY(meanFuncY),
+      m_residualFuncX(residualFuncX),
+      m_residualFuncY(residualFuncY),
+      m_addFuncX(addFuncX) {
+  m_contQ = MakeCovMatrix(stateStdDevs);
+  m_contR = MakeCovMatrix(measurementStdDevs);
+  m_dt = dt;
+
+  Reset();
+}
+
+template <int States, int Inputs, int Outputs>
+void UnscentedKalmanFilter<States, Inputs, Outputs>::Predict(
+    const InputVector& u, units::second_t dt) {
+  m_dt = dt;
+
+  // Discretize Q before projecting mean and covariance forward
+  StateMatrix contA =
+      NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
+  StateMatrix discA;
+  StateMatrix discQ;
+  DiscretizeAQTaylor<States>(contA, m_contQ, m_dt, &discA, &discQ);
+  Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
+
+  Matrixd<States, 2 * States + 1> sigmas =
+      m_pts.SquareRootSigmaPoints(m_xHat, m_S);
+
+  for (int i = 0; i < m_pts.NumSigmas(); ++i) {
+    StateVector x = sigmas.template block<States, 1>(0, i);
+    m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
+  }
+
+  auto [xHat, S] = SquareRootUnscentedTransform<States, States>(
+      m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX,
+      discQ.template triangularView<Eigen::Lower>());
+  m_xHat = xHat;
+  m_S = S;
+}
+
+template <int States, int Inputs, int Outputs>
+template <int Rows>
+void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
+    const InputVector& u, const Vectord<Rows>& y,
+    std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+    const Matrixd<Rows, Rows>& R) {
+  auto meanFuncY = [](auto sigmas, auto Wc) -> Vectord<Rows> {
+    return sigmas * Wc;
+  };
+  auto residualFuncX = [](auto a, auto b) -> StateVector { return a - b; };
+  auto residualFuncY = [](auto a, auto b) -> Vectord<Rows> { return a - b; };
+  auto addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
+  Correct<Rows>(u, y, h, R, meanFuncY, residualFuncY, residualFuncX, addFuncX);
+}
+
+template <int States, int Inputs, int Outputs>
+template <int Rows>
+void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
+    const InputVector& u, const Vectord<Rows>& y,
+    std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
+    const Matrixd<Rows, Rows>& R,
+    std::function<Vectord<Rows>(const Matrixd<Rows, 2 * States + 1>&,
+                                const Vectord<2 * States + 1>&)>
+        meanFuncY,
+    std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
+        residualFuncY,
+    std::function<StateVector(const StateVector&, const StateVector&)>
+        residualFuncX,
+    std::function<StateVector(const StateVector&, const StateVector&)>
+        addFuncX) {
+  Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
+  Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discR);
+
+  // Transform sigma points into measurement space
+  Matrixd<Rows, 2 * States + 1> sigmasH;
+  Matrixd<States, 2 * States + 1> sigmas =
+      m_pts.SquareRootSigmaPoints(m_xHat, m_S);
+  for (int i = 0; i < m_pts.NumSigmas(); ++i) {
+    sigmasH.template block<Rows, 1>(0, i) =
+        h(sigmas.template block<States, 1>(0, i), u);
+  }
+
+  // Mean and covariance of prediction passed through UT
+  auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, States>(
+      sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY,
+      discR.template triangularView<Eigen::Lower>());
+
+  // Compute cross covariance of the state and the measurements
+  Matrixd<States, Rows> Pxy;
+  Pxy.setZero();
+  for (int i = 0; i < m_pts.NumSigmas(); ++i) {
+    // Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
+    Pxy += m_pts.Wc(i) *
+           (residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
+           (residualFuncY(sigmasH.template block<Rows, 1>(0, i), yHat))
+               .transpose();
+  }
+
+  // K = (P_{xy} / S_yᵀ) / S_y
+  // K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
+  // K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
+  Matrixd<States, Rows> K =
+      Sy.transpose()
+          .fullPivHouseholderQr()
+          .solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))
+          .transpose();
+
+  // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
+  m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
+
+  Matrixd<States, Rows> U = K * Sy;
+  for (int i = 0; i < Rows; i++) {
+    Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
+        m_S, U.template block<States, 1>(0, i), -1);
+  }
+}
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
index 2df0a47..e28f094 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
@@ -6,15 +6,17 @@
 
 #include <tuple>
 
-#include "Eigen/Core"
+#include "Eigen/QR"
+#include "frc/EigenCore.h"
 
 namespace frc {
 
 /**
  * Computes unscented transform of a set of sigma points and weights. CovDim
- * returns the mean and covariance in a tuple.
+ * returns the mean and square-root covariance of the sigma points in a tuple.
  *
- * This works in conjunction with the UnscentedKalmanFilter class.
+ * This works in conjunction with the UnscentedKalmanFilter class. For use with
+ * square-root form UKFs.
  *
  * @tparam CovDim      Dimension of covariance of sigma points after passing
  *                     through the transform.
@@ -26,41 +28,48 @@
  *                     vectors using a given set of weights.
  * @param residualFunc A function that computes the residual of two state
  *                     vectors (i.e. it subtracts them.)
+ * @param squareRootR  Square-root of the noise covaraince of the sigma points.
  *
- * @return Tuple of x, mean of sigma points; P, covariance of sigma points after
- *         passing through the transform.
+ * @return Tuple of x, mean of sigma points; S, square-root covariance of
+ * sigmas.
  */
 template <int CovDim, int States>
-std::tuple<Eigen::Vector<double, CovDim>, Eigen::Matrix<double, CovDim, CovDim>>
-UnscentedTransform(const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
-                   const Eigen::Vector<double, 2 * States + 1>& Wm,
-                   const Eigen::Vector<double, 2 * States + 1>& Wc,
-                   std::function<Eigen::Vector<double, CovDim>(
-                       const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
-                       const Eigen::Vector<double, 2 * States + 1>&)>
-                       meanFunc,
-                   std::function<Eigen::Vector<double, CovDim>(
-                       const Eigen::Vector<double, CovDim>&,
-                       const Eigen::Vector<double, CovDim>&)>
-                       residualFunc) {
+std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>>
+SquareRootUnscentedTransform(
+    const Matrixd<CovDim, 2 * States + 1>& sigmas,
+    const Vectord<2 * States + 1>& Wm, const Vectord<2 * States + 1>& Wc,
+    std::function<Vectord<CovDim>(const Matrixd<CovDim, 2 * States + 1>&,
+                                  const Vectord<2 * States + 1>&)>
+        meanFunc,
+    std::function<Vectord<CovDim>(const Vectord<CovDim>&,
+                                  const Vectord<CovDim>&)>
+        residualFunc,
+    const Matrixd<CovDim, CovDim>& squareRootR) {
   // New mean is usually just the sum of the sigmas * weight:
   //       n
   // dot = Σ W[k] Xᵢ[k]
   //      k=1
-  Eigen::Vector<double, CovDim> x = meanFunc(sigmas, Wm);
+  Vectord<CovDim> x = meanFunc(sigmas, Wm);
 
-  // New covariance is the sum of the outer product of the residuals times the
-  // weights
-  Eigen::Matrix<double, CovDim, 2 * States + 1> y;
-  for (int i = 0; i < 2 * States + 1; ++i) {
-    // y[:, i] = sigmas[:, i] - x
-    y.template block<CovDim, 1>(0, i) =
-        residualFunc(sigmas.template block<CovDim, 1>(0, i), x);
+  Matrixd<CovDim, States * 2 + CovDim> Sbar;
+  for (int i = 0; i < States * 2; i++) {
+    Sbar.template block<CovDim, 1>(0, i) =
+        std::sqrt(Wc[1]) *
+        residualFunc(sigmas.template block<CovDim, 1>(0, 1 + i), x);
   }
-  Eigen::Matrix<double, CovDim, CovDim> P =
-      y * Eigen::DiagonalMatrix<double, 2 * States + 1>(Wc) * y.transpose();
+  Sbar.template block<CovDim, CovDim>(0, States * 2) = squareRootR;
 
-  return std::make_tuple(x, P);
+  // Merwe defines the QR decomposition as Aᵀ = QR
+  Matrixd<CovDim, CovDim> S = Sbar.transpose()
+                                  .householderQr()
+                                  .matrixQR()
+                                  .template block<CovDim, CovDim>(0, 0)
+                                  .template triangularView<Eigen::Upper>();
+
+  Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
+      S, residualFunc(sigmas.template block<CovDim, 1>(0, 0), x), Wc[0]);
+
+  return std::make_tuple(x, S);
 }
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/filter/Debouncer.h b/wpimath/src/main/native/include/frc/filter/Debouncer.h
index 7307102..a929f37 100644
--- a/wpimath/src/main/native/include/frc/filter/Debouncer.h
+++ b/wpimath/src/main/native/include/frc/filter/Debouncer.h
@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
 #include <wpi/timestamp.h>
 
 #include "units/time.h"
diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
index 6295327..0fb4f48 100644
--- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h
+++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
@@ -7,15 +7,15 @@
 #include <algorithm>
 #include <cmath>
 #include <initializer_list>
+#include <span>
 #include <stdexcept>
 #include <vector>
 
 #include <wpi/array.h>
 #include <wpi/circular_buffer.h>
-#include <wpi/span.h>
 
-#include "Eigen/Core"
 #include "Eigen/QR"
+#include "frc/EigenCore.h"
 #include "units/time.h"
 #include "wpimath/MathShared.h"
 
@@ -80,7 +80,7 @@
    * @param ffGains The "feedforward" or FIR gains.
    * @param fbGains The "feedback" or IIR gains.
    */
-  LinearFilter(wpi::span<const double> ffGains, wpi::span<const double> fbGains)
+  LinearFilter(std::span<const double> ffGains, std::span<const double> fbGains)
       : m_inputs(ffGains.size()),
         m_outputs(fbGains.size()),
         m_inputGains(ffGains.begin(), ffGains.end()),
@@ -157,6 +157,7 @@
    *
    * @param taps The number of samples to average over. Higher = smoother but
    *             slower
+   * @throws std::runtime_error if number of taps is less than 1.
    */
   static LinearFilter<T> MovingAverage(int taps) {
     if (taps <= 0) {
@@ -175,12 +176,12 @@
    * difference. 0 is the current sample, -1 is the previous sample, -2 is the
    * sample before that, etc. Don't use positive stencil points (samples from
    * the future) if the LinearFilter will be used for stream-based online
-   * filtering.
+   * filtering (e.g., taking derivative of encoder samples in real-time).
    *
    * @tparam Derivative The order of the derivative to compute.
    * @tparam Samples    The number of samples to use to compute the given
    *                    derivative. This must be one more than the order of
-   *                    derivative or higher.
+   *                    the derivative or higher.
    * @param stencil     List of stencil points.
    * @param period      The period in seconds between samples taken by the user.
    */
@@ -209,7 +210,7 @@
     static_assert(Derivative < Samples,
                   "Order of derivative must be less than number of samples.");
 
-    Eigen::Matrix<double, Samples, Samples> S;
+    Matrixd<Samples, Samples> S;
     for (int row = 0; row < Samples; ++row) {
       for (int col = 0; col < Samples; ++col) {
         S(row, col) = std::pow(stencil[col], row);
@@ -217,12 +218,12 @@
     }
 
     // Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta
-    Eigen::Vector<double, Samples> d;
+    Vectord<Samples> d;
     for (int i = 0; i < Samples; ++i) {
       d(i) = (i == Derivative) ? Factorial(Derivative) : 0.0;
     }
 
-    Eigen::Vector<double, Samples> a =
+    Vectord<Samples> a =
         S.householderQr().solve(d) / std::pow(period.value(), Derivative);
 
     // Reverse gains list
diff --git a/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h b/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
index f99c1af..542cd94 100644
--- a/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
+++ b/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
@@ -6,6 +6,7 @@
 
 #include <algorithm>
 
+#include <wpi/deprecated.h>
 #include <wpi/timestamp.h>
 
 #include "units/time.h"
@@ -28,15 +29,45 @@
   using Rate_t = units::unit_t<Rate>;
 
   /**
-   * Creates a new SlewRateLimiter with the given rate limit and initial value.
+   * Creates a new SlewRateLimiter with the given positive and negative rate
+   * limits and initial value.
+   *
+   * @param positiveRateLimit The rate-of-change limit in the positive
+   *                          direction, in units per second. This is expected
+   *                          to be positive.
+   * @param negativeRateLimit The rate-of-change limit in the negative
+   *                          direction, in units per second. This is expected
+   *                          to be negative.
+   * @param initialValue The initial value of the input.
+   */
+  SlewRateLimiter(Rate_t positiveRateLimit, Rate_t negativeRateLimit,
+                  Unit_t initialValue = Unit_t{0})
+      : m_positiveRateLimit{positiveRateLimit},
+        m_negativeRateLimit{negativeRateLimit},
+        m_prevVal{initialValue},
+        m_prevTime{units::microsecond_t(wpi::Now())} {}
+
+  /**
+   * Creates a new SlewRateLimiter with the given positive rate limit and
+   * negative rate limit of -rateLimit.
+   *
+   * @param rateLimit The rate-of-change limit.
+   */
+  explicit SlewRateLimiter(Rate_t rateLimit)
+      : 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.
    */
-  explicit SlewRateLimiter(Rate_t rateLimit, Unit_t initialValue = Unit_t{0})
-      : m_rateLimit{rateLimit},
-        m_prevVal{initialValue},
-        m_prevTime{units::microsecond_t(wpi::Now())} {}
+  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.
@@ -48,8 +79,9 @@
   Unit_t Calculate(Unit_t input) {
     units::second_t currentTime = units::microsecond_t(wpi::Now());
     units::second_t elapsedTime = currentTime - m_prevTime;
-    m_prevVal += std::clamp(input - m_prevVal, -m_rateLimit * elapsedTime,
-                            m_rateLimit * elapsedTime);
+    m_prevVal +=
+        std::clamp(input - m_prevVal, m_negativeRateLimit * elapsedTime,
+                   m_positiveRateLimit * elapsedTime);
     m_prevTime = currentTime;
     return m_prevVal;
   }
@@ -66,7 +98,8 @@
   }
 
  private:
-  Rate_t m_rateLimit;
+  Rate_t m_positiveRateLimit;
+  Rate_t m_negativeRateLimit;
   Unit_t m_prevVal;
   units::second_t m_prevTime;
 };
diff --git a/wpimath/src/main/native/include/frc/fmt/Eigen.h b/wpimath/src/main/native/include/frc/fmt/Eigen.h
index f6cc7b6..c6b2ee6 100644
--- a/wpimath/src/main/native/include/frc/fmt/Eigen.h
+++ b/wpimath/src/main/native/include/frc/fmt/Eigen.h
@@ -7,9 +7,10 @@
 #include <fmt/format.h>
 
 #include "Eigen/Core"
+#include "Eigen/SparseCore"
 
 /**
- * Formatter for Eigen::Matrix<>.
+ * Formatter for Eigen::Matrix<double, Rows, Cols>.
  *
  * @tparam Rows Number of rows.
  * @tparam Cols Number of columns.
@@ -51,12 +52,68 @@
   auto format(const Eigen::Matrix<double, Rows, Cols, Args...>& mat,
               FormatContext& ctx) {
     auto out = ctx.out();
-    for (int i = 0; i < Rows; ++i) {
-      for (int j = 0; j < Cols; ++j) {
+    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));
       }
 
-      if (i < Rows - 1) {
+      if (i < 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");
       }
     }
diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h b/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h
new file mode 100644
index 0000000..58b966a
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h
@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/EigenCore.h"
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Rotation3d.h"
+
+namespace frc {
+
+/**
+ * A class representing a coordinate system axis within the NWU coordinate
+ * system.
+ */
+class WPILIB_DLLEXPORT CoordinateAxis {
+ public:
+  /**
+   * Constructs a coordinate system axis within the NWU coordinate system and
+   * normalizes it.
+   *
+   * @param x The x component.
+   * @param y The y component.
+   * @param z The z component.
+   */
+  CoordinateAxis(double x, double y, double z);
+
+  CoordinateAxis(const CoordinateAxis&) = default;
+  CoordinateAxis& operator=(const CoordinateAxis&) = default;
+
+  CoordinateAxis(CoordinateAxis&&) = default;
+  CoordinateAxis& operator=(CoordinateAxis&&) = default;
+
+  /**
+   * Returns a coordinate axis corresponding to +X in the NWU coordinate system.
+   */
+  static const CoordinateAxis& N();
+
+  /**
+   * Returns a coordinate axis corresponding to -X in the NWU coordinate system.
+   */
+  static const CoordinateAxis& S();
+
+  /**
+   * Returns a coordinate axis corresponding to -Y in the NWU coordinate system.
+   */
+  static const CoordinateAxis& E();
+
+  /**
+   * Returns a coordinate axis corresponding to +Y in the NWU coordinate system.
+   */
+  static const CoordinateAxis& W();
+
+  /**
+   * Returns a coordinate axis corresponding to +Z in the NWU coordinate system.
+   */
+  static const CoordinateAxis& U();
+
+  /**
+   * Returns a coordinate axis corresponding to -Z in the NWU coordinate system.
+   */
+  static const CoordinateAxis& D();
+
+ private:
+  friend class CoordinateSystem;
+
+  Vectord<3> 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
new file mode 100644
index 0000000..232455f
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
@@ -0,0 +1,114 @@
+// 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/EigenCore.h"
+#include "frc/geometry/CoordinateAxis.h"
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Rotation3d.h"
+#include "frc/geometry/Translation3d.h"
+
+namespace frc {
+
+/**
+ * A helper class that converts Pose3d objects between different standard
+ * coordinate frames.
+ */
+class WPILIB_DLLEXPORT CoordinateSystem {
+ public:
+  /**
+   * Constructs a coordinate system with the given cardinal directions for each
+   * axis.
+   *
+   * @param positiveX The cardinal direction of the positive x-axis.
+   * @param positiveY The cardinal direction of the positive y-axis.
+   * @param positiveZ The cardinal direction of the positive z-axis.
+   * @throws std::domain_error if the coordinate system isn't special orthogonal
+   */
+  CoordinateSystem(const CoordinateAxis& positiveX,
+                   const CoordinateAxis& positiveY,
+                   const CoordinateAxis& positiveZ);
+
+  CoordinateSystem(const CoordinateSystem&) = default;
+  CoordinateSystem& operator=(const CoordinateSystem&) = default;
+  CoordinateSystem(CoordinateSystem&&) = default;
+  CoordinateSystem& operator=(CoordinateSystem&&) = default;
+
+  /**
+   * Returns an instance of the North-West-Up (NWU) coordinate system.
+   *
+   * The +X axis is north, the +Y axis is west, and the +Z axis is up.
+   */
+  static const CoordinateSystem& NWU();
+
+  /**
+   * Returns an instance of the East-Down-North (EDN) coordinate system.
+   *
+   * The +X axis is east, the +Y axis is down, and the +Z axis is north.
+   */
+  static const CoordinateSystem& EDN();
+
+  /**
+   * Returns an instance of the NED coordinate system.
+   *
+   * The +X axis is north, the +Y axis is east, and the +Z axis is down.
+   */
+  static const CoordinateSystem& NED();
+
+  /**
+   * Converts the given translation from one coordinate system to another.
+   *
+   * @param translation The translation to convert.
+   * @param from The coordinate system the translation starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given translation in the desired coordinate system.
+   */
+  static Translation3d Convert(const Translation3d& translation,
+                               const CoordinateSystem& from,
+                               const CoordinateSystem& to);
+
+  /**
+   * Converts the given rotation from one coordinate system to another.
+   *
+   * @param rotation The rotation to convert.
+   * @param from The coordinate system the rotation starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given rotation in the desired coordinate system.
+   */
+  static Rotation3d Convert(const Rotation3d& rotation,
+                            const CoordinateSystem& from,
+                            const CoordinateSystem& to);
+
+  /**
+   * Converts the given pose from one coordinate system to another.
+   *
+   * @param pose The pose to convert.
+   * @param from The coordinate system the pose starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given pose in the desired coordinate system.
+   */
+  static Pose3d Convert(const Pose3d& pose, const CoordinateSystem& from,
+                        const CoordinateSystem& to);
+
+  /**
+   * Converts the given transform from one coordinate system to another.
+   *
+   * @param transform The transform to convert.
+   * @param from The coordinate system the transform starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given transform in the desired coordinate system.
+   */
+  static Transform3d Convert(const Transform3d& transform,
+                             const CoordinateSystem& from,
+                             const CoordinateSystem& to);
+
+ private:
+  // Rotation from this coordinate system to NWU coordinate system
+  Rotation3d m_rotation;
+};
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h
index ebaa7c1..d096e8c 100644
--- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h
@@ -17,13 +17,12 @@
 namespace frc {
 
 /**
- * Represents a 2d pose containing translational and rotational elements.
+ * Represents a 2D pose containing translational and rotational elements.
  */
 class WPILIB_DLLEXPORT Pose2d {
  public:
   /**
    * Constructs a pose at the origin facing toward the positive X axis.
-   * (Translation2d{0, 0} and Rotation{0})
    */
   constexpr Pose2d() = default;
 
@@ -33,31 +32,33 @@
    * @param translation The translational component of the pose.
    * @param rotation The rotational component of the pose.
    */
-  Pose2d(Translation2d translation, Rotation2d rotation);
+  constexpr Pose2d(Translation2d translation, Rotation2d rotation);
 
   /**
-   * Convenience constructors that takes in x and y values directly instead of
-   * having to construct a Translation2d.
+   * Constructs a pose with x and y translations instead of a separate
+   * Translation2d.
    *
    * @param x The x component of the translational component of the pose.
    * @param y The y component of the translational component of the pose.
    * @param rotation The rotational component of the pose.
    */
-  Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
+  constexpr Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
 
   /**
    * Transforms the pose by the given transformation and returns the new
    * transformed pose.
    *
+   * <pre>
    * [x_new]    [cos, -sin, 0][transform.x]
    * [y_new] += [sin,  cos, 0][transform.y]
-   * [t_new]    [0,    0,   1][transform.t]
+   * [t_new]    [  0,    0, 1][transform.t]
+   * </pre>
    *
    * @param other The transform to transform the pose by.
    *
    * @return The transformed pose.
    */
-  Pose2d operator+(const Transform2d& other) const;
+  constexpr Pose2d operator+(const Transform2d& other) const;
 
   /**
    * Returns the Transform2d that maps the one pose to another.
@@ -69,47 +70,54 @@
 
   /**
    * Checks equality between this Pose2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are equal.
    */
-  bool operator==(const Pose2d& other) const;
-
-  /**
-   * Checks inequality between this Pose2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const Pose2d& other) const;
+  bool operator==(const Pose2d&) const = default;
 
   /**
    * Returns the underlying translation.
    *
    * @return Reference to the translational component of the pose.
    */
-  const Translation2d& Translation() const { return m_translation; }
+  constexpr const Translation2d& Translation() const { return m_translation; }
 
   /**
    * Returns the X component of the pose's translation.
    *
    * @return The x component of the pose's translation.
    */
-  units::meter_t X() const { return m_translation.X(); }
+  constexpr units::meter_t X() const { return m_translation.X(); }
 
   /**
    * Returns the Y component of the pose's translation.
    *
    * @return The y component of the pose's translation.
    */
-  units::meter_t Y() const { return m_translation.Y(); }
+  constexpr units::meter_t Y() const { return m_translation.Y(); }
 
   /**
    * Returns the underlying rotation.
    *
    * @return Reference to the rotational component of the pose.
    */
-  const Rotation2d& Rotation() const { return m_rotation; }
+  constexpr const Rotation2d& Rotation() const { return m_rotation; }
+
+  /**
+   * Multiplies the current pose by a scalar.
+   *
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Pose2d.
+   */
+  constexpr Pose2d operator*(double scalar) const;
+
+  /**
+   * Divides the current pose by a scalar.
+   *
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Pose2d.
+   */
+  constexpr Pose2d operator/(double scalar) const;
 
   /**
    * Transforms the pose by the given transformation and returns the new pose.
@@ -119,10 +127,10 @@
    *
    * @return The transformed pose.
    */
-  Pose2d TransformBy(const Transform2d& other) const;
+  constexpr Pose2d TransformBy(const Transform2d& other) const;
 
   /**
-   * Returns the other pose relative to the current pose.
+   * Returns the current pose relative to the given pose.
    *
    * This function can often be used for trajectory tracking or pose
    * stabilization algorithms to get the error between the reference and the
@@ -152,7 +160,7 @@
    * @param twist The change in pose in the robot's coordinate frame since the
    * previous pose update. For example, if a non-holonomic robot moves forward
    * 0.01 meters and changes angle by 0.5 degrees since the previous pose
-   * update, the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
+   * update, the twist would be Twist2d{0.01_m, 0_m, 0.5_deg}.
    *
    * @return The new pose of the robot.
    */
@@ -180,3 +188,5 @@
 void from_json(const wpi::json& json, Pose2d& pose);
 
 }  // namespace frc
+
+#include "Pose2d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc
new file mode 100644
index 0000000..c549f26
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc
@@ -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.
+
+#pragma once
+
+#include <utility>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "units/length.h"
+
+namespace frc {
+
+constexpr Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
+    : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
+
+constexpr Pose2d::Pose2d(units::meter_t x, units::meter_t y,
+                         Rotation2d rotation)
+    : m_translation(x, y), m_rotation(std::move(rotation)) {}
+
+constexpr Pose2d Pose2d::operator+(const Transform2d& other) const {
+  return TransformBy(other);
+}
+
+constexpr Pose2d Pose2d::operator*(double scalar) const {
+  return Pose2d{m_translation * scalar, m_rotation * scalar};
+}
+
+constexpr Pose2d Pose2d::operator/(double scalar) const {
+  return *this * (1.0 / scalar);
+}
+
+constexpr Pose2d Pose2d::TransformBy(const Transform2d& other) const {
+  return {m_translation + (other.Translation().RotateBy(m_rotation)),
+          other.Rotation() + m_rotation};
+}
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h
new file mode 100644
index 0000000..8c7ace3
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h
@@ -0,0 +1,204 @@
+// 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 "Pose2d.h"
+#include "Transform3d.h"
+#include "Translation3d.h"
+#include "Twist3d.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace frc {
+
+/**
+ * Represents a 3D pose containing translational and rotational elements.
+ */
+class WPILIB_DLLEXPORT Pose3d {
+ public:
+  /**
+   * Constructs a pose at the origin facing toward the positive X axis.
+   */
+  constexpr Pose3d() = default;
+
+  /**
+   * Constructs a pose with the specified translation and rotation.
+   *
+   * @param translation The translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  Pose3d(Translation3d translation, Rotation3d rotation);
+
+  /**
+   * Constructs a pose with x, y, and z translations instead of a separate
+   * Translation3d.
+   *
+   * @param x The x component of the translational component of the pose.
+   * @param y The y component of the translational component of the pose.
+   * @param z The z component of the translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
+         Rotation3d rotation);
+
+  /**
+   * Constructs a 3D pose from a 2D pose in the X-Y plane.
+   *
+   * @param pose The 2D pose.
+   */
+  explicit Pose3d(const Pose2d& pose);
+
+  /**
+   * Transforms the pose by the given transformation and returns the new
+   * transformed pose.
+   *
+   * @param other The transform to transform the pose by.
+   *
+   * @return The transformed pose.
+   */
+  Pose3d operator+(const Transform3d& other) const;
+
+  /**
+   * Returns the Transform3d that maps the one pose to another.
+   *
+   * @param other The initial pose of the transformation.
+   * @return The transform that maps the other pose to the current pose.
+   */
+  Transform3d operator-(const Pose3d& other) const;
+
+  /**
+   * Checks equality between this Pose3d and another object.
+   */
+  bool operator==(const Pose3d&) const = default;
+
+  /**
+   * Returns the underlying translation.
+   *
+   * @return Reference to the translational component of the pose.
+   */
+  const Translation3d& Translation() const { return m_translation; }
+
+  /**
+   * Returns the X component of the pose's translation.
+   *
+   * @return The x component of the pose's translation.
+   */
+  units::meter_t X() const { return m_translation.X(); }
+
+  /**
+   * Returns the Y component of the pose's translation.
+   *
+   * @return The y component of the pose's translation.
+   */
+  units::meter_t Y() const { return m_translation.Y(); }
+
+  /**
+   * Returns the Z component of the pose's translation.
+   *
+   * @return The z component of the pose's translation.
+   */
+  units::meter_t Z() const { return m_translation.Z(); }
+
+  /**
+   * Returns the underlying rotation.
+   *
+   * @return Reference to the rotational component of the pose.
+   */
+  const Rotation3d& Rotation() const { return m_rotation; }
+
+  /**
+   * Multiplies the current pose by a scalar.
+   *
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Pose2d.
+   */
+  Pose3d operator*(double scalar) const;
+
+  /**
+   * Divides the current pose by a scalar.
+   *
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Pose2d.
+   */
+  Pose3d operator/(double scalar) const;
+
+  /**
+   * Transforms the pose by the given transformation and returns the new pose.
+   * See + operator for the matrix multiplication performed.
+   *
+   * @param other The transform to transform the pose by.
+   *
+   * @return The transformed pose.
+   */
+  Pose3d TransformBy(const Transform3d& other) const;
+
+  /**
+   * Returns the current pose relative to the given pose.
+   *
+   * This function can often be used for trajectory tracking or pose
+   * stabilization algorithms to get the error between the reference and the
+   * current pose.
+   *
+   * @param other The pose that is the origin of the new coordinate frame that
+   * the current pose will be converted into.
+   *
+   * @return The current pose relative to the new origin pose.
+   */
+  Pose3d RelativeTo(const Pose3d& other) const;
+
+  /**
+   * Obtain a new Pose3d from a (constant curvature) velocity.
+   *
+   * The twist is a change in pose in the robot's coordinate frame since the
+   * previous pose update. When the user runs exp() on the previous known
+   * field-relative pose with the argument being the twist, the user will
+   * receive the new field-relative pose.
+   *
+   * "Exp" represents the pose exponential, which is solving a differential
+   * equation moving the pose forward in time.
+   *
+   * @param twist The change in pose in the robot's coordinate frame since the
+   * previous pose update. For example, if a non-holonomic robot moves forward
+   * 0.01 meters and changes angle by 0.5 degrees since the previous pose
+   * update, the twist would be Twist3d{0.01_m, 0_m, 0_m, Rotation3d{0.0, 0.0,
+   * 0.5_deg}}.
+   *
+   * @return The new pose of the robot.
+   */
+  Pose3d Exp(const Twist3d& twist) const;
+
+  /**
+   * Returns a Twist3d that maps this pose to the end pose. If c is the output
+   * of a.Log(b), then a.Exp(c) would yield b.
+   *
+   * @param end The end pose for the transformation.
+   *
+   * @return The twist that maps this to end.
+   */
+  Twist3d Log(const Pose3d& end) const;
+
+  /**
+   * Returns a Pose2d representing this Pose3d projected into the X-Y plane.
+   */
+  Pose2d ToPose2d() const;
+
+ private:
+  Translation3d m_translation;
+  Rotation3d m_rotation;
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Pose3d& pose);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Pose3d& pose);
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/wpimath/src/main/native/include/frc/geometry/Quaternion.h
new file mode 100644
index 0000000..b5a318b
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Quaternion.h
@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/EigenCore.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace frc {
+
+class WPILIB_DLLEXPORT Quaternion {
+ public:
+  /**
+   * Constructs a quaternion with a default angle of 0 degrees.
+   */
+  Quaternion() = default;
+
+  /**
+   * Constructs a quaternion with the given components.
+   *
+   * @param w W component of the quaternion.
+   * @param x X component of the quaternion.
+   * @param y Y component of the quaternion.
+   * @param z Z component of the quaternion.
+   */
+  Quaternion(double w, double x, double y, double z);
+
+  /**
+   * Multiply with another quaternion.
+   *
+   * @param other The other quaternion.
+   */
+  Quaternion operator*(const Quaternion& other) const;
+
+  /**
+   * Checks equality between this Quaternion and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Quaternion& other) const;
+
+  /**
+   * Returns the inverse of the quaternion.
+   */
+  Quaternion Inverse() const;
+
+  /**
+   * Normalizes the quaternion.
+   */
+  Quaternion Normalize() const;
+
+  /**
+   * Returns W component of the quaternion.
+   */
+  double W() const;
+
+  /**
+   * Returns X component of the quaternion.
+   */
+  double X() const;
+
+  /**
+   * Returns Y component of the quaternion.
+   */
+  double Y() const;
+
+  /**
+   * Returns Z component of the quaternion.
+   */
+  double Z() const;
+
+  /**
+   * Returns the rotation vector representation of this quaternion.
+   *
+   * This is also the log operator of SO(3).
+   */
+  Eigen::Vector3d ToRotationVector() const;
+
+ private:
+  double m_r = 1.0;
+  Eigen::Vector3d m_v{0.0, 0.0, 0.0};
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Quaternion& quaternion);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Quaternion& quaternion);
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
index 94a17fc..406ef3c 100644
--- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
@@ -15,8 +15,13 @@
 namespace frc {
 
 /**
- * A rotation in a 2d coordinate frame represented a point on the unit circle
+ * A rotation in a 2D coordinate frame represented by a point on the unit circle
  * (cosine and sine).
+ *
+ * The angle is continuous, that is if a Rotation2d is constructed with 361
+ * degrees, it will return 361 degrees. This allows algorithms that wouldn't
+ * want to see a discontinuity in the rotations as it sweeps past from 360 to 0
+ * on the second time around.
  */
 class WPILIB_DLLEXPORT Rotation2d {
  public:
@@ -30,14 +35,14 @@
    *
    * @param value The value of the angle in radians.
    */
-  Rotation2d(units::radian_t value);  // NOLINT
+  constexpr Rotation2d(units::radian_t value);  // NOLINT
 
   /**
    * Constructs a Rotation2d with the given degree value.
    *
    * @param value The value of the angle in degrees.
    */
-  Rotation2d(units::degree_t value);  // NOLINT
+  constexpr Rotation2d(units::degree_t value);  // NOLINT
 
   /**
    * Constructs a Rotation2d with the given x and y (cosine and sine)
@@ -46,33 +51,33 @@
    * @param x The x component or cosine of the rotation.
    * @param y The y component or sine of the rotation.
    */
-  Rotation2d(double x, double y);
+  constexpr Rotation2d(double x, double y);
 
   /**
    * Adds two rotations together, with the result being bounded between -pi and
    * pi.
    *
-   * For example, Rotation2d.FromDegrees(30) + Rotation2d.FromDegrees(60) =
-   * Rotation2d{-pi/2}
+   * For example, <code>Rotation2d{30_deg} + Rotation2d{60_deg}</code> equals
+   * <code>Rotation2d{units::radian_t{std::numbers::pi/2.0}}</code>
    *
    * @param other The rotation to add.
    *
    * @return The sum of the two rotations.
    */
-  Rotation2d operator+(const Rotation2d& other) const;
+  constexpr Rotation2d operator+(const Rotation2d& other) const;
 
   /**
    * Subtracts the new rotation from the current rotation and returns the new
    * rotation.
    *
-   * For example, Rotation2d.FromDegrees(10) - Rotation2d.FromDegrees(100) =
-   * Rotation2d{-pi/2}
+   * For example, <code>Rotation2d{10_deg} - Rotation2d{100_deg}</code> equals
+   * <code>Rotation2d{units::radian_t{-std::numbers::pi/2.0}}</code>
    *
    * @param other The rotation to subtract.
    *
    * @return The difference between the two rotations.
    */
-  Rotation2d operator-(const Rotation2d& other) const;
+  constexpr Rotation2d operator-(const Rotation2d& other) const;
 
   /**
    * Takes the inverse of the current rotation. This is simply the negative of
@@ -80,15 +85,25 @@
    *
    * @return The inverse of the current rotation.
    */
-  Rotation2d operator-() const;
+  constexpr Rotation2d operator-() const;
 
   /**
    * Multiplies the current rotation by a scalar.
+   *
    * @param scalar The scalar.
    *
    * @return The new scaled Rotation2d.
    */
-  Rotation2d operator*(double scalar) const;
+  constexpr Rotation2d operator*(double scalar) const;
+
+  /**
+   * Divides the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Rotation2d.
+   */
+  constexpr Rotation2d operator/(double scalar) const;
 
   /**
    * Checks equality between this Rotation2d and another object.
@@ -96,15 +111,7 @@
    * @param other The other object.
    * @return Whether the two objects are equal.
    */
-  bool operator==(const Rotation2d& other) const;
-
-  /**
-   * Checks inequality between this Rotation2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const Rotation2d& other) const;
+  constexpr bool operator==(const Rotation2d& other) const;
 
   /**
    * Adds the new rotation to the current rotation using a rotation matrix.
@@ -119,42 +126,44 @@
    *
    * @return The new rotated Rotation2d.
    */
-  Rotation2d RotateBy(const Rotation2d& other) const;
+  constexpr Rotation2d RotateBy(const Rotation2d& other) const;
 
   /**
    * Returns the radian value of the rotation.
    *
    * @return The radian value of the rotation.
+   * @see AngleModulus to constrain the angle within (-pi, pi]
    */
-  units::radian_t Radians() const { return m_value; }
+  constexpr units::radian_t Radians() const { return m_value; }
 
   /**
    * Returns the degree value of the rotation.
    *
    * @return The degree value of the rotation.
+   * @see InputModulus to constrain the angle within (-180, 180]
    */
-  units::degree_t Degrees() const { return m_value; }
+  constexpr units::degree_t Degrees() const { return m_value; }
 
   /**
    * Returns the cosine of the rotation.
    *
    * @return The cosine of the rotation.
    */
-  double Cos() const { return m_cos; }
+  constexpr double Cos() const { return m_cos; }
 
   /**
    * Returns the sine of the rotation.
    *
    * @return The sine of the rotation.
    */
-  double Sin() const { return m_sin; }
+  constexpr double Sin() const { return m_sin; }
 
   /**
    * Returns the tangent of the rotation.
    *
    * @return The tangent of the rotation.
    */
-  double Tan() const { return m_sin / m_cos; }
+  constexpr double Tan() const { return Sin() / Cos(); }
 
  private:
   units::radian_t m_value = 0_rad;
@@ -169,3 +178,5 @@
 void from_json(const wpi::json& json, Rotation2d& rotation);
 
 }  // namespace frc
+
+#include "Rotation2d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
new file mode 100644
index 0000000..eb31ebd
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
@@ -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.
+
+#pragma once
+
+#include <type_traits>
+
+#include "frc/geometry/Rotation2d.h"
+#include "gcem.hpp"
+#include "units/angle.h"
+
+namespace frc {
+
+constexpr Rotation2d::Rotation2d(units::radian_t value)
+    : m_value(value),
+      m_cos(std::is_constant_evaluated() ? gcem::cos(value.to<double>())
+                                         : std::cos(value.to<double>())),
+      m_sin(std::is_constant_evaluated() ? gcem::sin(value.to<double>())
+                                         : std::sin(value.to<double>())) {}
+
+constexpr Rotation2d::Rotation2d(units::degree_t value)
+    : Rotation2d(units::radian_t{value}) {}
+
+constexpr Rotation2d::Rotation2d(double x, double y) {
+  const auto magnitude = gcem::hypot(x, y);
+  if (magnitude > 1e-6) {
+    m_sin = y / magnitude;
+    m_cos = x / magnitude;
+  } else {
+    m_sin = 0.0;
+    m_cos = 1.0;
+  }
+  m_value =
+      units::radian_t{std::is_constant_evaluated() ? gcem::atan2(m_sin, m_cos)
+                                                   : std::atan2(m_sin, m_cos)};
+}
+
+constexpr Rotation2d Rotation2d::operator-() const {
+  return Rotation2d{-m_value};
+}
+
+constexpr Rotation2d Rotation2d::operator*(double scalar) const {
+  return Rotation2d(m_value * scalar);
+}
+
+constexpr Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
+  return RotateBy(other);
+}
+
+constexpr Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
+  return *this + -other;
+}
+
+constexpr Rotation2d Rotation2d::operator/(double scalar) const {
+  return *this * (1.0 / scalar);
+}
+
+constexpr bool Rotation2d::operator==(const Rotation2d& other) const {
+  return (std::is_constant_evaluated()
+              ? gcem::hypot(Cos() - other.Cos(), Sin() - other.Sin())
+              : std::hypot(Cos() - other.Cos(), Sin() - other.Sin())) < 1E-9;
+}
+
+constexpr Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
+  return {Cos() * other.Cos() - Sin() * other.Sin(),
+          Cos() * other.Sin() + Sin() * other.Cos()};
+}
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
new file mode 100644
index 0000000..7c1a60d
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
@@ -0,0 +1,186 @@
+// 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 "Quaternion.h"
+#include "Rotation2d.h"
+#include "frc/EigenCore.h"
+#include "units/angle.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace frc {
+
+/**
+ * A rotation in a 3D coordinate frame represented by a quaternion.
+ */
+class WPILIB_DLLEXPORT Rotation3d {
+ public:
+  /**
+   * Constructs a Rotation3d with a default angle of 0 degrees.
+   */
+  Rotation3d() = default;
+
+  /**
+   * Constructs a Rotation3d from a quaternion.
+   *
+   * @param q The quaternion.
+   */
+  explicit Rotation3d(const Quaternion& q);
+
+  /**
+   * Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
+   *
+   * Extrinsic rotations occur in that order around the axes in the fixed global
+   * frame rather than the body frame.
+   *
+   * Angles are measured counterclockwise with the rotation axis pointing "out
+   * of the page". If you point your right thumb along the positive axis
+   * direction, your fingers curl in the direction of positive rotation.
+   *
+   * @param roll The counterclockwise rotation angle around the X axis (roll).
+   * @param pitch The counterclockwise rotation angle around the Y axis (pitch).
+   * @param yaw The counterclockwise rotation angle around the Z axis (yaw).
+   */
+  Rotation3d(units::radian_t roll, units::radian_t pitch, units::radian_t yaw);
+
+  /**
+   * Constructs a Rotation3d with the given axis-angle representation. The axis
+   * doesn't have to be normalized.
+   *
+   * @param axis The rotation axis.
+   * @param angle The rotation around the axis.
+   */
+  Rotation3d(const Vectord<3>& axis, units::radian_t angle);
+
+  /**
+   * Constructs a Rotation3d from a rotation matrix.
+   *
+   * @param rotationMatrix The rotation matrix.
+   * @throws std::domain_error if the rotation matrix isn't special orthogonal.
+   */
+  explicit Rotation3d(const Matrixd<3, 3>& rotationMatrix);
+
+  /**
+   * Constructs a Rotation3d that rotates the initial vector onto the final
+   * vector.
+   *
+   * This is useful for turning a 3D vector (final) into an orientation relative
+   * to a coordinate system vector (initial).
+   *
+   * @param initial The initial vector.
+   * @param final The final vector.
+   */
+  Rotation3d(const Vectord<3>& initial, const Vectord<3>& final);
+
+  /**
+   * Adds two rotations together.
+   *
+   * @param other The rotation to add.
+   *
+   * @return The sum of the two rotations.
+   */
+  Rotation3d operator+(const Rotation3d& other) const;
+
+  /**
+   * Subtracts the new rotation from the current rotation and returns the new
+   * rotation.
+   *
+   * @param other The rotation to subtract.
+   *
+   * @return The difference between the two rotations.
+   */
+  Rotation3d operator-(const Rotation3d& other) const;
+
+  /**
+   * Takes the inverse of the current rotation.
+   *
+   * @return The inverse of the current rotation.
+   */
+  Rotation3d operator-() const;
+
+  /**
+   * Multiplies the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Rotation3d.
+   */
+  Rotation3d operator*(double scalar) const;
+
+  /**
+   * Divides the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Rotation3d.
+   */
+  Rotation3d operator/(double scalar) const;
+
+  /**
+   * Checks equality between this Rotation3d and another object.
+   */
+  bool operator==(const Rotation3d&) const = default;
+
+  /**
+   * Adds the new rotation to the current rotation.
+   *
+   * @param other The rotation to rotate by.
+   *
+   * @return The new rotated Rotation3d.
+   */
+  Rotation3d RotateBy(const Rotation3d& other) const;
+
+  /**
+   * Returns the quaternion representation of the Rotation3d.
+   */
+  const Quaternion& GetQuaternion() const;
+
+  /**
+   * Returns the counterclockwise rotation angle around the X axis (roll).
+   */
+  units::radian_t X() const;
+
+  /**
+   * Returns the counterclockwise rotation angle around the Y axis (pitch).
+   */
+  units::radian_t Y() const;
+
+  /**
+   * Returns the counterclockwise rotation angle around the Z axis (yaw).
+   */
+  units::radian_t Z() const;
+
+  /**
+   * Returns the axis in the axis-angle representation of this rotation.
+   */
+  Vectord<3> Axis() const;
+
+  /**
+   * Returns the angle in the axis-angle representation of this rotation.
+   */
+  units::radian_t Angle() const;
+
+  /**
+   * Returns a Rotation2d representing this Rotation3d projected into the X-Y
+   * plane.
+   */
+  Rotation2d ToRotation2d() const;
+
+ private:
+  Quaternion m_q;
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Rotation3d& rotation);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Rotation3d& rotation);
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h
index 3d5e1d6..3c21ec1 100644
--- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h
@@ -31,7 +31,7 @@
    * @param translation Translational component of the transform.
    * @param rotation Rotational component of the transform.
    */
-  Transform2d(Translation2d translation, Rotation2d rotation);
+  constexpr Transform2d(Translation2d translation, Rotation2d rotation);
 
   /**
    * Constructs the identity transform -- maps an initial pose to itself.
@@ -43,47 +43,57 @@
    *
    * @return Reference to the translational component of the transform.
    */
-  const Translation2d& Translation() const { return m_translation; }
+  constexpr const Translation2d& Translation() const { return m_translation; }
 
   /**
    * Returns the X component of the transformation's translation.
    *
    * @return The x component of the transformation's translation.
    */
-  units::meter_t X() const { return m_translation.X(); }
+  constexpr units::meter_t X() const { return m_translation.X(); }
 
   /**
    * Returns the Y component of the transformation's translation.
    *
    * @return The y component of the transformation's translation.
    */
-  units::meter_t Y() const { return m_translation.Y(); }
+  constexpr units::meter_t Y() const { return m_translation.Y(); }
 
   /**
    * Returns the rotational component of the transformation.
    *
    * @return Reference to the rotational component of the transform.
    */
-  const Rotation2d& Rotation() const { return m_rotation; }
+  constexpr const Rotation2d& Rotation() const { return m_rotation; }
 
   /**
    * Invert the transformation. This is useful for undoing a transformation.
    *
    * @return The inverted transformation.
    */
-  Transform2d Inverse() const;
+  constexpr Transform2d Inverse() const;
 
   /**
-   * Scales the transform by the scalar.
+   * Multiplies the transform by the scalar.
    *
    * @param scalar The scalar.
    * @return The scaled Transform2d.
    */
-  Transform2d operator*(double scalar) const {
+  constexpr Transform2d operator*(double scalar) const {
     return Transform2d(m_translation * scalar, m_rotation * scalar);
   }
 
   /**
+   * Divides the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform2d.
+   */
+  constexpr Transform2d operator/(double scalar) const {
+    return *this * (1.0 / scalar);
+  }
+
+  /**
    * Composes two transformations.
    *
    * @param other The transform to compose with this one.
@@ -93,22 +103,13 @@
 
   /**
    * Checks equality between this Transform2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are equal.
    */
-  bool operator==(const Transform2d& other) const;
-
-  /**
-   * Checks inequality between this Transform2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const Transform2d& other) const;
+  bool operator==(const Transform2d&) const = default;
 
  private:
   Translation2d m_translation;
   Rotation2d m_rotation;
 };
 }  // namespace frc
+
+#include "Transform2d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.inc b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc
new file mode 100644
index 0000000..f851a05
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc
@@ -0,0 +1,26 @@
+// 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 <utility>
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Transform2d.h"
+#include "frc/geometry/Translation2d.h"
+
+namespace frc {
+
+constexpr Transform2d::Transform2d(Translation2d translation,
+                                   Rotation2d rotation)
+    : m_translation(std::move(translation)), 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
+  // delta into a local delta (relative to the initial pose).
+  return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
+}
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h
new file mode 100644
index 0000000..5f50ec2
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h
@@ -0,0 +1,118 @@
+// 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 "Translation3d.h"
+
+namespace frc {
+
+class WPILIB_DLLEXPORT Pose3d;
+
+/**
+ * Represents a transformation for a Pose3d.
+ */
+class WPILIB_DLLEXPORT Transform3d {
+ public:
+  /**
+   * Constructs the transform that maps the initial pose to the final pose.
+   *
+   * @param initial The initial pose for the transformation.
+   * @param final The final pose for the transformation.
+   */
+  Transform3d(Pose3d initial, Pose3d final);
+
+  /**
+   * Constructs a transform with the given translation and rotation components.
+   *
+   * @param translation Translational component of the transform.
+   * @param rotation Rotational component of the transform.
+   */
+  Transform3d(Translation3d translation, Rotation3d rotation);
+
+  /**
+   * Constructs the identity transform -- maps an initial pose to itself.
+   */
+  constexpr Transform3d() = default;
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return Reference to the translational component of the transform.
+   */
+  const Translation3d& Translation() const { return m_translation; }
+
+  /**
+   * Returns the X component of the transformation's translation.
+   *
+   * @return The x component of the transformation's translation.
+   */
+  units::meter_t X() const { return m_translation.X(); }
+
+  /**
+   * Returns the Y component of the transformation's translation.
+   *
+   * @return The y component of the transformation's translation.
+   */
+  units::meter_t Y() const { return m_translation.Y(); }
+
+  /**
+   * Returns the Z component of the transformation's translation.
+   *
+   * @return The z component of the transformation's translation.
+   */
+  units::meter_t Z() const { return m_translation.Z(); }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return Reference to the rotational component of the transform.
+   */
+  const Rotation3d& Rotation() const { return m_rotation; }
+
+  /**
+   * Invert the transformation. This is useful for undoing a transformation.
+   *
+   * @return The inverted transformation.
+   */
+  Transform3d Inverse() const;
+
+  /**
+   * Multiplies the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform3d.
+   */
+  Transform3d operator*(double scalar) const {
+    return Transform3d(m_translation * scalar, m_rotation * scalar);
+  }
+
+  /**
+   * Divides the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform3d.
+   */
+  Transform3d operator/(double scalar) const { return *this * (1.0 / scalar); }
+
+  /**
+   * Composes two transformations.
+   *
+   * @param other The transform to compose with this one.
+   * @return The composition of the two transformations.
+   */
+  Transform3d operator+(const Transform3d& other) const;
+
+  /**
+   * Checks equality between this Transform3d and another object.
+   */
+  bool operator==(const Transform3d&) const = default;
+
+ private:
+  Translation3d m_translation;
+  Rotation3d m_rotation;
+};
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
index 204da30..e168510 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
@@ -16,12 +16,12 @@
 namespace frc {
 
 /**
- * Represents a translation in 2d space.
+ * Represents a translation in 2D space.
  * This object can be used to represent a point or a vector.
  *
  * This assumes that you are using conventional mathematical axes.
- * When the robot is placed on the origin, facing toward the X direction,
- * moving forward increases the X, whereas moving to the left increases the Y.
+ * When the robot is at the origin facing in the positive X direction, forward
+ * is positive X and left is positive Y.
  */
 class WPILIB_DLLEXPORT Translation2d {
  public:
@@ -37,7 +37,7 @@
    * @param x The x component of the translation.
    * @param y The y component of the translation.
    */
-  Translation2d(units::meter_t x, units::meter_t y);
+  constexpr Translation2d(units::meter_t x, units::meter_t y);
 
   /**
    * Constructs a Translation2d with the provided distance and angle. This is
@@ -46,13 +46,12 @@
    * @param distance The distance from the origin to the end of the translation.
    * @param angle The angle between the x-axis and the translation vector.
    */
-  Translation2d(units::meter_t distance, const Rotation2d& angle);
+  constexpr Translation2d(units::meter_t distance, const Rotation2d& angle);
 
   /**
-   * Calculates the distance between two translations in 2d space.
+   * Calculates the distance between two translations in 2D space.
    *
-   * This function uses the pythagorean theorem to calculate the distance.
-   * distance = std::sqrt((x2 - x1)^2 + (y2 - y1)^2)
+   * The distance between translations is defined as √((x₂−x₁)²+(y₂−y₁)²).
    *
    * @param other The translation to compute the distance to.
    *
@@ -63,16 +62,16 @@
   /**
    * Returns the X component of the translation.
    *
-   * @return The x component of the translation.
+   * @return The X component of the translation.
    */
-  units::meter_t X() const { return m_x; }
+  constexpr units::meter_t X() const { return m_x; }
 
   /**
    * Returns the Y component of the translation.
    *
-   * @return The y component of the translation.
+   * @return The Y component of the translation.
    */
-  units::meter_t Y() const { return m_y; }
+  constexpr units::meter_t Y() const { return m_y; }
 
   /**
    * Returns the norm, or distance from the origin to the translation.
@@ -82,79 +81,86 @@
   units::meter_t Norm() const;
 
   /**
-   * Applies a rotation to the translation in 2d space.
+   * Returns the angle this translation forms with the positive X axis.
+   *
+   * @return The angle of the translation
+   */
+  constexpr Rotation2d Angle() const;
+
+  /**
+   * Applies a rotation to the translation in 2D space.
    *
    * This multiplies the translation vector by a counterclockwise rotation
    * matrix of the given angle.
    *
+   * <pre>
    * [x_new]   [other.cos, -other.sin][x]
    * [y_new] = [other.sin,  other.cos][y]
+   * </pre>
    *
-   * For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
-   * Translation2d of {0, 2}.
+   * For example, rotating a Translation2d of &lt;2, 0&gt; by 90 degrees will
+   * return a Translation2d of &lt;0, 2&gt;.
    *
    * @param other The rotation to rotate the translation by.
    *
    * @return The new rotated translation.
    */
-  Translation2d RotateBy(const Rotation2d& other) const;
+  constexpr Translation2d RotateBy(const Rotation2d& other) const;
 
   /**
-   * Adds two translations in 2d space and returns the sum. This is similar to
-   * vector addition.
+   * Returns the sum of two translations in 2D space.
    *
-   * For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
-   * Translation2d{3.0, 8.0}
+   * For example, Translation3d{1.0, 2.5} + Translation3d{2.0, 5.5} =
+   * Translation3d{3.0, 8.0}.
    *
    * @param other The translation to add.
    *
    * @return The sum of the translations.
    */
-  Translation2d operator+(const Translation2d& other) const;
+  constexpr Translation2d operator+(const Translation2d& other) const;
 
   /**
-   * Subtracts the other translation from the other translation and returns the
-   * difference.
+   * Returns the difference between two translations.
    *
    * For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
-   * Translation2d{4.0, 2.0}
+   * Translation2d{4.0, 2.0}.
    *
    * @param other The translation to subtract.
    *
    * @return The difference between the two translations.
    */
-  Translation2d operator-(const Translation2d& other) const;
+  constexpr Translation2d operator-(const Translation2d& other) const;
 
   /**
    * Returns the inverse of the current translation. This is equivalent to
-   * rotating by 180 degrees, flipping the point over both axes, or simply
-   * negating both components of the translation.
+   * rotating by 180 degrees, flipping the point over both axes, or negating all
+   * components of the translation.
    *
    * @return The inverse of the current translation.
    */
-  Translation2d operator-() const;
+  constexpr Translation2d operator-() const;
 
   /**
-   * Multiplies the translation by a scalar and returns the new translation.
+   * Returns the translation multiplied by a scalar.
    *
-   * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
+   * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}.
    *
    * @param scalar The scalar to multiply by.
    *
    * @return The scaled translation.
    */
-  Translation2d operator*(double scalar) const;
+  constexpr Translation2d operator*(double scalar) const;
 
   /**
-   * Divides the translation by a scalar and returns the new translation.
+   * Returns the translation divided by a scalar.
    *
-   * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+   * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}.
    *
    * @param scalar The scalar to divide by.
    *
    * @return The scaled translation.
    */
-  Translation2d operator/(double scalar) const;
+  constexpr Translation2d operator/(double scalar) const;
 
   /**
    * Checks equality between this Translation2d and another object.
@@ -164,14 +170,6 @@
    */
   bool operator==(const Translation2d& other) const;
 
-  /**
-   * Checks inequality between this Translation2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const Translation2d& other) const;
-
  private:
   units::meter_t m_x = 0_m;
   units::meter_t m_y = 0_m;
@@ -184,3 +182,5 @@
 void from_json(const wpi::json& json, Translation2d& state);
 
 }  // namespace frc
+
+#include "Translation2d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.inc b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc
new file mode 100644
index 0000000..3be897f
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc
@@ -0,0 +1,50 @@
+// 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/geometry/Translation2d.h"
+#include "units/length.h"
+
+namespace frc {
+
+constexpr Translation2d::Translation2d(units::meter_t x, units::meter_t y)
+    : m_x(x), m_y(y) {}
+
+constexpr Translation2d::Translation2d(units::meter_t distance,
+                                       const Rotation2d& angle)
+    : m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {}
+
+constexpr Rotation2d Translation2d::Angle() const {
+  return Rotation2d{m_x.value(), m_y.value()};
+}
+
+constexpr Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
+  return {m_x * other.Cos() - m_y * other.Sin(),
+          m_x * other.Sin() + m_y * other.Cos()};
+}
+
+constexpr Translation2d Translation2d::operator+(
+    const Translation2d& other) const {
+  return {X() + other.X(), Y() + other.Y()};
+}
+
+constexpr Translation2d Translation2d::operator-(
+    const Translation2d& other) const {
+  return *this + -other;
+}
+
+constexpr Translation2d Translation2d::operator-() const {
+  return {-m_x, -m_y};
+}
+
+constexpr Translation2d Translation2d::operator*(double scalar) const {
+  return {scalar * m_x, scalar * m_y};
+}
+
+constexpr Translation2d Translation2d::operator/(double scalar) const {
+  return operator*(1.0 / scalar);
+}
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
new file mode 100644
index 0000000..ab641fa
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
@@ -0,0 +1,189 @@
+// 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 "Rotation3d.h"
+#include "Translation2d.h"
+#include "units/length.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace frc {
+
+/**
+ * Represents a translation in 3D space.
+ * This object can be used to represent a point or a vector.
+ *
+ * This assumes that you are using conventional mathematical axes. When the
+ * robot is at the origin facing in the positive X direction, forward is
+ * positive X, left is positive Y, and up is positive Z.
+ */
+class WPILIB_DLLEXPORT Translation3d {
+ public:
+  /**
+   * Constructs a Translation3d with X, Y, and Z components equal to zero.
+   */
+  constexpr Translation3d() = default;
+
+  /**
+   * Constructs a Translation3d with the X, Y, and Z components equal to the
+   * provided values.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   * @param z The z component of the translation.
+   */
+  constexpr Translation3d(units::meter_t x, units::meter_t y, units::meter_t z);
+
+  /**
+   * Constructs a Translation3d with the provided distance and angle. This is
+   * essentially converting from polar coordinates to Cartesian coordinates.
+   *
+   * @param distance The distance from the origin to the end of the translation.
+   * @param angle The angle between the x-axis and the translation vector.
+   */
+  Translation3d(units::meter_t distance, const Rotation3d& angle);
+
+  /**
+   * Calculates the distance between two translations in 3D space.
+   *
+   * The distance between translations is defined as
+   * √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²).
+   *
+   * @param other The translation to compute the distance to.
+   *
+   * @return The distance between the two translations.
+   */
+  units::meter_t Distance(const Translation3d& other) const;
+
+  /**
+   * Returns the X component of the translation.
+   *
+   * @return The Z component of the translation.
+   */
+  constexpr units::meter_t X() const { return m_x; }
+
+  /**
+   * Returns the Y component of the translation.
+   *
+   * @return The Y component of the translation.
+   */
+  constexpr units::meter_t Y() const { return m_y; }
+
+  /**
+   * Returns the Z component of the translation.
+   *
+   * @return The Z component of the translation.
+   */
+  constexpr units::meter_t Z() const { return m_z; }
+
+  /**
+   * Returns the norm, or distance from the origin to the translation.
+   *
+   * @return The norm of the translation.
+   */
+  units::meter_t Norm() const;
+
+  /**
+   * Applies a rotation to the translation in 3D space.
+   *
+   * For example, rotating a Translation3d of &lt;2, 0, 0&gt; by 90 degrees
+   * around the Z axis will return a Translation3d of &lt;0, 2, 0&gt;.
+   *
+   * @param other The rotation to rotate the translation by.
+   *
+   * @return The new rotated translation.
+   */
+  Translation3d RotateBy(const Rotation3d& other) const;
+
+  /**
+   * Returns a Translation2d representing this Translation3d projected into the
+   * X-Y plane.
+   */
+  constexpr Translation2d ToTranslation2d() const;
+
+  /**
+   * Returns the sum of two translations in 3D space.
+   *
+   * For example, Translation3d{1.0, 2.5, 3.5} + Translation3d{2.0, 5.5, 7.5} =
+   * Translation3d{3.0, 8.0, 11.0}.
+   *
+   * @param other The translation to add.
+   *
+   * @return The sum of the translations.
+   */
+  constexpr Translation3d operator+(const Translation3d& other) const;
+
+  /**
+   * Returns the difference between two translations.
+   *
+   * For example, Translation3d{5.0, 4.0, 3.0} - Translation3d{1.0, 2.0, 3.0} =
+   * Translation3d{4.0, 2.0, 0.0}.
+   *
+   * @param other The translation to subtract.
+   *
+   * @return The difference between the two translations.
+   */
+  constexpr Translation3d operator-(const Translation3d& other) const;
+
+  /**
+   * Returns the inverse of the current translation. This is equivalent to
+   * negating all components of the translation.
+   *
+   * @return The inverse of the current translation.
+   */
+  constexpr Translation3d operator-() const;
+
+  /**
+   * Returns the translation multiplied by a scalar.
+   *
+   * For example, Translation3d{2.0, 2.5, 4.5} * 2 = Translation3d{4.0, 5.0,
+   * 9.0}.
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled translation.
+   */
+  constexpr Translation3d operator*(double scalar) const;
+
+  /**
+   * Returns the translation divided by a scalar.
+   *
+   * For example, Translation3d{2.0, 2.5, 4.5} / 2 = Translation3d{1.0, 1.25,
+   * 2.25}.
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled translation.
+   */
+  constexpr Translation3d operator/(double scalar) const;
+
+  /**
+   * Checks equality between this Translation3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Translation3d& other) const;
+
+ private:
+  units::meter_t m_x = 0_m;
+  units::meter_t m_y = 0_m;
+  units::meter_t m_z = 0_m;
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Translation3d& state);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Translation3d& state);
+
+}  // namespace frc
+
+#include "Translation3d.inc"
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.inc b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
new file mode 100644
index 0000000..8ab6e94
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc
@@ -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.
+
+#pragma once
+
+#include "frc/geometry/Translation2d.h"
+#include "frc/geometry/Translation3d.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+
+constexpr Translation3d::Translation3d(units::meter_t x, units::meter_t y,
+                                       units::meter_t z)
+    : m_x(x), m_y(y), m_z(z) {}
+
+constexpr Translation2d Translation3d::ToTranslation2d() const {
+  return Translation2d{m_x, m_y};
+}
+
+constexpr Translation3d Translation3d::operator+(
+    const Translation3d& other) const {
+  return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
+}
+
+constexpr Translation3d Translation3d::operator-(
+    const Translation3d& other) const {
+  return operator+(-other);
+}
+
+constexpr Translation3d Translation3d::operator-() const {
+  return {-m_x, -m_y, -m_z};
+}
+
+constexpr Translation3d Translation3d::operator*(double scalar) const {
+  return {scalar * m_x, scalar * m_y, scalar * m_z};
+}
+
+constexpr Translation3d Translation3d::operator/(double scalar) const {
+  return operator*(1.0 / scalar);
+}
+
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Twist2d.h b/wpimath/src/main/native/include/frc/geometry/Twist2d.h
index 6ad2b38..620b688 100644
--- a/wpimath/src/main/native/include/frc/geometry/Twist2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Twist2d.h
@@ -12,9 +12,9 @@
 
 namespace frc {
 /**
- * A change in distance along arc since the last pose update. We can use ideas
- * from differential calculus to create new Pose2ds from a Twist2d and vise
- * versa.
+ * 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.
  *
  * A Twist can be used to represent a difference between two poses.
  */
@@ -47,20 +47,12 @@
   }
 
   /**
-   * Checks inequality between this Twist2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const Twist2d& other) const { return !operator==(other); }
-
-  /**
    * Scale this by a given factor.
    *
    * @param factor The factor by which to scale.
    * @return The scaled Twist2d.
    */
-  Twist2d operator*(double factor) const {
+  constexpr Twist2d operator*(double factor) const {
     return Twist2d{dx * factor, dy * factor, dtheta * factor};
   }
 };
diff --git a/wpimath/src/main/native/include/frc/geometry/Twist3d.h b/wpimath/src/main/native/include/frc/geometry/Twist3d.h
new file mode 100644
index 0000000..3040ab3
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Twist3d.h
@@ -0,0 +1,79 @@
+// 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/Rotation3d.h"
+#include "units/angle.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+/**
+ * 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.
+ *
+ * A Twist can be used to represent a difference between two poses.
+ */
+struct WPILIB_DLLEXPORT Twist3d {
+  /**
+   * Linear "dx" component
+   */
+  units::meter_t dx = 0_m;
+
+  /**
+   * Linear "dy" component
+   */
+  units::meter_t dy = 0_m;
+
+  /**
+   * Linear "dz" component
+   */
+  units::meter_t dz = 0_m;
+
+  /**
+   * Rotation vector x component.
+   */
+  units::radian_t rx = 0_rad;
+
+  /**
+   * Rotation vector y component.
+   */
+  units::radian_t ry = 0_rad;
+
+  /**
+   * Rotation vector z component.
+   */
+  units::radian_t rz = 0_rad;
+
+  /**
+   * Checks equality between this Twist3d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Twist3d& other) const {
+    return units::math::abs(dx - other.dx) < 1E-9_m &&
+           units::math::abs(dy - other.dy) < 1E-9_m &&
+           units::math::abs(dz - other.dz) < 1E-9_m &&
+           units::math::abs(rx - other.rx) < 1E-9_rad &&
+           units::math::abs(ry - other.ry) < 1E-9_rad &&
+           units::math::abs(rz - other.rz) < 1E-9_rad;
+  }
+
+  /**
+   * Scale this by a given factor.
+   *
+   * @param factor The factor by which to scale.
+   * @return The scaled Twist3d.
+   */
+  constexpr Twist3d operator*(double factor) const {
+    return Twist3d{dx * factor, dy * factor, dz * factor,
+                   rx * factor, ry * factor, rz * factor};
+  }
+};
+}  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
index a049e40..771fe84 100644
--- a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
+++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
@@ -4,9 +4,11 @@
 
 #pragma once
 
+#include <algorithm>
 #include <array>
 #include <functional>
 #include <map>
+#include <optional>
 #include <utility>
 #include <vector>
 
@@ -81,12 +83,16 @@
   void Clear() { m_pastSnapshots.clear(); }
 
   /**
-   * Sample the buffer at the given time. If there are no elements in the
-   * buffer, calling this function results in undefined behavior.
+   * Sample the buffer at the given time. If the buffer is empty, an empty
+   * optional is returned.
    *
    * @param time The time at which to sample the buffer.
    */
-  T Sample(units::second_t time) {
+  std::optional<T> Sample(units::second_t time) {
+    if (m_pastSnapshots.empty()) {
+      return {};
+    }
+
     // We will perform a binary search to find the index of the element in the
     // vector that has a timestamp that is equal to or greater than the vision
     // measurement timestamp.
@@ -114,6 +120,14 @@
     return m_interpolatingFunc(lower_bound->second, upper_bound->second, t);
   }
 
+  /**
+   * Grant access to the internal sample buffer. Used in Pose Estimation to
+   * replay odometry inputs stored within this buffer.
+   */
+  std::vector<std::pair<units::second_t, T>>& GetInternalBuffer() {
+    return m_pastSnapshots;
+  }
+
  private:
   units::second_t m_historySize;
   std::vector<std::pair<units::second_t, T>> m_pastSnapshots;
diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
index 7414dec..37fe768 100644
--- a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
+++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
@@ -60,5 +60,26 @@
     return {vx * robotAngle.Cos() + vy * robotAngle.Sin(),
             -vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega};
   }
+
+  /**
+   * Converts a user provided field-relative ChassisSpeeds object into a
+   * robot-relative ChassisSpeeds object.
+   *
+   * @param fieldRelativeSpeeds The ChassisSpeeds object representing the speeds
+   *    in the field frame of reference. Positive x is away from your alliance
+   *    wall. Positive y is to your left when standing behind the alliance wall.
+   * @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 robot's frame
+   *    of reference.
+   */
+  static ChassisSpeeds FromFieldRelativeSpeeds(
+      const ChassisSpeeds& fieldRelativeSpeeds, const Rotation2d& robotAngle) {
+    return FromFieldRelativeSpeeds(fieldRelativeSpeeds.vx,
+                                   fieldRelativeSpeeds.vy,
+                                   fieldRelativeSpeeds.omega, robotAngle);
+  }
 };
 }  // 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 4bf27ff..930c7a6 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
@@ -6,6 +6,7 @@
 
 #include <wpi/SymbolExports.h>
 
+#include "frc/geometry/Twist2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
 #include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
 #include "units/angle.h"
@@ -64,6 +65,20 @@
             chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
   }
 
+  /**
+   * Returns a twist from left and right distance deltas using
+   * forward kinematics.
+   *
+   * @param leftDistance The distance measured by the left encoder.
+   * @param rightDistance The distance measured by the right encoder.
+   * @return The resulting Twist2d.
+   */
+  constexpr Twist2d ToTwist2d(const units::meter_t leftDistance,
+                              const units::meter_t rightDistance) const {
+    return {(leftDistance + rightDistance) / 2, 0_m,
+            (rightDistance - leftDistance) / trackWidth * 1_rad};
+  }
+
   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 8e992ef..cc198ac 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
@@ -27,30 +27,41 @@
   /**
    * Constructs a DifferentialDriveOdometry object.
    *
+   * IF leftDistance and rightDistance are unspecified,
+   * You NEED to reset your encoders (to zero).
+   *
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
    * @param initialPose The starting position of the robot on the field.
    */
   explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle,
-                                     const Pose2d& initialPose = Pose2d());
+                                     units::meter_t leftDistance,
+                                     units::meter_t rightDistance,
+                                     const Pose2d& initialPose = Pose2d{});
 
   /**
    * Resets the robot's position on the field.
    *
-   * You NEED to reset your encoders (to zero) when calling this method.
+   * IF leftDistance and rightDistance are unspecified,
+   * You NEED to reset your encoders (to zero).
    *
    * 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 pose The position on the field that your robot is at.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
    */
-  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
+  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 = 0_m;
-    m_prevRightDistance = 0_m;
+    m_prevLeftDistance = leftDistance;
+    m_prevRightDistance = rightDistance;
   }
 
   /**
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
index 9a7cef9..2880cef 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
@@ -6,10 +6,12 @@
 
 #include <wpi/SymbolExports.h>
 
-#include "Eigen/Core"
 #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/MecanumDriveWheelPositions.h"
 #include "frc/kinematics/MecanumDriveWheelSpeeds.h"
 #include "wpimath/MathShared.h"
 
@@ -99,7 +101,7 @@
    */
   MecanumDriveWheelSpeeds ToWheelSpeeds(
       const ChassisSpeeds& chassisSpeeds,
-      const Translation2d& centerOfRotation = Translation2d()) const;
+      const Translation2d& centerOfRotation = Translation2d{}) const;
 
   /**
    * Performs forward kinematics to return the resulting chassis state from the
@@ -114,9 +116,21 @@
   ChassisSpeeds ToChassisSpeeds(
       const MecanumDriveWheelSpeeds& wheelSpeeds) const;
 
+  /**
+   * Performs forward kinematics to return the resulting Twist2d from the
+   * given wheel position deltas. This method is often used for odometry --
+   * determining the robot's position on the field using data from the
+   * distance driven by each wheel on the robot.
+   *
+   * @param wheelDeltas The change in distance driven by each wheel.
+   *
+   * @return The resulting chassis speed.
+   */
+  Twist2d ToTwist2d(const MecanumDriveWheelPositions& wheelDeltas) const;
+
  private:
-  mutable Eigen::Matrix<double, 4, 3> m_inverseKinematics;
-  Eigen::HouseholderQR<Eigen::Matrix<double, 4, 3>> m_forwardKinematics;
+  mutable Matrixd<4, 3> m_inverseKinematics;
+  Eigen::HouseholderQR<Matrixd<4, 3>> m_forwardKinematics;
   Translation2d m_frontLeftWheel;
   Translation2d m_frontRightWheel;
   Translation2d m_rearLeftWheel;
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
index bdd1239..5e949ca 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
@@ -30,11 +30,13 @@
    *
    * @param kinematics The mecanum drive 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 MecanumDriveOdometry(MecanumDriveKinematics kinematics,
-                                const Rotation2d& gyroAngle,
-                                const Pose2d& initialPose = Pose2d());
+  explicit MecanumDriveOdometry(
+      MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
+      const MecanumDriveWheelPositions& wheelPositions,
+      const Pose2d& initialPose = Pose2d{});
 
   /**
    * Resets the robot's position on the field.
@@ -42,13 +44,17 @@
    * 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 pose The position on the field that your robot is at.
    * @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 Pose2d& pose, const Rotation2d& gyroAngle) {
+  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;
   }
 
   /**
@@ -59,45 +65,23 @@
 
   /**
    * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method takes in the current time as
-   * a parameter to calculate period (difference between two timestamps). The
-   * period is used to calculate the change in distance from a velocity. This
-   * also takes in an angle parameter which is used instead of the
-   * angular rate that is calculated from forward kinematics.
-   *
-   * @param currentTime The current time.
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelSpeeds The current wheel speeds.
-   *
-   * @return The new pose of the robot.
-   */
-  const Pose2d& UpdateWithTime(units::second_t currentTime,
-                               const Rotation2d& gyroAngle,
-                               MecanumDriveWheelSpeeds wheelSpeeds);
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method automatically calculates
-   * the current time to calculate period (difference between two timestamps).
-   * The period is used to calculate the change in distance from a velocity.
-   * This also takes in an angle parameter which is used instead of the
-   * angular rate that is calculated from forward kinematics.
+   * 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 wheelSpeeds The current wheel speeds.
+   * @param wheelPositions The current distances measured by each wheel.
    *
    * @return The new pose of the robot.
    */
   const Pose2d& Update(const Rotation2d& gyroAngle,
-                       MecanumDriveWheelSpeeds wheelSpeeds) {
-    return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, wheelSpeeds);
-  }
+                       const MecanumDriveWheelPositions& wheelPositions);
 
  private:
   MecanumDriveKinematics m_kinematics;
   Pose2d m_pose;
 
-  units::second_t m_previousTime = -1_s;
+  MecanumDriveWheelPositions m_previousWheelPositions;
   Rotation2d m_previousAngle;
   Rotation2d m_gyroOffset;
 };
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
new file mode 100644
index 0000000..b69aceb
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
@@ -0,0 +1,53 @@
+// 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 "units/length.h"
+
+namespace frc {
+/**
+ * Represents the wheel speeds for a mecanum drive drivetrain.
+ */
+struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
+  /**
+   * Distance driven by the front-left wheel.
+   */
+  units::meter_t frontLeft = 0_m;
+
+  /**
+   * Distance driven by the front-right wheel.
+   */
+  units::meter_t frontRight = 0_m;
+
+  /**
+   * Distance driven by the rear-left wheel.
+   */
+  units::meter_t rearLeft = 0_m;
+
+  /**
+   * Distance driven by the rear-right wheel.
+   */
+  units::meter_t rearRight = 0_m;
+
+  /**
+   * Checks equality between this MecanumDriveWheelPositions and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const MecanumDriveWheelPositions& other) const = default;
+
+  /**
+   * Checks inequality between this MecanumDriveWheelPositions and another
+   * object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const MecanumDriveWheelPositions& other) const = default;
+};
+}  // 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 9801092..97ee233 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
@@ -6,13 +6,16 @@
 
 #include <cstddef>
 
+#include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "Eigen/Core"
 #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/SwerveModulePosition.h"
 #include "frc/kinematics/SwerveModuleState.h"
 #include "units/velocity.h"
 #include "wpimath/MathShared.h"
@@ -58,7 +61,7 @@
    */
   template <typename... Wheels>
   explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels)
-      : m_modules{wheel, wheels...} {
+      : m_modules{wheel, wheels...}, m_moduleStates(wpi::empty_array) {
     static_assert(sizeof...(wheels) >= 1,
                   "A swerve drive requires at least two modules");
 
@@ -78,7 +81,7 @@
 
   explicit SwerveDriveKinematics(
       const wpi::array<Translation2d, NumModules>& wheels)
-      : m_modules{wheels} {
+      : m_modules{wheels}, m_moduleStates(wpi::empty_array) {
     for (size_t i = 0; i < NumModules; i++) {
       // clang-format off
       m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
@@ -106,6 +109,9 @@
    * However, if you wish to change the center of rotation for evasive
    * maneuvers, vision alignment, or for any other use case, you can do so.
    *
+   * In the case that the desired chassis speeds are zero (i.e. the robot will
+   * be stationary), the previously calculated module angle will be maintained.
+   *
    * @param chassisSpeeds The desired chassis speed.
    * @param centerOfRotation The center of rotation. For example, if you set the
    * center of rotation at one corner of the robot and provide a chassis speed
@@ -125,7 +131,7 @@
    */
   wpi::array<SwerveModuleState, NumModules> ToSwerveModuleStates(
       const ChassisSpeeds& chassisSpeeds,
-      const Translation2d& centerOfRotation = Translation2d()) const;
+      const Translation2d& centerOfRotation = Translation2d{}) const;
 
   /**
    * Performs forward kinematics to return the resulting chassis state from the
@@ -159,6 +165,38 @@
       wpi::array<SwerveModuleState, NumModules> moduleStates) const;
 
   /**
+   * Performs forward kinematics to return the resulting Twist2d from the
+   * given module position deltas. This method is often used for odometry --
+   * 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
+   * 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;
+
+  /**
+   * Performs forward kinematics to return the resulting Twist2d from the
+   * given module position deltas. This method is often used for odometry --
+   * 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
+   * 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.
+   */
+  Twist2d ToTwist2d(
+      wpi::array<SwerveModulePosition, NumModules> wheelDeltas) const;
+
+  /**
    * Renormalizes the wheel speeds if any individual speed is above the
    * specified maximum.
    *
@@ -177,14 +215,46 @@
       wpi::array<SwerveModuleState, NumModules>* moduleStates,
       units::meters_per_second_t attainableMaxSpeed);
 
+  /**
+   * Renormalizes the wheel speeds if any individual speed is above the
+   * specified maximum, as well as getting rid of joystick saturation at edges
+   * of joystick.
+   *
+   * Sometimes, after inverse kinematics, the requested speed
+   * from one or more modules may be above the max attainable speed for the
+   * driving motor on that module. To fix this issue, one can reduce all the
+   * wheel speeds to make sure that all requested module speeds are at-or-below
+   * the absolute threshold, while maintaining the ratio of speeds between
+   * modules.
+   *
+   * @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 attainableMaxModuleSpeed The absolute max speed a module can reach
+   * @param attainableMaxRobotTranslationSpeed The absolute max speed the robot
+   * can reach while translating
+   * @param attainableMaxRobotRotationSpeed The absolute max speed the robot can
+   * reach while rotating
+   */
+  static void DesaturateWheelSpeeds(
+      wpi::array<SwerveModuleState, NumModules>* moduleStates,
+      ChassisSpeeds currentChassisSpeed,
+      units::meters_per_second_t attainableMaxModuleSpeed,
+      units::meters_per_second_t attainableMaxRobotTranslationSpeed,
+      units::radians_per_second_t attainableMaxRobotRotationSpeed);
+
  private:
-  mutable Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
-  Eigen::HouseholderQR<Eigen::Matrix<double, NumModules * 2, 3>>
-      m_forwardKinematics;
+  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 Translation2d m_previousCoR;
 };
+
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    SwerveDriveKinematics<4>;
+
 }  // namespace frc
 
 #include "SwerveDriveKinematics.inc"
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
index 7b958c1..c7f26e0 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
@@ -5,7 +5,9 @@
 #pragma once
 
 #include <algorithm>
+#include <utility>
 
+#include "frc/kinematics/ChassisSpeeds.h"
 #include "frc/kinematics/SwerveDriveKinematics.h"
 #include "units/math.h"
 
@@ -20,12 +22,21 @@
 SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
     const ChassisSpeeds& chassisSpeeds,
     const Translation2d& centerOfRotation) const {
+  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;
+    }
+
+    return m_moduleStates;
+  }
+
   // We have a new center of rotation. We need to compute the matrix again.
   if (centerOfRotation != m_previousCoR) {
     for (size_t i = 0; i < NumModules; i++) {
       // clang-format off
       m_inverseKinematics.template block<2, 3>(i * 2, 0) =
-        Eigen::Matrix<double, 2, 3>{
+        Matrixd<2, 3>{
           {1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).value()},
           {0, 1, (+m_modules[i].X() - centerOfRotation.X()).value()}};
       // clang-format on
@@ -37,21 +48,20 @@
                                       chassisSpeeds.vy.value(),
                                       chassisSpeeds.omega.value()};
 
-  Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix =
+  Matrixd<NumModules * 2, 1> moduleStateMatrix =
       m_inverseKinematics * chassisSpeedsVector;
-  wpi::array<SwerveModuleState, NumModules> moduleStates{wpi::empty_array};
 
   for (size_t i = 0; i < NumModules; i++) {
-    units::meters_per_second_t x{moduleStatesMatrix(i * 2, 0)};
-    units::meters_per_second_t y{moduleStatesMatrix(i * 2 + 1, 0)};
+    units::meters_per_second_t x{moduleStateMatrix(i * 2, 0)};
+    units::meters_per_second_t y{moduleStateMatrix(i * 2 + 1, 0)};
 
     auto speed = units::math::hypot(x, y);
     Rotation2d rotation{x.value(), y.value()};
 
-    moduleStates[i] = {speed, rotation};
+    m_moduleStates[i] = {speed, rotation};
   }
 
-  return moduleStates;
+  return m_moduleStates;
 }
 
 template <size_t NumModules>
@@ -70,17 +80,16 @@
 template <size_t NumModules>
 ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
     wpi::array<SwerveModuleState, NumModules> moduleStates) const {
-  Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
+  Matrixd<NumModules * 2, 1> moduleStateMatrix;
 
   for (size_t i = 0; i < NumModules; ++i) {
     SwerveModuleState module = moduleStates[i];
-    moduleStatesMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos();
-    moduleStatesMatrix(i * 2 + 1, 0) =
-        module.speed.value() * module.angle.Sin();
+    moduleStateMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos();
+    moduleStateMatrix(i * 2 + 1, 0) = module.speed.value() * module.angle.Sin();
   }
 
   Eigen::Vector3d chassisSpeedsVector =
-      m_forwardKinematics.solve(moduleStatesMatrix);
+      m_forwardKinematics.solve(moduleStateMatrix);
 
   return {units::meters_per_second_t{chassisSpeedsVector(0)},
           units::meters_per_second_t{chassisSpeedsVector(1)},
@@ -88,6 +97,39 @@
 }
 
 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;
+
+  for (size_t i = 0; i < NumModules; ++i) {
+    SwerveModulePosition module = moduleDeltas[i];
+    moduleDeltaMatrix(i * 2, 0) = module.distance.value() * module.angle.Cos();
+    moduleDeltaMatrix(i * 2 + 1, 0) =
+        module.distance.value() * module.angle.Sin();
+  }
+
+  Eigen::Vector3d chassisDeltaVector =
+      m_forwardKinematics.solve(moduleDeltaMatrix);
+
+  return {units::meter_t{chassisDeltaVector(0)},
+          units::meter_t{chassisDeltaVector(1)},
+          units::radian_t{chassisDeltaVector(2)}};
+}
+
+template <size_t NumModules>
 void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
     wpi::array<SwerveModuleState, NumModules>* moduleStates,
     units::meters_per_second_t attainableMaxSpeed) {
@@ -106,4 +148,41 @@
   }
 }
 
+template <size_t NumModules>
+void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
+    wpi::array<SwerveModuleState, NumModules>* moduleStates,
+    ChassisSpeeds currentChassisSpeed,
+    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;
+
+  if (attainableMaxRobotTranslationSpeed == 0_mps ||
+      attainableMaxRobotRotationSpeed == 0_rad_per_s || realMaxSpeed == 0_mps) {
+    return;
+  }
+
+  auto translationalK =
+      units::math::hypot(currentChassisSpeed.vx, currentChassisSpeed.vy) /
+      attainableMaxRobotTranslationSpeed;
+
+  auto rotationalK = units::math::abs(currentChassisSpeed.omega) /
+                     attainableMaxRobotRotationSpeed;
+
+  auto k = units::math::max(translationalK, rotationalK);
+
+  auto scale = units::math::min(k * attainableMaxModuleSpeed / realMaxSpeed,
+                                units::scalar_t(1));
+  for (auto& module : states) {
+    module.speed = module.speed * scale;
+  }
+}
+
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
index d1a4958..015c2c0 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
@@ -8,10 +8,11 @@
 #include <cstddef>
 #include <ctime>
 
+#include <wpi/SymbolExports.h>
 #include <wpi/timestamp.h>
 
 #include "SwerveDriveKinematics.h"
-#include "SwerveModuleState.h"
+#include "SwerveModulePosition.h"
 #include "frc/geometry/Pose2d.h"
 #include "units/time.h"
 
@@ -34,11 +35,13 @@
    *
    * @param kinematics The swerve drive kinematics for your drivetrain.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param modulePositions The wheel positions reported by each module.
    * @param initialPose The starting position of the robot on the field.
    */
-  SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
-                      const Rotation2d& gyroAngle,
-                      const Pose2d& initialPose = Pose2d());
+  SwerveDriveOdometry(
+      SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
+      const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
+      const Pose2d& initialPose = Pose2d{});
 
   /**
    * Resets the robot's position on the field.
@@ -46,14 +49,14 @@
    * 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 pose The position on the field that your robot is at.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param modulePositions The wheel positions reported by each module.
+   * @param pose The position on the field that your robot is at.
    */
-  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
-    m_pose = pose;
-    m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
-  }
+  void ResetPosition(
+      const Rotation2d& gyroAngle,
+      const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
+      const Pose2d& pose);
 
   /**
    * Returns the position of the robot on the field.
@@ -63,55 +66,35 @@
 
   /**
    * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method takes in the current time as
-   * a parameter to calculate period (difference between two timestamps). The
-   * period is used to calculate the change in distance from a velocity. This
-   * also takes in an angle parameter which is used instead of the
-   * angular rate that is calculated from forward kinematics.
+   * integration of the pose over time. This also takes in an angle parameter
+   * which is used instead of the angular rate that is calculated from forward
+   * kinematics.
    *
-   * @param currentTime The current time.
    * @param gyroAngle The angle reported by the gyroscope.
-   * @param moduleStates The current state of all swerve modules. Please provide
-   *                     the states in the same order in which you instantiated
-   *                     your SwerveDriveKinematics.
+   * @param modulePositions The current position of all swerve modules. Please
+   * provide the positions in the same order in which you instantiated your
+   * SwerveDriveKinematics.
    *
    * @return The new pose of the robot.
    */
-  template <typename... ModuleStates>
-  const Pose2d& UpdateWithTime(units::second_t currentTime,
-                               const Rotation2d& gyroAngle,
-                               ModuleStates&&... moduleStates);
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method automatically calculates
-   * the current time to calculate period (difference between two timestamps).
-   * The period is used to calculate the change in distance from a velocity.
-   * This also takes in an angle parameter which is used instead of the
-   * angular rate that is calculated from forward kinematics.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param moduleStates The current state of all swerve modules. Please provide
-   *                     the states in the same order in which you instantiated
-   *                     your SwerveDriveKinematics.
-   *
-   * @return The new pose of the robot.
-   */
-  template <typename... ModuleStates>
-  const Pose2d& Update(const Rotation2d& gyroAngle,
-                       ModuleStates&&... moduleStates) {
-    return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, moduleStates...);
-  }
+  const Pose2d& Update(
+      const Rotation2d& gyroAngle,
+      const wpi::array<SwerveModulePosition, NumModules>& modulePositions);
 
  private:
   SwerveDriveKinematics<NumModules> m_kinematics;
   Pose2d m_pose;
 
-  units::second_t m_previousTime = -1_s;
   Rotation2d m_previousAngle;
   Rotation2d m_gyroOffset;
+
+  wpi::array<SwerveModulePosition, NumModules> m_previousModulePositions{
+      wpi::empty_array};
 };
 
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    SwerveDriveOdometry<4>;
+
 }  // namespace frc
 
 #include "SwerveDriveOdometry.inc"
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
index 96db930..64b46c1 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
@@ -11,30 +11,56 @@
 template <size_t NumModules>
 SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
     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};
+  }
+
   wpi::math::MathSharedStore::ReportUsage(
       wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
 }
 
 template <size_t NumModules>
-template <typename... ModuleStates>
-const Pose2d& frc::SwerveDriveOdometry<NumModules>::UpdateWithTime(
-    units::second_t currentTime, const Rotation2d& gyroAngle,
-    ModuleStates&&... moduleStates) {
-  units::second_t deltaTime =
-      (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
-  m_previousTime = currentTime;
+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 [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...);
-  static_cast<void>(dtheta);
+  auto twist = m_kinematics.ToTwist2d(moduleDeltas);
+  twist.dtheta = (angle - m_previousAngle).Radians();
 
-  auto newPose = m_pose.Exp(
-      {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
+  auto newPose = m_pose.Exp(twist);
 
   m_previousAngle = angle;
   m_pose = {newPose.Translation(), angle};
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
new file mode 100644
index 0000000..18ed464
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
@@ -0,0 +1,29 @@
+// 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/Rotation2d.h"
+#include "units/angle.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+/**
+ * Represents the position of one swerve module.
+ */
+struct WPILIB_DLLEXPORT SwerveModulePosition {
+  /**
+   * Distance the wheel of a module has traveled
+   */
+  units::meter_t distance = 0_m;
+
+  /**
+   * Angle of the module.
+   */
+  Rotation2d angle;
+};
+}  // 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 8126349..0636707 100644
--- a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
+++ b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
@@ -7,7 +7,7 @@
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/spline/Spline.h"
 
 namespace frc {
@@ -40,44 +40,41 @@
    * Returns the coefficients matrix.
    * @return The coefficients matrix.
    */
-  Eigen::Matrix<double, 6, 3 + 1> Coefficients() const override {
-    return m_coefficients;
-  }
+  Matrixd<6, 3 + 1> Coefficients() const override { return m_coefficients; }
 
  private:
-  Eigen::Matrix<double, 6, 4> m_coefficients =
-      Eigen::Matrix<double, 6, 4>::Zero();
+  Matrixd<6, 4> m_coefficients = Matrixd<6, 4>::Zero();
 
   /**
    * Returns the hermite basis matrix for cubic hermite spline interpolation.
    * @return The hermite basis matrix for cubic hermite spline interpolation.
    */
-  static Eigen::Matrix<double, 4, 4> MakeHermiteBasis() {
+  static Matrixd<4, 4> 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) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
+    // the coefficients of the spline P(t) = a₃t³ + a₂t² + a₁t + a₀.
     //
-    // P(i)    = P(0)  = a0
-    // P'(i)   = P'(0) = a1
-    // P(i+1)  = P(1)  = a3 + a2 + a1 + a0
-    // P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1
+    // P(i)    = P(0)  = a₀
+    // P'(i)   = P'(0) = a₁
+    // P(i+1)  = P(1)  = a₃ + a₂ + a₁ + a₀
+    // P'(i+1) = P'(1) = 3a₃ + 2a₂ + a₁
     //
-    // [ P(i)    ] = [ 0 0 0 1 ][ a3 ]
-    // [ P'(i)   ] = [ 0 0 1 0 ][ a2 ]
-    // [ P(i+1)  ] = [ 1 1 1 1 ][ a1 ]
-    // [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ]
+    // [P(i)   ] = [0 0 0 1][a₃]
+    // [P'(i)  ] = [0 0 1 0][a₂]
+    // [P(i+1) ] = [1 1 1 1][a₁]
+    // [P'(i+1)] = [3 2 1 0][a₀]
     //
     // To solve for the coefficients, we can invert the 4x4 matrix and move it
     // to the other side of the equation.
     //
-    // [ a3 ] = [  2  1 -2  1 ][ P(i)    ]
-    // [ a2 ] = [ -3 -2  3 -1 ][ P'(i)   ]
-    // [ a1 ] = [  0  1  0  0 ][ P(i+1)  ]
-    // [ a0 ] = [  1  0  0  0 ][ P'(i+1) ]
+    // [a₃] = [ 2  1 -2  1][P(i)   ]
+    // [a₂] = [-3 -2  3 -1][P'(i)  ]
+    // [a₁] = [ 0  1  0  0][P(i+1) ]
+    // [a₀] = [ 1  0  0  0][P'(i+1)]
 
-    static const Eigen::Matrix<double, 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 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}};
     return basis;
   }
 
diff --git a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
index 5ba3e2a..ad03a23 100644
--- a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
+++ b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
@@ -7,7 +7,7 @@
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/spline/Spline.h"
 
 namespace frc {
@@ -40,48 +40,45 @@
    * Returns the coefficients matrix.
    * @return The coefficients matrix.
    */
-  Eigen::Matrix<double, 6, 6> Coefficients() const override {
-    return m_coefficients;
-  }
+  Matrixd<6, 6> Coefficients() const override { return m_coefficients; }
 
  private:
-  Eigen::Matrix<double, 6, 6> m_coefficients =
-      Eigen::Matrix<double, 6, 6>::Zero();
+  Matrixd<6, 6> m_coefficients = Matrixd<6, 6>::Zero();
 
   /**
    * Returns the hermite basis matrix for quintic hermite spline interpolation.
    * @return The hermite basis matrix for quintic hermite spline interpolation.
    */
-  static Eigen::Matrix<double, 6, 6> MakeHermiteBasis() {
-    // Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
-    // vectors, we want to find the coefficients of the spline
-    // P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
+  static Matrixd<6, 6> MakeHermiteBasis() {
+    // Given P(i), P'(i), P"(i), P(i+1), 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₂t² + a₁t + a₀.
     //
-    // P(i)     = P(0)   = a0
-    // P'(i)    = P'(0)  = a1
-    // P''(i)   = P''(0) = 2 * a2
-    // P(i+1)   = P(1)   = a5 + a4 + a3 + a2 + a1 + a0
-    // P'(i+1)  = P'(1)  = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1
-    // P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2
+    // P(i)    = P(0)  = a₀
+    // P'(i)   = P'(0) = a₁
+    // P''(i)  = P"(0) = 2a₂
+    // P(i+1)  = P(1)  = a₅ + a₄ + a₃ + a₂ + a₁ + a₀
+    // P'(i+1) = P'(1) = 5a₅ + 4a₄ + 3a₃ + 2a₂ + a₁
+    // P"(i+1) = P"(1) = 20a₅ + 12a₄ + 6a₃ + 2a₂
     //
-    // [ P(i)     ] = [  0  0  0  0  0  1 ][ a5 ]
-    // [ P'(i)    ] = [  0  0  0  0  1  0 ][ a4 ]
-    // [ P''(i)   ] = [  0  0  0  2  0  0 ][ a3 ]
-    // [ P(i+1)   ] = [  1  1  1  1  1  1 ][ a2 ]
-    // [ P'(i+1)  ] = [  5  4  3  2  1  0 ][ a1 ]
-    // [ P''(i+1) ] = [ 20 12  6  2  0  0 ][ a0 ]
+    // [P(i)   ] = [ 0  0  0  0  0  1][a₅]
+    // [P'(i)  ] = [ 0  0  0  0  1  0][a₄]
+    // [P"(i)  ] = [ 0  0  0  2  0  0][a₃]
+    // [P(i+1) ] = [ 1  1  1  1  1  1][a₂]
+    // [P'(i+1)] = [ 5  4  3  2  1  0][a₁]
+    // [P"(i+1)] = [20 12  6  2  0  0][a₀]
     //
     // To solve for the coefficients, we can invert the 6x6 matrix and move it
     // to the other side of the equation.
     //
-    // [ a5 ] = [  -6.0  -3.0  -0.5   6.0  -3.0   0.5 ][ P(i)     ]
-    // [ a4 ] = [  15.0   8.0   1.5 -15.0   7.0  -1.0 ][ P'(i)    ]
-    // [ a3 ] = [ -10.0  -6.0  -1.5  10.0  -4.0   0.5 ][ P''(i)   ]
-    // [ a2 ] = [   0.0   0.0   0.5   0.0   0.0   0.0 ][ P(i+1)   ]
-    // [ a1 ] = [   0.0   1.0   0.0   0.0   0.0   0.0 ][ P'(i+1)  ]
-    // [ a0 ] = [   1.0   0.0   0.0   0.0   0.0   0.0 ][ P''(i+1) ]
+    // [a₅] = [ -6.0  -3.0  -0.5   6.0  -3.0   0.5][P(i)   ]
+    // [a₄] = [ 15.0   8.0   1.5 -15.0   7.0  -1.0][P'(i)  ]
+    // [a₃] = [-10.0  -6.0  -1.5  10.0  -4.0   0.5][P"(i)  ]
+    // [a₂] = [  0.0   0.0   0.5   0.0   0.0   0.0][P(i+1) ]
+    // [a₁] = [  0.0   1.0   0.0   0.0   0.0   0.0][P'(i+1)]
+    // [a₀] = [  1.0   0.0   0.0   0.0   0.0   0.0][P"(i+1)]
 
-    static const Eigen::Matrix<double, 6, 6> basis{
+    static const Matrixd<6, 6> basis{
         {-06.0, -03.0, -00.5, +06.0, -03.0, +00.5},
         {+15.0, +08.0, +01.5, -15.0, +07.0, -01.0},
         {-10.0, -06.0, -01.5, +10.0, -04.0, +00.5},
@@ -100,11 +97,10 @@
    *
    * @return The control vector matrix for a dimension.
    */
-  static Eigen::Vector<double, 6> ControlVectorFromArrays(
-      wpi::array<double, 3> initialVector, wpi::array<double, 3> finalVector) {
-    return Eigen::Vector<double, 6>{initialVector[0], initialVector[1],
-                                    initialVector[2], finalVector[0],
-                                    finalVector[1],   finalVector[2]};
+  static Vectord<6> ControlVectorFromArrays(wpi::array<double, 3> initialVector,
+                                            wpi::array<double, 3> finalVector) {
+    return Vectord<6>{initialVector[0], initialVector[1], initialVector[2],
+                      finalVector[0],   finalVector[1],   finalVector[2]};
   }
 };
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/spline/Spline.h b/wpimath/src/main/native/include/frc/spline/Spline.h
index 2dd248a..b5ff024 100644
--- a/wpimath/src/main/native/include/frc/spline/Spline.h
+++ b/wpimath/src/main/native/include/frc/spline/Spline.h
@@ -9,7 +9,7 @@
 
 #include <wpi/array.h>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/geometry/Pose2d.h"
 #include "units/curvature.h"
 #include "units/length.h"
@@ -55,7 +55,7 @@
    * @return The pose and curvature at that point.
    */
   PoseWithCurvature GetPoint(double t) const {
-    Eigen::Vector<double, Degree + 1> polynomialBases;
+    Vectord<Degree + 1> polynomialBases;
 
     // Populate the polynomial bases
     for (int i = 0; i <= Degree; i++) {
@@ -64,7 +64,7 @@
 
     // This simply multiplies by the coefficients. We need to divide out t some
     // n number of times where n is the derivative we want to take.
-    Eigen::Vector<double, 6> combined = Coefficients() * polynomialBases;
+    Vectord<6> combined = Coefficients() * polynomialBases;
 
     double dx, dy, ddx, ddy;
 
@@ -90,8 +90,8 @@
         (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));
 
     return {
-        {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d(dx, dy)},
-        units::curvature_t(curvature)};
+        {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d{dx, dy}},
+        units::curvature_t{curvature}};
   }
 
  protected:
@@ -100,7 +100,7 @@
    *
    * @return The coefficients of the spline.
    */
-  virtual Eigen::Matrix<double, 6, Degree + 1> Coefficients() const = 0;
+  virtual Matrixd<6, Degree + 1> Coefficients() const = 0;
 
   /**
    * Converts a Translation2d into a vector that is compatible with Eigen.
@@ -119,7 +119,7 @@
    * @return The Translation2d.
    */
   static Translation2d FromVector(const Eigen::Vector2d& vector) {
-    return Translation2d(units::meter_t(vector(0)), units::meter_t(vector(1)));
+    return Translation2d{units::meter_t{vector(0)}, units::meter_t{vector(1)}};
   }
 };
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/system/Discretization.h b/wpimath/src/main/native/include/frc/system/Discretization.h
index 722bb5f..957b875 100644
--- a/wpimath/src/main/native/include/frc/system/Discretization.h
+++ b/wpimath/src/main/native/include/frc/system/Discretization.h
@@ -4,7 +4,7 @@
 
 #pragma once
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "units/time.h"
 #include "unsupported/Eigen/MatrixFunctions"
 
@@ -19,9 +19,9 @@
  * @param discA Storage for discrete system matrix.
  */
 template <int States>
-void DiscretizeA(const Eigen::Matrix<double, States, States>& contA,
-                 units::second_t dt,
-                 Eigen::Matrix<double, States, States>* discA) {
+void DiscretizeA(const Matrixd<States, States>& contA, units::second_t dt,
+                 Matrixd<States, States>* discA) {
+  // A_d = eᴬᵀ
   *discA = (contA * dt.value()).exp();
 }
 
@@ -37,21 +37,23 @@
  * @param discB Storage for discrete input matrix.
  */
 template <int States, int Inputs>
-void DiscretizeAB(const Eigen::Matrix<double, States, States>& contA,
-                  const Eigen::Matrix<double, States, Inputs>& contB,
-                  units::second_t dt,
-                  Eigen::Matrix<double, States, States>* discA,
-                  Eigen::Matrix<double, States, Inputs>* discB) {
-  // Matrices are blocked here to minimize matrix exponentiation calculations
-  Eigen::Matrix<double, States + Inputs, States + Inputs> Mcont;
-  Mcont.setZero();
-  Mcont.template block<States, States>(0, 0) = contA * dt.value();
-  Mcont.template block<States, Inputs>(0, States) = contB * dt.value();
+void DiscretizeAB(const Matrixd<States, States>& contA,
+                  const Matrixd<States, Inputs>& contB, units::second_t dt,
+                  Matrixd<States, States>* discA,
+                  Matrixd<States, Inputs>* discB) {
+  // 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;
 
-  // Discretize A and B with the given timestep
-  Eigen::Matrix<double, States + Inputs, States + Inputs> Mdisc = Mcont.exp();
-  *discA = Mdisc.template block<States, States>(0, 0);
-  *discB = Mdisc.template block<States, Inputs>(0, States);
+  // ϕ = eᴹᵀ = [A_d  B_d]
+  //           [ 0    I ]
+  Matrixd<States + Inputs, States + Inputs> phi = (M * dt.value()).exp();
+
+  *discA = phi.template block<States, States>(0, 0);
+  *discB = phi.template block<States, Inputs>(0, States);
 }
 
 /**
@@ -65,29 +67,30 @@
  * @param discQ Storage for discrete process noise covariance matrix.
  */
 template <int States>
-void DiscretizeAQ(const Eigen::Matrix<double, States, States>& contA,
-                  const Eigen::Matrix<double, States, States>& contQ,
-                  units::second_t dt,
-                  Eigen::Matrix<double, States, States>* discA,
-                  Eigen::Matrix<double, States, States>* discQ) {
+void DiscretizeAQ(const Matrixd<States, States>& contA,
+                  const Matrixd<States, States>& contQ, units::second_t dt,
+                  Matrixd<States, States>* discA,
+                  Matrixd<States, States>* discQ) {
   // Make continuous Q symmetric if it isn't already
-  Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
+  Matrixd<States, States> Q = (contQ + contQ.transpose()) / 2.0;
 
-  // Set up the matrix M = [[-A, Q], [0, A.T]]
-  Eigen::Matrix<double, 2 * States, 2 * States> M;
+  // M = [−A  Q ]
+  //     [ 0  Aᵀ]
+  Matrixd<2 * States, 2 * States> M;
   M.template block<States, States>(0, 0) = -contA;
   M.template block<States, States>(0, States) = Q;
   M.template block<States, States>(States, 0).setZero();
   M.template block<States, States>(States, States) = contA.transpose();
 
-  Eigen::Matrix<double, 2 * States, 2 * States> phi = (M * dt.value()).exp();
+  // ϕ = eᴹᵀ = [−A_d  A_d⁻¹Q_d]
+  //           [ 0      A_dᵀ  ]
+  Matrixd<2 * States, 2 * States> phi = (M * dt.value()).exp();
 
-  // Phi12 = phi[0:States,        States:2*States]
-  // Phi22 = phi[States:2*States, States:2*States]
-  Eigen::Matrix<double, States, States> phi12 =
-      phi.block(0, States, States, States);
-  Eigen::Matrix<double, States, States> phi22 =
-      phi.block(States, States, States, States);
+  // ϕ₁₂ = A_d⁻¹Q_d
+  Matrixd<States, States> phi12 = phi.block(0, States, States, States);
+
+  // ϕ₂₂ = A_dᵀ
+  Matrixd<States, States> phi22 = phi.block(States, States, States, States);
 
   *discA = phi22.transpose();
 
@@ -104,10 +107,12 @@
  * (which is expensive), we take advantage of the structure of the block matrix
  * of A and Q.
  *
- * 1) The exponential of A*t, which is only N x N, is relatively cheap.
- * 2) 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>
+ *   <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.
@@ -117,30 +122,64 @@
  * @param discQ Storage for discrete process noise covariance matrix.
  */
 template <int States>
-void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
-                        const Eigen::Matrix<double, States, States>& contQ,
-                        units::second_t dt,
-                        Eigen::Matrix<double, States, States>* discA,
-                        Eigen::Matrix<double, States, States>* discQ) {
-  // Make continuous Q symmetric if it isn't already
-  Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
+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ᵀ² = …
+  //   …
+  // ```
 
-  Eigen::Matrix<double, States, States> lastTerm = Q;
+  // 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ᵀⁿ
-  Eigen::Matrix<double, States, States> Atn = contA.transpose();
+  Matrixd<States, States> ATn = contA.transpose();
 
-  Eigen::Matrix<double, States, States> phi12 = lastTerm * lastCoeff;
+  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;
+    lastTerm = -contA * lastTerm + Q * ATn;
     lastCoeff *= dt.value() / static_cast<double>(i);
 
     phi12 += lastTerm * lastCoeff;
 
-    Atn *= contA.transpose();
+    ATn *= contA.transpose();
   }
 
   DiscretizeA<States>(contA, dt, discA);
@@ -159,8 +198,9 @@
  * @param dt Discretization timestep.
  */
 template <int Outputs>
-Eigen::Matrix<double, Outputs, Outputs> DiscretizeR(
-    const Eigen::Matrix<double, Outputs, Outputs>& R, units::second_t dt) {
+Matrixd<Outputs, Outputs> DiscretizeR(const Matrixd<Outputs, Outputs>& R,
+                                      units::second_t dt) {
+  // R_d = 1/T R
   return R / dt.value();
 }
 
diff --git a/wpimath/src/main/native/include/frc/system/LinearSystem.h b/wpimath/src/main/native/include/frc/system/LinearSystem.h
index bd3ff40..dff2433 100644
--- a/wpimath/src/main/native/include/frc/system/LinearSystem.h
+++ b/wpimath/src/main/native/include/frc/system/LinearSystem.h
@@ -8,7 +8,7 @@
 #include <functional>
 #include <stdexcept>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/system/Discretization.h"
 #include "units/time.h"
@@ -30,6 +30,10 @@
 template <int States, int Inputs, int Outputs>
 class LinearSystem {
  public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+  using OutputVector = Vectord<Outputs>;
+
   /**
    * Constructs a discrete plant with the given continuous system coefficients.
    *
@@ -39,10 +43,10 @@
    * @param D    Feedthrough matrix.
    * @throws std::domain_error if any matrix element isn't finite.
    */
-  LinearSystem(const Eigen::Matrix<double, States, States>& A,
-               const Eigen::Matrix<double, States, Inputs>& B,
-               const Eigen::Matrix<double, Outputs, States>& C,
-               const Eigen::Matrix<double, Outputs, Inputs>& D) {
+  LinearSystem(const Matrixd<States, States>& A,
+               const Matrixd<States, Inputs>& B,
+               const Matrixd<Outputs, States>& C,
+               const Matrixd<Outputs, Inputs>& D) {
     if (!A.allFinite()) {
       throw std::domain_error(
           "Elements of A aren't finite. This is usually due to model "
@@ -78,7 +82,7 @@
   /**
    * Returns the system matrix A.
    */
-  const Eigen::Matrix<double, States, States>& A() const { return m_A; }
+  const Matrixd<States, States>& A() const { return m_A; }
 
   /**
    * Returns an element of the system matrix A.
@@ -91,7 +95,7 @@
   /**
    * Returns the input matrix B.
    */
-  const Eigen::Matrix<double, States, Inputs>& B() const { return m_B; }
+  const Matrixd<States, Inputs>& B() const { return m_B; }
 
   /**
    * Returns an element of the input matrix B.
@@ -104,7 +108,7 @@
   /**
    * Returns the output matrix C.
    */
-  const Eigen::Matrix<double, Outputs, States>& C() const { return m_C; }
+  const Matrixd<Outputs, States>& C() const { return m_C; }
 
   /**
    * Returns an element of the output matrix C.
@@ -117,7 +121,7 @@
   /**
    * Returns the feedthrough matrix D.
    */
-  const Eigen::Matrix<double, Outputs, Inputs>& D() const { return m_D; }
+  const Matrixd<Outputs, Inputs>& D() const { return m_D; }
 
   /**
    * Returns an element of the feedthrough matrix D.
@@ -137,11 +141,10 @@
    * @param clampedU The control input.
    * @param dt       Timestep for model update.
    */
-  Eigen::Vector<double, States> CalculateX(
-      const Eigen::Vector<double, States>& x,
-      const Eigen::Vector<double, Inputs>& clampedU, units::second_t dt) const {
-    Eigen::Matrix<double, States, States> discA;
-    Eigen::Matrix<double, States, Inputs> discB;
+  StateVector CalculateX(const StateVector& x, const InputVector& clampedU,
+                         units::second_t dt) const {
+    Matrixd<States, States> discA;
+    Matrixd<States, Inputs> discB;
     DiscretizeAB<States, Inputs>(m_A, m_B, dt, &discA, &discB);
 
     return discA * x + discB * clampedU;
@@ -156,9 +159,8 @@
    * @param x The current state.
    * @param clampedU The control input.
    */
-  Eigen::Vector<double, Outputs> CalculateY(
-      const Eigen::Vector<double, States>& x,
-      const Eigen::Vector<double, Inputs>& clampedU) const {
+  OutputVector CalculateY(const StateVector& x,
+                          const InputVector& clampedU) const {
     return m_C * x + m_D * clampedU;
   }
 
@@ -166,22 +168,22 @@
   /**
    * Continuous system matrix.
    */
-  Eigen::Matrix<double, States, States> m_A;
+  Matrixd<States, States> m_A;
 
   /**
    * Continuous input matrix.
    */
-  Eigen::Matrix<double, States, Inputs> m_B;
+  Matrixd<States, Inputs> m_B;
 
   /**
    * Output matrix.
    */
-  Eigen::Matrix<double, Outputs, States> m_C;
+  Matrixd<Outputs, States> m_C;
 
   /**
    * Feedthrough matrix.
    */
-  Eigen::Matrix<double, Outputs, Inputs> m_D;
+  Matrixd<Outputs, Inputs> m_D;
 };
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
index a18c03b..1300a82 100644
--- a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
+++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
@@ -4,7 +4,9 @@
 
 #pragma once
 
-#include "Eigen/Core"
+#include <wpi/SymbolExports.h>
+
+#include "frc/EigenCore.h"
 #include "frc/controller/LinearPlantInversionFeedforward.h"
 #include "frc/controller/LinearQuadraticRegulator.h"
 #include "frc/estimator/KalmanFilter.h"
@@ -35,6 +37,10 @@
 template <int States, int Inputs, int Outputs>
 class LinearSystemLoop {
  public:
+  using StateVector = Vectord<States>;
+  using InputVector = Vectord<Inputs>;
+  using OutputVector = Vectord<Outputs>;
+
   /**
    * Constructs a state-space loop with the given plant, controller, and
    * observer. By default, the initial reference is all zeros. Users should
@@ -53,7 +59,7 @@
                    units::volt_t maxVoltage, units::second_t dt)
       : LinearSystemLoop(
             plant, controller, observer,
-            [=](const Eigen::Vector<double, Inputs>& u) {
+            [=](const InputVector& u) {
               return frc::DesaturateInputVector<Inputs>(u, maxVoltage.value());
             },
             dt) {}
@@ -73,9 +79,7 @@
   LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
                    LinearQuadraticRegulator<States, Inputs>& controller,
                    KalmanFilter<States, Inputs, Outputs>& observer,
-                   std::function<Eigen::Vector<double, Inputs>(
-                       const Eigen::Vector<double, Inputs>&)>
-                       clampFunction,
+                   std::function<InputVector(const InputVector&)> clampFunction,
                    units::second_t dt)
       : LinearSystemLoop(
             controller,
@@ -97,11 +101,10 @@
       LinearQuadraticRegulator<States, Inputs>& controller,
       const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
       KalmanFilter<States, Inputs, Outputs>& observer, units::volt_t maxVoltage)
-      : LinearSystemLoop(controller, feedforward, observer,
-                         [=](const Eigen::Vector<double, Inputs>& u) {
-                           return frc::DesaturateInputVector<Inputs>(
-                               u, maxVoltage.value());
-                         }) {}
+      : LinearSystemLoop(
+            controller, feedforward, observer, [=](const InputVector& u) {
+              return frc::DesaturateInputVector<Inputs>(u, maxVoltage.value());
+            }) {}
 
   /**
    * Constructs a state-space loop with the given controller, feedforward,
@@ -117,9 +120,7 @@
       LinearQuadraticRegulator<States, Inputs>& controller,
       const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
       KalmanFilter<States, Inputs, Outputs>& observer,
-      std::function<
-          Eigen::Vector<double, Inputs>(const Eigen::Vector<double, Inputs>&)>
-          clampFunction)
+      std::function<InputVector(const InputVector&)> clampFunction)
       : m_controller(&controller),
         m_feedforward(feedforward),
         m_observer(&observer),
@@ -134,9 +135,7 @@
   /**
    * Returns the observer's state estimate x-hat.
    */
-  const Eigen::Vector<double, States>& Xhat() const {
-    return m_observer->Xhat();
-  }
+  const StateVector& Xhat() const { return m_observer->Xhat(); }
 
   /**
    * Returns an element of the observer's state estimate x-hat.
@@ -148,7 +147,7 @@
   /**
    * Returns the controller's next reference r.
    */
-  const Eigen::Vector<double, States>& NextR() const { return m_nextR; }
+  const StateVector& NextR() const { return m_nextR; }
 
   /**
    * Returns an element of the controller's next reference r.
@@ -160,7 +159,7 @@
   /**
    * Returns the controller's calculated control input u.
    */
-  Eigen::Vector<double, Inputs> U() const {
+  InputVector U() const {
     return ClampInput(m_controller->U() + m_feedforward.Uff());
   }
 
@@ -176,9 +175,7 @@
    *
    * @param xHat The initial state estimate x-hat.
    */
-  void SetXhat(const Eigen::Vector<double, States>& xHat) {
-    m_observer->SetXhat(xHat);
-  }
+  void SetXhat(const StateVector& xHat) { m_observer->SetXhat(xHat); }
 
   /**
    * Set an element of the initial state estimate x-hat.
@@ -193,7 +190,7 @@
    *
    * @param nextR Next reference.
    */
-  void SetNextR(const Eigen::Vector<double, States>& nextR) { m_nextR = nextR; }
+  void SetNextR(const StateVector& nextR) { m_nextR = nextR; }
 
   /**
    * Return the controller used internally.
@@ -215,7 +212,7 @@
    * Return the observer used internally.
    */
   const KalmanFilter<States, Inputs, Outputs>& Observer() const {
-    return m_observer;
+    return *m_observer;
   }
 
   /**
@@ -225,7 +222,7 @@
    *
    * @param initialState The initial state.
    */
-  void Reset(const Eigen::Vector<double, States>& initialState) {
+  void Reset(const StateVector& initialState) {
     m_nextR.setZero();
     m_controller->Reset();
     m_feedforward.Reset(initialState);
@@ -235,18 +232,14 @@
   /**
    * Returns difference between reference r and current state x-hat.
    */
-  Eigen::Vector<double, States> Error() const {
-    return m_controller->R() - m_observer->Xhat();
-  }
+  StateVector Error() const { return m_controller->R() - m_observer->Xhat(); }
 
   /**
    * Correct the state estimate x-hat using the measurements in y.
    *
    * @param y Measurement vector.
    */
-  void Correct(const Eigen::Vector<double, Outputs>& y) {
-    m_observer->Correct(U(), y);
-  }
+  void Correct(const OutputVector& y) { m_observer->Correct(U(), y); }
 
   /**
    * Sets new controller output, projects model forward, and runs observer
@@ -258,7 +251,7 @@
    * @param dt Timestep for model update.
    */
   void Predict(units::second_t dt) {
-    Eigen::Vector<double, Inputs> u =
+    InputVector u =
         ClampInput(m_controller->Calculate(m_observer->Xhat(), m_nextR) +
                    m_feedforward.Calculate(m_nextR));
     m_observer->Predict(u, dt);
@@ -270,10 +263,7 @@
    * @param u Input vector to clamp.
    * @return Clamped input vector.
    */
-  Eigen::Vector<double, Inputs> ClampInput(
-      const Eigen::Vector<double, Inputs>& u) const {
-    return m_clampFunc(u);
-  }
+  InputVector ClampInput(const InputVector& u) const { return m_clampFunc(u); }
 
  protected:
   LinearQuadraticRegulator<States, Inputs>* m_controller;
@@ -283,12 +273,10 @@
   /**
    * Clamping function.
    */
-  std::function<Eigen::Vector<double, Inputs>(
-      const Eigen::Vector<double, Inputs>&)>
-      m_clampFunc;
+  std::function<InputVector(const InputVector&)> m_clampFunc;
 
   // Reference to go to in the next cycle (used by feedforward controller).
-  Eigen::Vector<double, States> m_nextR;
+  StateVector m_nextR;
 
   // These are accessible from non-templated subclasses.
   static constexpr int kStates = States;
@@ -296,4 +284,9 @@
   static constexpr int kOutputs = Outputs;
 };
 
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    LinearSystemLoop<1, 1, 1>;
+extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
+    LinearSystemLoop<2, 1, 1>;
+
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
index 68d047f..bb856ec 100644
--- a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
+++ b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
@@ -54,80 +54,6 @@
 }
 
 /**
- * Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described
- * in https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
- *
- * @param f        The function to integrate. It must take two arguments x and
- *                 u.
- * @param x        The initial value of x.
- * @param u        The value u held constant over the integration period.
- * @param dt       The time over which to integrate.
- * @param maxError The maximum acceptable truncation error. Usually a small
- *                 number like 1e-6.
- */
-template <typename F, typename T, typename U>
-T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
-  // See
-  // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
-  // for the Butcher tableau the following arrays came from.
-  constexpr int kDim = 6;
-
-  // clang-format off
-  constexpr double A[kDim - 1][kDim - 1]{
-      {     1.0 / 4.0},
-      {     3.0 / 32.0,       9.0 / 32.0},
-      {1932.0 / 2197.0, -7200.0 / 2197.0,  7296.0 / 2197.0},
-      {  439.0 / 216.0,             -8.0,   3680.0 / 513.0, -845.0 / 4104.0},
-      {    -8.0 / 27.0,              2.0, -3544.0 / 2565.0, 1859.0 / 4104.0, -11.0 / 40.0}};
-  // clang-format on
-
-  constexpr std::array<double, kDim> b1{16.0 / 135.0,     0.0,
-                                        6656.0 / 12825.0, 28561.0 / 56430.0,
-                                        -9.0 / 50.0,      2.0 / 55.0};
-  constexpr std::array<double, kDim> b2{
-      25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0};
-
-  T newX;
-  double truncationError;
-
-  double dtElapsed = 0.0;
-  double h = dt.value();
-
-  // Loop until we've gotten to our desired dt
-  while (dtElapsed < dt.value()) {
-    do {
-      // Only allow us to advance up to the dt remaining
-      h = std::min(h, dt.value() - dtElapsed);
-
-      // Notice how the derivative in the Wikipedia notation is dy/dx.
-      // That means their y is our x and their x is our t
-      // clang-format off
-      T k1 = f(x, u);
-      T k2 = f(x + h * (A[0][0] * k1), u);
-      T k3 = f(x + h * (A[1][0] * k1 + A[1][1] * k2), u);
-      T k4 = f(x + h * (A[2][0] * k1 + A[2][1] * k2 + A[2][2] * k3), u);
-      T k5 = f(x + h * (A[3][0] * k1 + A[3][1] * k2 + A[3][2] * k3 + A[3][3] * k4), u);
-      T k6 = f(x + h * (A[4][0] * k1 + A[4][1] * k2 + A[4][2] * k3 + A[4][3] * k4 + A[4][4] * k5), u);
-      // clang-format on
-
-      newX = x + h * (b1[0] * k1 + b1[1] * k2 + b1[2] * k3 + b1[3] * k4 +
-                      b1[4] * k5 + b1[5] * k6);
-      truncationError = (h * ((b1[0] - b2[0]) * k1 + (b1[1] - b2[1]) * k2 +
-                              (b1[2] - b2[2]) * k3 + (b1[3] - b2[3]) * k4 +
-                              (b1[4] - b2[4]) * k5 + (b1[5] - b2[5]) * k6))
-                            .norm();
-
-      h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
-    } while (truncationError > maxError);
-
-    dtElapsed += h;
-    x = newX;
-  }
-
-  return x;
-}
-
-/**
  * Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt.
  *
  * @param f        The function to integrate. It must take two arguments x and
@@ -173,7 +99,7 @@
   while (dtElapsed < dt.value()) {
     do {
       // Only allow us to advance up to the dt remaining
-      h = std::min(h, dt.value() - dtElapsed);
+      h = (std::min)(h, dt.value() - dtElapsed);
 
       // clang-format off
       T k1 = f(x, u);
diff --git a/wpimath/src/main/native/include/frc/system/NumericalJacobian.h b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h
index 5f1bc78..c4cb695 100644
--- a/wpimath/src/main/native/include/frc/system/NumericalJacobian.h
+++ b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h
@@ -4,7 +4,7 @@
 
 #pragma once
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 
 namespace frc {
 
@@ -17,16 +17,16 @@
  * @param x     Vector argument.
  */
 template <int Rows, int Cols, typename F>
-auto NumericalJacobian(F&& f, const Eigen::Vector<double, Cols>& x) {
+auto NumericalJacobian(F&& f, const Vectord<Cols>& x) {
   constexpr double kEpsilon = 1e-5;
-  Eigen::Matrix<double, Rows, Cols> result;
+  Matrixd<Rows, Cols> result;
   result.setZero();
 
   // It's more expensive, but +- epsilon will be more accurate
   for (int i = 0; i < Cols; ++i) {
-    Eigen::Vector<double, Cols> dX_plus = x;
+    Vectord<Cols> dX_plus = x;
     dX_plus(i) += kEpsilon;
-    Eigen::Vector<double, Cols> dX_minus = x;
+    Vectord<Cols> dX_minus = x;
     dX_minus(i) -= kEpsilon;
     result.col(i) = (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0);
   }
@@ -48,12 +48,10 @@
  * @param args     Remaining arguments to f(x, u, ...).
  */
 template <int Rows, int States, int Inputs, typename F, typename... Args>
-auto NumericalJacobianX(F&& f, const Eigen::Vector<double, States>& x,
-                        const Eigen::Vector<double, Inputs>& u,
-                        Args&&... args) {
+auto NumericalJacobianX(F&& f, const Vectord<States>& x,
+                        const Vectord<Inputs>& u, Args&&... args) {
   return NumericalJacobian<Rows, States>(
-      [&](const Eigen::Vector<double, States>& x) { return f(x, u, args...); },
-      x);
+      [&](const Vectord<States>& x) { return f(x, u, args...); }, x);
 }
 
 /**
@@ -70,12 +68,10 @@
  * @param args     Remaining arguments to f(x, u, ...).
  */
 template <int Rows, int States, int Inputs, typename F, typename... Args>
-auto NumericalJacobianU(F&& f, const Eigen::Vector<double, States>& x,
-                        const Eigen::Vector<double, Inputs>& u,
-                        Args&&... args) {
+auto NumericalJacobianU(F&& f, const Vectord<States>& x,
+                        const Vectord<Inputs>& u, Args&&... args) {
   return NumericalJacobian<Rows, Inputs>(
-      [&](const Eigen::Vector<double, Inputs>& u) { return f(x, u, args...); },
-      u);
+      [&](const Vectord<Inputs>& u) { return f(x, u, args...); }, u);
 }
 
 }  // namespace frc
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 a519b0e..831f532 100644
--- a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
+++ b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
@@ -76,6 +76,49 @@
   }
 
   /**
+   * Returns torque produced by the motor with a given current.
+   *
+   * @param current     The current drawn by the motor.
+   */
+  constexpr units::newton_meter_t Torque(units::ampere_t current) const {
+    return current * Kt;
+  }
+
+  /**
+   * Returns the voltage provided to the motor for a given torque and
+   * angular velocity.
+   *
+   * @param torque      The torque produced by the motor.
+   * @param speed       The current angular velocity of the motor.
+   */
+  constexpr units::volt_t Voltage(units::newton_meter_t torque,
+                                  units::radians_per_second_t speed) const {
+    return 1.0 / Kv * speed + 1.0 / Kt * R * torque;
+  }
+
+  /**
+   * Returns the speed produced by the motor at a given torque and input
+   * voltage.
+   *
+   * @param torque        The torque produced by the motor.
+   * @param inputVoltage  The input voltage provided to the motor.
+   */
+  constexpr units::radians_per_second_t Speed(
+      units::newton_meter_t torque, units::volt_t inputVoltage) const {
+    return inputVoltage * Kv - 1.0 / Kt * torque * R * Kv;
+  }
+
+  /**
+   * Returns a copy of this motor with the given gearbox reduction applied.
+   *
+   * @param gearboxReduction  The gearbox reduction.
+   */
+  constexpr DCMotor WithReduction(double gearboxReduction) {
+    return DCMotor(nominalVoltage, stallTorque * gearboxReduction, stallCurrent,
+                   freeCurrent, freeSpeed / gearboxReduction);
+  }
+
+  /**
    * Returns instance of CIM.
    */
   static constexpr DCMotor CIM(int numMotors = 1) {
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 26142bf..3e69545 100644
--- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
+++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
@@ -31,94 +31,49 @@
       units::inverse<units::seconds>>>;
 
   /**
-   * Constructs the state-space model for an elevator.
+   * Create a state-space model of the elevator system. The states of the system
+   * are [position, velocity], inputs are [voltage], and outputs are [position].
    *
-   * States: [[position], [velocity]]
-   * Inputs: [[voltage]]
-   * Outputs: [[position]]
-   *
-   * @param motor Instance of DCMotor.
-   * @param m Carriage mass.
-   * @param r Pulley radius.
+   * @param motor The motor (or gearbox) attached to the carriage.
+   * @param mass The mass of the elevator carriage, in kilograms.
+   * @param radius The radius of the elevator's driving drum, in meters.
    * @param G Gear ratio from motor to carriage.
-   * @throws std::domain_error if m <= 0, r <= 0, or G <= 0.
+   * @throws std::domain_error if mass <= 0, radius <= 0, or G <= 0.
    */
   static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor,
-                                              units::kilogram_t m,
-                                              units::meter_t r, double G) {
-    if (m <= 0_kg) {
-      throw std::domain_error("m must be greater than zero.");
-    }
-    if (r <= 0_m) {
-      throw std::domain_error("r must be greater than zero.");
-    }
-    if (G <= 0.0) {
-      throw std::domain_error("G must be greater than zero.");
-    }
-
-    Eigen::Matrix<double, 2, 2> A{
-        {0.0, 1.0},
-        {0.0, (-std::pow(G, 2) * motor.Kt /
-               (motor.R * units::math::pow<2>(r) * m * motor.Kv))
-                  .value()}};
-    Eigen::Matrix<double, 2, 1> B{0.0,
-                                  (G * motor.Kt / (motor.R * r * m)).value()};
-    Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
-    Eigen::Matrix<double, 1, 1> D{0.0};
-
-    return LinearSystem<2, 1, 1>(A, B, C, D);
-  }
+                                              units::kilogram_t mass,
+                                              units::meter_t radius, double G);
 
   /**
-   * Constructs the state-space model for a single-jointed arm.
+   * Create a state-space model of a single-jointed arm system.The states of the
+   * system are [angle, angular velocity], inputs are [voltage], and outputs are
+   * [angle].
    *
-   * States: [[angle], [angular velocity]]
-   * Inputs: [[voltage]]
-   * Outputs: [[angle]]
-   *
-   * @param motor Instance of DCMotor.
-   * @param J Moment of inertia.
-   * @param G Gear ratio from motor to carriage.
+   * @param motor The motor (or gearbox) attached to the arm.
+   * @param J The moment of inertia J of the arm.
+   * @param G Gear ratio from motor to arm.
    * @throws std::domain_error if J <= 0 or G <= 0.
    */
   static LinearSystem<2, 1, 1> SingleJointedArmSystem(
-      DCMotor motor, units::kilogram_square_meter_t J, double G) {
-    if (J <= 0_kg_sq_m) {
-      throw std::domain_error("J must be greater than zero.");
-    }
-    if (G <= 0.0) {
-      throw std::domain_error("G must be greater than zero.");
-    }
-
-    Eigen::Matrix<double, 2, 2> A{
-        {0.0, 1.0},
-        {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
-    Eigen::Matrix<double, 2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
-    Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
-    Eigen::Matrix<double, 1, 1> D{0.0};
-
-    return LinearSystem<2, 1, 1>(A, B, C, D);
-  }
+      DCMotor motor, units::kilogram_square_meter_t J, double G);
 
   /**
-   * Constructs the state-space model for a 1 DOF velocity-only system from
-   * system identification data.
+   * Create a state-space model for a 1 DOF velocity 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 [velocity], inputs are
+   * [voltage], and outputs are [velocity].
    *
    * 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.
    *
-   * States: [[velocity]]
-   * Inputs: [[voltage]]
-   * Outputs: [[velocity]]
-   *
    * The parameters provided by the user are from this feedforward model:
    *
    * u = K_v v + K_a a
    *
-   * @param kV The velocity gain, in volt seconds per distance.
-   * @param kA The acceleration gain, in volt seconds^2 per distance.
+   * @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, typename = std::enable_if_t<
@@ -134,33 +89,32 @@
       throw std::domain_error("Ka must be greater than zero.");
     }
 
-    Eigen::Matrix<double, 1, 1> A{-kV.value() / kA.value()};
-    Eigen::Matrix<double, 1, 1> B{1.0 / kA.value()};
-    Eigen::Matrix<double, 1, 1> C{1.0};
-    Eigen::Matrix<double, 1, 1> D{0.0};
+    Matrixd<1, 1> A{-kV.value() / kA.value()};
+    Matrixd<1, 1> B{1.0 / kA.value()};
+    Matrixd<1, 1> C{1.0};
+    Matrixd<1, 1> D{0.0};
 
     return LinearSystem<1, 1, 1>(A, B, C, D);
   }
 
   /**
-   * Constructs the state-space model for a 1 DOF position system from system
-   * identification data.
+   * Create a state-space model for a 1 DOF position 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.
    *
-   * States: [[position], [velocity]]
-   * Inputs: [[voltage]]
-   * Outputs: [[position]]
-   *
    * The parameters provided by the user are from this feedforward model:
    *
    * u = K_v v + K_a a
    *
-   * @param kV The velocity gain, in volt seconds per distance.
-   * @param kA The acceleration gain, in volt seconds^2 per distance.
+   * @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, typename = std::enable_if_t<
@@ -176,236 +130,111 @@
       throw std::domain_error("Ka must be greater than zero.");
     }
 
-    Eigen::Matrix<double, 2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
-    Eigen::Matrix<double, 2, 1> B{0.0, 1.0 / kA.value()};
-    Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
-    Eigen::Matrix<double, 1, 1> D{0.0};
+    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<1, 2> C{1.0, 0.0};
+    Matrixd<1, 1> D{0.0};
 
     return LinearSystem<2, 1, 1>(A, B, C, D);
   }
 
   /**
-   * Constructs the state-space model for a 2 DOF drivetrain velocity system
-   * from system identification data.
+   * Identify a differential drive drivetrain given the drivetrain's kV and kA
+   * in both linear {(volts/(meter/sec), (volts/(meter/sec²))} and angular
+   * {(volts/(radian/sec), (volts/(radian/sec²))} cases. These constants can be
+   * found using SysId.
    *
-   * States: [[left velocity], [right velocity]]
-   * Inputs: [[left voltage], [right voltage]]
+   * States: [[left velocity], [right velocity]]<br>
+   * Inputs: [[left voltage], [right voltage]]<br>
    * Outputs: [[left velocity], [right velocity]]
    *
-   * @param kVlinear  The linear velocity gain in volts per (meter per second).
-   * @param kAlinear  The linear acceleration gain in volts per (meter per
+   * @param kVLinear  The linear velocity gain in volts per (meters per second).
+   * @param kALinear  The linear acceleration gain in volts per (meters per
    *                  second squared).
-   * @param kVangular The angular velocity gain in volts per (meter per second).
-   * @param kAangular The angular acceleration gain in volts per (meter per
+   * @param kVAngular The angular velocity gain in volts per (meters per
+   *                  second).
+   * @param kAAngular The angular acceleration gain in volts per (meters per
    *                  second squared).
-   * @throws domain_error if kVlinear <= 0, kAlinear <= 0, kVangular <= 0,
-   *         or kAangular <= 0.
+   * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
+   *         or kAAngular <= 0.
    */
   static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
-      decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear,
-      decltype(1_V / 1_mps) kVangular, decltype(1_V / 1_mps_sq) kAangular) {
-    if (kVlinear <= decltype(kVlinear){0}) {
-      throw std::domain_error("Kv,linear must be greater than zero.");
-    }
-    if (kAlinear <= decltype(kAlinear){0}) {
-      throw std::domain_error("Ka,linear must be greater than zero.");
-    }
-    if (kVangular <= decltype(kVangular){0}) {
-      throw std::domain_error("Kv,angular must be greater than zero.");
-    }
-    if (kAangular <= decltype(kAangular){0}) {
-      throw std::domain_error("Ka,angular must be greater than zero.");
-    }
-
-    double A1 = -(kVlinear.value() / kAlinear.value() +
-                  kVangular.value() / kAangular.value());
-    double A2 = -(kVlinear.value() / kAlinear.value() -
-                  kVangular.value() / kAangular.value());
-    double B1 = 1.0 / kAlinear.value() + 1.0 / kAangular.value();
-    double B2 = 1.0 / kAlinear.value() - 1.0 / kAangular.value();
-
-    Eigen::Matrix<double, 2, 2> A =
-        0.5 * Eigen::Matrix<double, 2, 2>{{A1, A2}, {A2, A1}};
-    Eigen::Matrix<double, 2, 2> B =
-        0.5 * Eigen::Matrix<double, 2, 2>{{B1, B2}, {B2, B1}};
-    Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
-    Eigen::Matrix<double, 2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
-
-    return LinearSystem<2, 2, 2>(A, B, C, D);
-  }
+      decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
+      decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular);
 
   /**
-   * Constructs the state-space model for a 2 DOF drivetrain velocity system
-   * from system identification data.
+   * Identify a differential drive drivetrain given the drivetrain's kV and kA
+   * in both linear {(volts/(meter/sec)), (volts/(meter/sec²))} and angular
+   * {(volts/(radian/sec)), (volts/(radian/sec²))} cases. This can be found
+   * using SysId.
    *
-   * States: [[left velocity], [right velocity]]
-   * Inputs: [[left voltage], [right voltage]]
+   * States: [[left velocity], [right velocity]]<br>
+   * Inputs: [[left voltage], [right voltage]]<br>
    * Outputs: [[left velocity], [right velocity]]
    *
-   * @param kVlinear   The linear velocity gain in volts per (meter per second).
-   * @param kAlinear   The linear acceleration gain in volts per (meter per
+   * @param kVLinear   The linear velocity gain in volts per (meters per
+   * second).
+   * @param kALinear   The linear acceleration gain in volts per (meters per
    *                   second squared).
-   * @param kVangular  The angular velocity gain in volts per (radian per
+   * @param kVAngular  The angular velocity gain in volts per (radians per
    *                   second).
-   * @param kAangular  The angular acceleration gain in volts per (radian per
+   * @param kAAngular  The angular acceleration gain in volts per (radians per
    *                   second squared).
-   * @param trackwidth The width of the drivetrain.
-   * @throws domain_error if kVlinear <= 0, kAlinear <= 0, kVangular <= 0,
-   *         kAangular <= 0, or trackwidth <= 0.
+   * @param trackwidth The distance between the differential drive's left and
+   *                   right wheels, in meters.
+   * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
+   *         kAAngular <= 0, or trackwidth <= 0.
    */
   static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
-      decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear,
-      decltype(1_V / 1_rad_per_s) kVangular,
-      decltype(1_V / 1_rad_per_s_sq) kAangular, units::meter_t trackwidth) {
-    if (kVlinear <= decltype(kVlinear){0}) {
-      throw std::domain_error("Kv,linear must be greater than zero.");
-    }
-    if (kAlinear <= decltype(kAlinear){0}) {
-      throw std::domain_error("Ka,linear must be greater than zero.");
-    }
-    if (kVangular <= decltype(kVangular){0}) {
-      throw std::domain_error("Kv,angular must be greater than zero.");
-    }
-    if (kAangular <= decltype(kAangular){0}) {
-      throw std::domain_error("Ka,angular must be greater than zero.");
-    }
-    if (trackwidth <= 0_m) {
-      throw std::domain_error("r must be greater than zero.");
-    }
-
-    // We want to find a factor to include in Kv,angular that will convert
-    // `u = Kv,angular omega` to `u = Kv,angular v`.
-    //
-    // v = omega r
-    // omega = v/r
-    // omega = 1/r v
-    // omega = 1/(trackwidth/2) v
-    // omega = 2/trackwidth v
-    //
-    // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
-    // to V/(m/s).
-    return IdentifyDrivetrainSystem(kVlinear, kAlinear,
-                                    kVangular * 2.0 / trackwidth * 1_rad,
-                                    kAangular * 2.0 / trackwidth * 1_rad);
-  }
+      decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
+      decltype(1_V / 1_rad_per_s) kVAngular,
+      decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth);
 
   /**
-   * Constructs the state-space model for a flywheel.
+   * Create a state-space model of a flywheel system, the states of the system
+   * are [angular velocity], inputs are [voltage], and outputs are [angular
+   * velocity].
    *
-   * States: [[angular velocity]]
-   * Inputs: [[voltage]]
-   * Outputs: [[angular velocity]]
-   *
-   * @param motor Instance of DCMotor.
-   * @param J Moment of inertia.
-   * @param G Gear ratio from motor to carriage.
+   * @param motor The motor (or gearbox) attached to the flywheel.
+   * @param J The moment of inertia J of the flywheel.
+   * @param G Gear ratio from motor to flywheel.
    * @throws std::domain_error if J <= 0 or G <= 0.
    */
   static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor,
                                               units::kilogram_square_meter_t J,
-                                              double G) {
-    if (J <= 0_kg_sq_m) {
-      throw std::domain_error("J must be greater than zero.");
-    }
-    if (G <= 0.0) {
-      throw std::domain_error("G must be greater than zero.");
-    }
-
-    Eigen::Matrix<double, 1, 1> A{
-        (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
-    Eigen::Matrix<double, 1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
-    Eigen::Matrix<double, 1, 1> C{1.0};
-    Eigen::Matrix<double, 1, 1> D{0.0};
-
-    return LinearSystem<1, 1, 1>(A, B, C, D);
-  }
+                                              double G);
 
   /**
-   * Constructs the state-space model for a DC motor motor.
+   * 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].
    *
-   * States: [[angular position, angular velocity]]
-   * Inputs: [[voltage]]
-   * Outputs: [[angular position, angular velocity]]
-   *
-   * @param motor Instance of DCMotor.
-   * @param J Moment of inertia.
-   * @param G Gear ratio from motor to carriage.
+   * @param motor The motor (or gearbox) attached to the system.
+   * @param J the moment of inertia J of the DC motor.
+   * @param G Gear ratio from motor to output.
    * @throws std::domain_error if J <= 0 or G <= 0.
    */
   static LinearSystem<2, 1, 2> DCMotorSystem(DCMotor motor,
                                              units::kilogram_square_meter_t J,
-                                             double G) {
-    if (J <= 0_kg_sq_m) {
-      throw std::domain_error("J must be greater than zero.");
-    }
-    if (G <= 0.0) {
-      throw std::domain_error("G must be greater than zero.");
-    }
-
-    Eigen::Matrix<double, 2, 2> A{
-        {0.0, 1.0},
-        {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
-    Eigen::Matrix<double, 2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
-    Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
-    Eigen::Matrix<double, 2, 1> D{0.0, 0.0};
-
-    return LinearSystem<2, 1, 2>(A, B, C, D);
-  }
-
+                                             double G);
   /**
-   * Constructs the state-space model for a drivetrain.
+   * Create a state-space model of differential drive drivetrain. In this model,
+   * the states are [left velocity, right velocity], the inputs are [left
+   * voltage, right voltage], and the outputs are [left velocity, right
+   * velocity]
    *
-   * States: [[left velocity], [right velocity]]
-   * Inputs: [[left voltage], [right voltage]]
-   * Outputs: [[left velocity], [right velocity]]
-   *
-   * @param motor Instance of DCMotor.
-   * @param m Drivetrain mass.
-   * @param r Wheel radius.
-   * @param rb Robot radius.
-   * @param J Moment of inertia.
+   * @param motor The motor (or gearbox) driving the drivetrain.
+   * @param mass The mass of the robot in kilograms.
+   * @param r The radius of the wheels in meters.
+   * @param rb The radius of the base (half of the track width), in meters.
+   * @param J The moment of inertia of the robot.
    * @param G Gear ratio from motor to wheel.
-   * @throws std::domain_error if m <= 0, r <= 0, rb <= 0, J <= 0, or
+   * @throws std::domain_error if mass <= 0, r <= 0, rb <= 0, J <= 0, or
    *         G <= 0.
    */
   static LinearSystem<2, 2, 2> DrivetrainVelocitySystem(
-      const DCMotor& motor, units::kilogram_t m, units::meter_t r,
-      units::meter_t rb, units::kilogram_square_meter_t J, double G) {
-    if (m <= 0_kg) {
-      throw std::domain_error("m must be greater than zero.");
-    }
-    if (r <= 0_m) {
-      throw std::domain_error("r must be greater than zero.");
-    }
-    if (rb <= 0_m) {
-      throw std::domain_error("rb must be greater than zero.");
-    }
-    if (J <= 0_kg_sq_m) {
-      throw std::domain_error("J must be greater than zero.");
-    }
-    if (G <= 0.0) {
-      throw std::domain_error("G must be greater than zero.");
-    }
-
-    auto C1 = -std::pow(G, 2) * motor.Kt /
-              (motor.Kv * motor.R * units::math::pow<2>(r));
-    auto C2 = G * motor.Kt / (motor.R * r);
-
-    Eigen::Matrix<double, 2, 2> A{
-        {((1 / m + units::math::pow<2>(rb) / J) * C1).value(),
-         ((1 / m - units::math::pow<2>(rb) / J) * C1).value()},
-        {((1 / m - units::math::pow<2>(rb) / J) * C1).value(),
-         ((1 / m + units::math::pow<2>(rb) / J) * C1).value()}};
-    Eigen::Matrix<double, 2, 2> B{
-        {((1 / m + units::math::pow<2>(rb) / J) * C2).value(),
-         ((1 / m - units::math::pow<2>(rb) / J) * C2).value()},
-        {((1 / m - units::math::pow<2>(rb) / J) * C2).value(),
-         ((1 / m + units::math::pow<2>(rb) / J) * C2).value()}};
-    Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
-    Eigen::Matrix<double, 2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
-
-    return LinearSystem<2, 2, 2>(A, B, C, D);
-  }
+      const DCMotor& motor, units::kilogram_t mass, units::meter_t r,
+      units::meter_t rb, units::kilogram_square_meter_t J, double G);
 };
 
 }  // 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 d071ca3..f5ee79b 100644
--- a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
+++ b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
@@ -48,19 +48,8 @@
 
     /**
      * Checks equality between this State and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are equal.
      */
-    bool operator==(const State& other) const;
-
-    /**
-     * Checks inequality between this State and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are not equal.
-     */
-    bool operator!=(const State& other) const;
+    bool operator==(const State&) const = default;
 
     /**
      * Interpolates between two States.
@@ -140,19 +129,8 @@
 
   /**
    * Checks equality between this Trajectory and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are equal.
    */
-  bool operator==(const Trajectory& other) const;
-
-  /**
-   * Checks inequality between this Trajectory and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are inequal.
-   */
-  bool operator!=(const Trajectory& other) const;
+  bool operator==(const Trajectory&) const = default;
 
  private:
   std::vector<State> m_states;
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
index eea1c07..807b315 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
@@ -81,7 +81,7 @@
    * the trajectory, max velocity, min acceleration and max acceleration.
    */
   struct ConstrainedState {
-    PoseWithCurvature pose = {Pose2d(), units::curvature_t(0.0)};
+    PoseWithCurvature pose = {Pose2d{}, units::curvature_t{0.0}};
     units::meter_t distance = 0_m;
     units::meters_per_second_t maxVelocity = 0_mps;
     units::meters_per_second_squared_t minAcceleration = 0_mps_sq;
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
index 6e52a09..3ba882f 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
@@ -25,7 +25,7 @@
   static void ToPathweaverJson(const Trajectory& trajectory,
                                std::string_view path);
   /**
-   * Imports a Trajectory from a PathWeaver-style JSON file.
+   * Imports a Trajectory from a JSON file exported from PathWeaver.
    *
    * @param path The path of the json file to import from.
    *
@@ -34,7 +34,7 @@
   static Trajectory FromPathweaverJson(std::string_view path);
 
   /**
-   * Deserializes a Trajectory from PathWeaver-style JSON.
+   * Deserializes a Trajectory from JSON exported from PathWeaver.
    *
    * @param trajectory the trajectory to export
    *
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
index 0e623eb..24a8253 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -19,14 +19,14 @@
  *
  * Initialization:
  * @code{.cpp}
- * TrapezoidalMotionProfile::Constraints constraints{kMaxV, kMaxA};
+ * TrapezoidProfile::Constraints constraints{kMaxV, kMaxA};
  * double previousProfiledReference = initialReference;
  * @endcode
  *
  * Run on update:
  * @code{.cpp}
- * TrapezoidalMotionProfile profile{constraints, unprofiledReference,
- *                                  previousProfiledReference};
+ * TrapezoidProfile profile{constraints, unprofiledReference,
+ *                          previousProfiledReference};
  * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate);
  * @endcode
  *
@@ -68,10 +68,7 @@
    public:
     Distance_t position{0};
     Velocity_t velocity{0};
-    bool operator==(const State& rhs) const {
-      return position == rhs.position && velocity == rhs.velocity;
-    }
-    bool operator!=(const State& rhs) const { return !(*this == rhs); }
+    bool operator==(const State&) const = default;
   };
 
   /**
@@ -82,7 +79,7 @@
    * @param initial     The initial state (usually the current state).
    */
   TrapezoidProfile(Constraints constraints, State goal,
-                   State initial = State{Distance_t(0), Velocity_t(0)});
+                   State initial = State{Distance_t{0}, Velocity_t{0}});
 
   TrapezoidProfile(const TrapezoidProfile&) = default;
   TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
index 4ec0f42..19eb1f3 100644
--- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
@@ -46,10 +46,10 @@
       accelerationTime * accelerationTime * m_constraints.maxAcceleration;
 
   // Handle the case where the profile never reaches full speed
-  if (fullSpeedDist < Distance_t(0)) {
+  if (fullSpeedDist < Distance_t{0}) {
     accelerationTime =
         units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
-    fullSpeedDist = Distance_t(0);
+    fullSpeedDist = Distance_t{0};
   }
 
   m_endAccel = accelerationTime - cutoffBegin;
@@ -110,7 +110,7 @@
 
   Distance_t distToTarget = units::math::abs(target - position);
 
-  if (distToTarget < Distance_t(1e-6)) {
+  if (distToTarget < Distance_t{1e-6}) {
     return 0_s;
   }
 
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 e2ef37b..f9f0d2e 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
@@ -44,8 +44,8 @@
     if (IsPoseInRegion(pose)) {
       return m_constraint.MaxVelocity(pose, curvature, velocity);
     } else {
-      return units::meters_per_second_t(
-          std::numeric_limits<double>::infinity());
+      return units::meters_per_second_t{
+          std::numeric_limits<double>::infinity()};
     }
   }
 
@@ -66,11 +66,16 @@
    * @return Whether the robot pose is within the constraint region.
    */
   bool IsPoseInRegion(const Pose2d& pose) const {
-    // The region (disk) bounded by the ellipse is given by the equation:
-    // ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
+    // The region bounded by the ellipse is given by the equation:
+    //
+    // (x−h)²/Rx² + (y−k)²/Ry² ≤ 1
+    //
+    // Multiply by Rx²Ry² for efficiency reasons:
+    //
+    // (x−h)²Ry² + (y−k)²Rx² ≤ Rx²Ry²
+    //
     // If the inequality is satisfied, then it is inside the ellipse; otherwise
     // it is outside the ellipse.
-    // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
     return units::math::pow<2>(pose.X() - m_center.X()) *
                    units::math::pow<2>(m_radii.Y()) +
                units::math::pow<2>(pose.Y() - m_center.Y()) *
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 c5bc559..18522fe 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
@@ -41,8 +41,8 @@
     if (IsPoseInRegion(pose)) {
       return m_constraint.MaxVelocity(pose, curvature, velocity);
     } else {
-      return units::meters_per_second_t(
-          std::numeric_limits<double>::infinity());
+      return units::meters_per_second_t{
+          std::numeric_limits<double>::infinity()};
     }
   }
 
diff --git a/wpimath/src/main/native/include/units/angular_acceleration.h b/wpimath/src/main/native/include/units/angular_acceleration.h
index 6a411c4..632982b 100644
--- a/wpimath/src/main/native/include/units/angular_acceleration.h
+++ b/wpimath/src/main/native/include/units/angular_acceleration.h
@@ -25,6 +25,16 @@
 UNIT_ADD(angular_acceleration, degrees_per_second_squared,
          degrees_per_second_squared, deg_per_s_sq,
          compound_unit<angle::degrees, inverse<squared<time::seconds>>>)
+UNIT_ADD(angular_acceleration, turns_per_second_squared,
+         turns_per_second_squared, tr_per_s_sq,
+         compound_unit<angle::turns, inverse<squared<time::seconds>>>)
+UNIT_ADD(angular_acceleration, revolutions_per_minute_squared,
+         revolutions_per_minute_squared, rev_per_m_sq,
+         compound_unit<angle::turns, inverse<squared<time::minutes>>>)
+UNIT_ADD(angular_acceleration, revolutions_per_minute_per_second,
+         revolutions_per_minute_per_second, rev_per_m_per_s,
+         compound_unit<angle::turns, compound_unit<inverse<time::minutes>,
+                                                   inverse<time::seconds>>>)
 
 UNIT_ADD_CATEGORY_TRAIT(angular_acceleration)
 
diff --git a/wpimath/src/main/native/include/units/base.h b/wpimath/src/main/native/include/units/base.h
index bc9fb44..9d021b1 100644
--- a/wpimath/src/main/native/include/units/base.h
+++ b/wpimath/src/main/native/include/units/base.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// 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.
 
 // Copyright (c) 2016 Nic Holthaus
 //
@@ -78,14 +75,13 @@
 	#include <iostream>
 	#include <locale>
 	#include <string>
-#else
+#endif
+#if !defined(UNIT_LIB_DISABLE_FMT)
 	#include <locale>
 	#include <string>
 	#include <fmt/format.h>
 #endif
 
-#include <wpi/SymbolExports.h>
-
 //------------------------------
 //	STRING FORMATTER
 //------------------------------
@@ -180,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_ENABLE_IOSTREAM)
+#if !defined(UNIT_LIB_DISABLE_FMT)
 	#define UNIT_ADD_IO(namespaceName, nameSingular, abbrev)\
 	}\
 	template <>\
@@ -205,7 +201,8 @@
 			return units::detail::to_string(obj()) + std::string(" "#abbrev);\
 		}\
 	}
-#else
+#endif
+#if defined(UNIT_LIB_ENABLE_IOSTREAM)
 	#define UNIT_ADD_IO(namespaceName, nameSingular, abbrev)\
 	namespace namespaceName\
 	{\
@@ -872,7 +869,7 @@
 	 *				- A `std::ratio` defining the conversion factor to the base unit type. (e.g. `std::ratio<1,12>` for inches to feet)
 	 *				- A base unit that the unit is derived from (or a unit category. Must be of type `unit` or `base_unit`)
 	 *				- An exponent representing factors of PI required by the conversion. (e.g. `std::ratio<-1>` for a radians to degrees conversion)
-	 *				- a ratio representing a datum translation required for the conversion (e.g. `std::ratio<32>` for a farenheit to celsius conversion)
+	 *				- a ratio representing a datum translation required for the conversion (e.g. `std::ratio<32>` for a fahrenheit to celsius conversion)
 	 *
 	 *				Typically, a specific unit, like `meters`, would be implemented as a type alias
 	 *				of `unit`, i.e. `using meters = unit<std::ratio<1>, units::category::length_unit`, or
@@ -2328,7 +2325,7 @@
 
 	// unary addition: +T
 	template<class Units, typename T, template<typename> class NonLinearScale>
-	inline unit_t<Units, T, NonLinearScale> operator+(const unit_t<Units, T, NonLinearScale>& u) noexcept
+	constexpr inline unit_t<Units, T, NonLinearScale> operator+(const unit_t<Units, T, NonLinearScale>& u) noexcept
 	{
 		return u;
 	}
@@ -2352,7 +2349,7 @@
 
 	// unary addition: -T
 	template<class Units, typename T, template<typename> class NonLinearScale>
-	inline unit_t<Units, T, NonLinearScale> operator-(const unit_t<Units, T, NonLinearScale>& u) noexcept
+	constexpr inline unit_t<Units, T, NonLinearScale> operator-(const unit_t<Units, T, NonLinearScale>& u) noexcept
 	{
 		return unit_t<Units, T, NonLinearScale>(-u());
 	}
@@ -2869,13 +2866,16 @@
 	namespace dimensionless
 	{
 		typedef unit_t<scalar, UNIT_LIB_DEFAULT_TYPE, decibel_scale> dB_t;
-#if defined(UNIT_LIB_ENABLE_IOSTREAM)
-		inline std::ostream& operator<<(std::ostream& os, const dB_t& obj) { os << obj() << " dB"; return os; }
 		typedef dB_t dBi_t;
 	}
-#else
+#if defined(UNIT_LIB_ENABLE_IOSTREAM)
+	namespace dimensionless
+	{
+		inline std::ostream& operator<<(std::ostream& os, const dB_t& obj) { os << obj() << " dB"; return os; }
+	}
+#endif
 }
-}
+#if !defined(UNIT_LIB_DISABLE_FMT)
 template <>
 struct fmt::formatter<units::dimensionless::dB_t> : fmt::formatter<double>
 {
@@ -2888,13 +2888,9 @@
 		return fmt::format_to(out, " dB");
 	}
 };
-
-namespace units {
-namespace dimensionless {
-		typedef dB_t dBi_t;
-	}
 #endif
 
+namespace units {
 	//------------------------------
 	//	DECIBEL ARITHMETIC
 	//------------------------------
@@ -3444,4 +3440,6 @@
 using namespace units::literals;
 #endif  // UNIT_HAS_LITERAL_SUPPORT
 
+#if !defined(UNIT_LIB_DISABLE_FMT)
 #include "frc/fmt/Units.h"
+#endif
diff --git a/wpimath/src/main/native/include/units/units.h b/wpimath/src/main/native/include/units/units.h
deleted file mode 100644
index 8d47679..0000000
--- a/wpimath/src/main/native/include/units/units.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message("warning: Including this header drastically increases compilation times and is bad style. Include only what you use instead.")
-#else
-#warning "Including this header drastically increases compilation times and is bad style. Include only what you use instead."
-#endif
-
-// clang-format on
-
-#include "units/acceleration.h"
-#include "units/angle.h"
-#include "units/angular_acceleration.h"
-#include "units/angular_velocity.h"
-#include "units/area.h"
-#include "units/capacitance.h"
-#include "units/charge.h"
-#include "units/concentration.h"
-#include "units/conductance.h"
-#include "units/constants.h"
-#include "units/current.h"
-#include "units/curvature.h"
-#include "units/data.h"
-#include "units/data_transfer_rate.h"
-#include "units/density.h"
-#include "units/dimensionless.h"
-#include "units/energy.h"
-#include "units/force.h"
-#include "units/frequency.h"
-#include "units/illuminance.h"
-#include "units/impedance.h"
-#include "units/inductance.h"
-#include "units/length.h"
-#include "units/luminous_flux.h"
-#include "units/luminous_intensity.h"
-#include "units/magnetic_field_strength.h"
-#include "units/magnetic_flux.h"
-#include "units/mass.h"
-#include "units/math.h"
-#include "units/moment_of_inertia.h"
-#include "units/power.h"
-#include "units/pressure.h"
-#include "units/radiation.h"
-#include "units/solid_angle.h"
-#include "units/substance.h"
-#include "units/temperature.h"
-#include "units/time.h"
-#include "units/torque.h"
-#include "units/velocity.h"
-#include "units/voltage.h"
-#include "units/volume.h"
diff --git a/wpimath/src/main/native/include/wpimath/MathShared.h b/wpimath/src/main/native/include/wpimath/MathShared.h
index f4a1795..3b9b3cd 100644
--- a/wpimath/src/main/native/include/wpimath/MathShared.h
+++ b/wpimath/src/main/native/include/wpimath/MathShared.h
@@ -34,12 +34,12 @@
 
   template <typename S, typename... Args>
   inline void ReportError(const S& format, Args&&... args) {
-    ReportErrorV(format, fmt::make_args_checked<Args...>(format, args...));
+    ReportErrorV(format, fmt::make_format_args(args...));
   }
 
   template <typename S, typename... Args>
   inline void ReportWarning(const S& format, Args&&... args) {
-    ReportWarningV(format, fmt::make_args_checked<Args...>(format, args...));
+    ReportWarningV(format, fmt::make_format_args(args...));
   }
 };
 
@@ -55,7 +55,7 @@
 
   template <typename S, typename... Args>
   static inline void ReportError(const S& format, Args&&... args) {
-    ReportErrorV(format, fmt::make_args_checked<Args...>(format, args...));
+    ReportErrorV(format, fmt::make_format_args(args...));
   }
 
   static void ReportWarningV(fmt::string_view format, fmt::format_args args) {
@@ -64,7 +64,7 @@
 
   template <typename S, typename... Args>
   static inline void ReportWarning(const S& format, Args&&... args) {
-    ReportWarningV(format, fmt::make_args_checked<Args...>(format, args...));
+    ReportWarningV(format, fmt::make_format_args(args...));
   }
 
   static void ReportUsage(MathUsageId id, int count) {
diff --git a/wpimath/src/main/native/include/drake/common/drake_assert.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h
similarity index 97%
rename from wpimath/src/main/native/include/drake/common/drake_assert.h
rename to wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h
index e9c3aa2..47097ed 100644
--- a/wpimath/src/main/native/include/drake/common/drake_assert.h
+++ b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h
@@ -92,9 +92,9 @@
 namespace assert {
 // Allows for specialization of how to bool-convert Conditions used in
 // assertions, in case they are not intrinsically convertible.  See
-// symbolic_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.
+// 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>;
diff --git a/wpimath/src/main/native/include/drake/common/drake_assertion_error.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h
similarity index 100%
rename from wpimath/src/main/native/include/drake/common/drake_assertion_error.h
rename to wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h
diff --git a/wpimath/src/main/native/include/drake/common/drake_copyable.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h
similarity index 100%
rename from wpimath/src/main/native/include/drake/common/drake_copyable.h
rename to wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h
diff --git a/wpimath/src/main/native/include/drake/common/drake_throw.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h
similarity index 100%
rename from wpimath/src/main/native/include/drake/common/drake_throw.h
rename to wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h
diff --git a/wpimath/src/main/native/include/drake/common/is_approx_equal_abstol.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h
similarity index 100%
rename from wpimath/src/main/native/include/drake/common/is_approx_equal_abstol.h
rename to wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h
diff --git a/wpimath/src/main/native/include/drake/common/never_destroyed.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h
similarity index 100%
rename from wpimath/src/main/native/include/drake/common/never_destroyed.h
rename to wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h
diff --git a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h b/wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h
similarity index 100%
rename from wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
rename to wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h
diff --git a/wpimath/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp b/wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp
similarity index 100%
rename from wpimath/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp
rename to wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp
diff --git a/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp b/wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp
similarity index 100%
rename from wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
rename to wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/Cholesky b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Cholesky
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/Cholesky
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/Cholesky
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/Core b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/Core
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/Eigenvalues b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Eigenvalues
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/Eigenvalues
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/Eigenvalues
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/Householder b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Householder
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/Householder
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/Householder
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/IterativeLinearSolvers b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/IterativeLinearSolvers
new file mode 100644
index 0000000..957d575
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/IterativeLinearSolvers
@@ -0,0 +1,48 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// 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_ITERATIVELINEARSOLVERS_MODULE_H
+#define EIGEN_ITERATIVELINEARSOLVERS_MODULE_H
+
+#include "SparseCore"
+#include "OrderingMethods"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/** 
+  * \defgroup IterativeLinearSolvers_Module IterativeLinearSolvers module
+  *
+  * This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a squared matrix, usually very large and sparse.
+  * Those solvers are accessible via the following classes:
+  *  - ConjugateGradient for selfadjoint (hermitian) matrices,
+  *  - LeastSquaresConjugateGradient for rectangular least-square problems,
+  *  - BiCGSTAB for general square matrices.
+  *
+  * These iterative solvers are associated with some preconditioners:
+  *  - IdentityPreconditioner - not really useful
+  *  - DiagonalPreconditioner - also called Jacobi preconditioner, work very well on diagonal dominant matrices.
+  *  - IncompleteLUT - incomplete LU factorization with dual thresholding
+  *
+  * Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport, UmfPackSupport, SuperLUSupport.
+  *
+    \code
+    #include <Eigen/IterativeLinearSolvers>
+    \endcode
+  */
+
+#include "src/IterativeLinearSolvers/SolveWithGuess.h"
+#include "src/IterativeLinearSolvers/IterativeSolverBase.h"
+#include "src/IterativeLinearSolvers/BasicPreconditioners.h"
+#include "src/IterativeLinearSolvers/ConjugateGradient.h"
+#include "src/IterativeLinearSolvers/LeastSquareConjugateGradient.h"
+#include "src/IterativeLinearSolvers/BiCGSTAB.h"
+#include "src/IterativeLinearSolvers/IncompleteLUT.h"
+#include "src/IterativeLinearSolvers/IncompleteCholesky.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_ITERATIVELINEARSOLVERS_MODULE_H
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/Jacobi b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/Jacobi
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/Jacobi
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/Jacobi
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/LU b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/LU
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/LU
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/LU
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/OrderingMethods b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/OrderingMethods
new file mode 100644
index 0000000..29691a6
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/OrderingMethods
@@ -0,0 +1,70 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// 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_ORDERINGMETHODS_MODULE_H
+#define EIGEN_ORDERINGMETHODS_MODULE_H
+
+#include "SparseCore"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/** 
+  * \defgroup OrderingMethods_Module OrderingMethods module
+  *
+  * This module is currently for internal use only
+  * 
+  * It defines various built-in and external ordering methods for sparse matrices. 
+  * They are typically used to reduce the number of elements during 
+  * the sparse matrix decomposition (LLT, LU, QR).
+  * Precisely, in a preprocessing step, a permutation matrix P is computed using 
+  * those ordering methods and applied to the columns of the matrix. 
+  * Using for instance the sparse Cholesky decomposition, it is expected that 
+  * the nonzeros elements in LLT(A*P) will be much smaller than that in LLT(A).
+  * 
+  * 
+  * Usage : 
+  * \code
+  * #include <Eigen/OrderingMethods>
+  * \endcode
+  * 
+  * A simple usage is as a template parameter in the sparse decomposition classes : 
+  * 
+  * \code 
+  * SparseLU<MatrixType, COLAMDOrdering<int> > solver;
+  * \endcode 
+  * 
+  * \code 
+  * SparseQR<MatrixType, COLAMDOrdering<int> > solver;
+  * \endcode
+  * 
+  * It is possible as well to call directly a particular ordering method for your own purpose, 
+  * \code 
+  * AMDOrdering<int> ordering;
+  * PermutationMatrix<Dynamic, Dynamic, int> perm;
+  * SparseMatrix<double> A; 
+  * //Fill the matrix ...
+  * 
+  * ordering(A, perm); // Call AMD
+  * \endcode
+  * 
+  * \note Some of these methods (like AMD or METIS), need the sparsity pattern 
+  * of the input matrix to be symmetric. When the matrix is structurally unsymmetric, 
+  * Eigen computes internally the pattern of \f$A^T*A\f$ before calling the method.
+  * If your matrix is already symmetric (at leat in structure), you can avoid that
+  * by calling the method with a SelfAdjointView type.
+  * 
+  * \code
+  *  // Call the ordering on the pattern of the lower triangular matrix A
+  * ordering(A.selfadjointView<Lower>(), perm);
+  * \endcode
+  */
+
+#include "src/OrderingMethods/Amd.h"
+#include "src/OrderingMethods/Ordering.h"
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_ORDERINGMETHODS_MODULE_H
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/QR b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/QR
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/QR
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/QR
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/SVD b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SVD
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/SVD
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/SVD
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCholesky b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCholesky
new file mode 100644
index 0000000..d2b1f12
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCholesky
@@ -0,0 +1,37 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2013 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSECHOLESKY_MODULE_H
+#define EIGEN_SPARSECHOLESKY_MODULE_H
+
+#include "SparseCore"
+#include "OrderingMethods"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/** 
+  * \defgroup SparseCholesky_Module SparseCholesky module
+  *
+  * This module currently provides two variants of the direct sparse Cholesky decomposition for selfadjoint (hermitian) matrices.
+  * Those decompositions are accessible via the following classes:
+  *  - SimplicialLLt,
+  *  - SimplicialLDLt
+  *
+  * Such problems can also be solved using the ConjugateGradient solver from the IterativeLinearSolvers module.
+  *
+  * \code
+  * #include <Eigen/SparseCholesky>
+  * \endcode
+  */
+
+#include "src/SparseCholesky/SimplicialCholesky.h"
+#include "src/SparseCholesky/SimplicialCholesky_impl.h"
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_SPARSECHOLESKY_MODULE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCore b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCore
new file mode 100644
index 0000000..76966c4
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseCore
@@ -0,0 +1,69 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// 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_SPARSECORE_MODULE_H
+#define EIGEN_SPARSECORE_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+#include <vector>
+#include <map>
+#include <cstdlib>
+#include <cstring>
+#include <algorithm>
+
+/** 
+  * \defgroup SparseCore_Module SparseCore module
+  *
+  * This module provides a sparse matrix representation, and basic associated matrix manipulations
+  * and operations.
+  *
+  * See the \ref TutorialSparse "Sparse tutorial"
+  *
+  * \code
+  * #include <Eigen/SparseCore>
+  * \endcode
+  *
+  * This module depends on: Core.
+  */
+
+#include "src/SparseCore/SparseUtil.h"
+#include "src/SparseCore/SparseMatrixBase.h"
+#include "src/SparseCore/SparseAssign.h"
+#include "src/SparseCore/CompressedStorage.h"
+#include "src/SparseCore/AmbiVector.h"
+#include "src/SparseCore/SparseCompressedBase.h"
+#include "src/SparseCore/SparseMatrix.h"
+#include "src/SparseCore/SparseMap.h"
+#include "src/SparseCore/MappedSparseMatrix.h"
+#include "src/SparseCore/SparseVector.h"
+#include "src/SparseCore/SparseRef.h"
+#include "src/SparseCore/SparseCwiseUnaryOp.h"
+#include "src/SparseCore/SparseCwiseBinaryOp.h"
+#include "src/SparseCore/SparseTranspose.h"
+#include "src/SparseCore/SparseBlock.h"
+#include "src/SparseCore/SparseDot.h"
+#include "src/SparseCore/SparseRedux.h"
+#include "src/SparseCore/SparseView.h"
+#include "src/SparseCore/SparseDiagonalProduct.h"
+#include "src/SparseCore/ConservativeSparseSparseProduct.h"
+#include "src/SparseCore/SparseSparseProductWithPruning.h"
+#include "src/SparseCore/SparseProduct.h"
+#include "src/SparseCore/SparseDenseProduct.h"
+#include "src/SparseCore/SparseSelfAdjointView.h"
+#include "src/SparseCore/SparseTriangularView.h"
+#include "src/SparseCore/TriangularSolver.h"
+#include "src/SparseCore/SparsePermutation.h"
+#include "src/SparseCore/SparseFuzzy.h"
+#include "src/SparseCore/SparseSolverBase.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_SPARSECORE_MODULE_H
+
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseLU b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseLU
new file mode 100644
index 0000000..37c4a5c
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseLU
@@ -0,0 +1,50 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSELU_MODULE_H
+#define EIGEN_SPARSELU_MODULE_H
+
+#include "SparseCore"
+
+/** 
+  * \defgroup SparseLU_Module SparseLU module
+  * This module defines a supernodal factorization of general sparse matrices.
+  * The code is fully optimized for supernode-panel updates with specialized kernels.
+  * Please, see the documentation of the SparseLU class for more details.
+  */
+
+// Ordering interface
+#include "OrderingMethods"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+#include "src/SparseLU/SparseLU_gemm_kernel.h"
+
+#include "src/SparseLU/SparseLU_Structs.h"
+#include "src/SparseLU/SparseLU_SupernodalMatrix.h"
+#include "src/SparseLU/SparseLUImpl.h"
+#include "src/SparseCore/SparseColEtree.h"
+#include "src/SparseLU/SparseLU_Memory.h"
+#include "src/SparseLU/SparseLU_heap_relax_snode.h"
+#include "src/SparseLU/SparseLU_relax_snode.h"
+#include "src/SparseLU/SparseLU_pivotL.h"
+#include "src/SparseLU/SparseLU_panel_dfs.h"
+#include "src/SparseLU/SparseLU_kernel_bmod.h"
+#include "src/SparseLU/SparseLU_panel_bmod.h"
+#include "src/SparseLU/SparseLU_column_dfs.h"
+#include "src/SparseLU/SparseLU_column_bmod.h"
+#include "src/SparseLU/SparseLU_copy_to_ucol.h"
+#include "src/SparseLU/SparseLU_pruneL.h"
+#include "src/SparseLU/SparseLU_Utils.h"
+#include "src/SparseLU/SparseLU.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_SPARSELU_MODULE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseQR b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseQR
new file mode 100644
index 0000000..f5fc5fa
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/SparseQR
@@ -0,0 +1,36 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// 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_SPARSEQR_MODULE_H
+#define EIGEN_SPARSEQR_MODULE_H
+
+#include "SparseCore"
+#include "OrderingMethods"
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/** \defgroup SparseQR_Module SparseQR module
+  * \brief Provides QR decomposition for sparse matrices
+  * 
+  * This module provides a simplicial version of the left-looking Sparse QR decomposition. 
+  * The columns of the input matrix should be reordered to limit the fill-in during the 
+  * decomposition. Built-in methods (COLAMD, AMD) or external  methods (METIS) can be used to this end.
+  * See the \link OrderingMethods_Module OrderingMethods\endlink module for the list 
+  * of built-in and external ordering methods.
+  * 
+  * \code
+  * #include <Eigen/SparseQR>
+  * \endcode
+  * 
+  * 
+  */
+
+#include "src/SparseCore/SparseColEtree.h"
+#include "src/SparseQR/SparseQR.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/StdDeque b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdDeque
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/StdDeque
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdDeque
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/StdList b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdList
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/StdList
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdList
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/StdVector b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdVector
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/StdVector
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/StdVector
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LDLT.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LDLT.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LDLT.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LDLT.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LLT.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LLT.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LLT.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Cholesky/LLT.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArithmeticSequence.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArithmeticSequence.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArithmeticSequence.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArithmeticSequence.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Array.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Array.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Array.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Array.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayWrapper.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayWrapper.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayWrapper.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ArrayWrapper.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Assign.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Assign.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/AssignEvaluator.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/AssignEvaluator.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/BandMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BandMatrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/BandMatrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BandMatrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Block.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Block.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Block.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Block.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/BooleanRedux.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BooleanRedux.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/BooleanRedux.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/BooleanRedux.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CommaInitializer.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CommaInitializer.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/CommaInitializer.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CommaInitializer.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ConditionEstimator.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ConditionEstimator.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/ConditionEstimator.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ConditionEstimator.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreEvaluators.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreEvaluators.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreIterators.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreIterators.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreIterators.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreIterators.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseBinaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseBinaryOp.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseBinaryOp.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseBinaryOp.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseNullaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseNullaryOp.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseTernaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseTernaryOp.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseTernaryOp.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseTernaryOp.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryOp.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryOp.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryOp.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryView.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseUnaryView.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseCoeffsBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseCoeffsBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseCoeffsBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseStorage.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseStorage.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseStorage.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseStorage.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Diagonal.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Diagonal.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Diagonal.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Diagonal.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalMatrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalMatrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalMatrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalProduct.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalProduct.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DiagonalProduct.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Dot.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Dot.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Dot.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Dot.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/EigenBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/EigenBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ForceAlignedAccess.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ForceAlignedAccess.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/ForceAlignedAccess.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ForceAlignedAccess.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Fuzzy.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Fuzzy.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Fuzzy.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Fuzzy.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GeneralProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GeneralProduct.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/GeneralProduct.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GeneralProduct.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GenericPacketMath.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/GenericPacketMath.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GlobalFunctions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GlobalFunctions.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/GlobalFunctions.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GlobalFunctions.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/IO.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IO.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/IO.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IO.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/IndexedView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/IndexedView.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/IndexedView.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Inverse.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Inverse.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Inverse.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Inverse.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Map.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Map.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Map.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Map.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MapBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MapBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctions.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctions.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctions.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctionsImpl.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctionsImpl.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MathFunctionsImpl.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Matrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Matrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Matrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Matrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NestByValue.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NestByValue.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/NestByValue.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NestByValue.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NoAlias.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NoAlias.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/NoAlias.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NoAlias.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NumTraits.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/NumTraits.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/NumTraits.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PartialReduxEvaluator.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PartialReduxEvaluator.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/PartialReduxEvaluator.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PartialReduxEvaluator.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PermutationMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PermutationMatrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/PermutationMatrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PermutationMatrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PlainObjectBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PlainObjectBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/PlainObjectBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/PlainObjectBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Product.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Product.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Product.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Product.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ProductEvaluators.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/ProductEvaluators.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Random.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Random.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Random.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Random.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Redux.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Redux.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Redux.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Redux.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Ref.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Ref.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Ref.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Ref.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Replicate.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Replicate.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Replicate.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Replicate.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reshaped.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reshaped.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reshaped.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reshaped.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ReturnByValue.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ReturnByValue.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/ReturnByValue.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ReturnByValue.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reverse.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reverse.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reverse.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Reverse.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Select.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Select.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Select.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Select.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfAdjointView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfAdjointView.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfAdjointView.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfAdjointView.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfCwiseBinaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfCwiseBinaryOp.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfCwiseBinaryOp.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SelfCwiseBinaryOp.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Solve.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Solve.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Solve.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Solve.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolveTriangular.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolveTriangular.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolveTriangular.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolveTriangular.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolverBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolverBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolverBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/SolverBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/StableNorm.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StableNorm.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/StableNorm.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StableNorm.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/StlIterators.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StlIterators.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/StlIterators.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/StlIterators.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Stride.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Stride.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Stride.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Stride.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Swap.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Swap.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Swap.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Swap.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpose.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpose.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpositions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpositions.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpositions.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Transpositions.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/TriangularMatrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/TriangularMatrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorBlock.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorBlock.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorBlock.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorBlock.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorwiseOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorwiseOp.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorwiseOp.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/VectorwiseOp.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Visitor.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Visitor.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/Visitor.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/Visitor.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/Complex.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/Complex.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/Complex.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/Complex.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/MathFunctions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/MathFunctions.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/MathFunctions.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/MathFunctions.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/PacketMath.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/PacketMath.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/PacketMath.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/PacketMath.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/TypeCasting.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/TypeCasting.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/TypeCasting.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/AVX/TypeCasting.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/BFloat16.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/BFloat16.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/BFloat16.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/BFloat16.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/ConjHelper.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/ConjHelper.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/ConjHelper.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/ConjHelper.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Half.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Half.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Half.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Half.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Settings.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Settings.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Settings.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/Settings.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/TypeCasting.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/TypeCasting.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/TypeCasting.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/Default/TypeCasting.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/Complex.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/Complex.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/Complex.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/Complex.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/MathFunctions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/MathFunctions.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/MathFunctions.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/MathFunctions.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/PacketMath.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/PacketMath.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/PacketMath.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/PacketMath.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/TypeCasting.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/TypeCasting.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/TypeCasting.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/NEON/TypeCasting.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/Complex.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/Complex.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/Complex.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/Complex.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/MathFunctions.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/MathFunctions.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/MathFunctions.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/MathFunctions.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/PacketMath.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/PacketMath.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/TypeCasting.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/TypeCasting.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/TypeCasting.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/TypeCasting.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/AssignmentFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/AssignmentFunctors.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/BinaryFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/BinaryFunctors.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/NullaryFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/NullaryFunctors.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/NullaryFunctors.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/NullaryFunctors.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/StlFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/StlFunctors.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/StlFunctors.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/StlFunctors.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/TernaryFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/TernaryFunctors.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/TernaryFunctors.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/TernaryFunctors.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/UnaryFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/UnaryFunctors.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/UnaryFunctors.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/UnaryFunctors.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralBlockPanelKernel.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralBlockPanelKernel.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralBlockPanelKernel.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixVector.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixVector.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/GeneralMatrixVector.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/Parallelizer.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/Parallelizer.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixVector.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixVector.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointMatrixVector.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointProduct.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointProduct.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointProduct.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointRank2Update.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointRank2Update.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointRank2Update.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/SelfadjointRank2Update.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixMatrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixMatrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixMatrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixVector.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixVector.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularMatrixVector.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverMatrix.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverMatrix.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverMatrix.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverVector.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverVector.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/TriangularSolverVector.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/BlasUtil.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/BlasUtil.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/BlasUtil.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/BlasUtil.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ConfigureVectorization.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ConfigureVectorization.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ConfigureVectorization.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ConfigureVectorization.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Constants.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Constants.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Constants.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Constants.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
similarity index 95%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
index 3bec072..d973255 100644
--- a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/DisableStupidWarnings.h
@@ -71,10 +71,17 @@
     // See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89325
     #pragma GCC diagnostic ignored "-Wattributes"
   #endif
-  #if __GNUC__==11
+  #if __GNUC__>=8
+    #pragma GCC diagnostic ignored "-Wclass-memaccess"
+  #endif
+  #if __GNUC__>=11
     // This warning is a false positive
     #pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
   #endif
+  #if __GNUC__==12
+    // This warning is a false positive
+    #pragma GCC diagnostic ignored "-Warray-bounds"
+  #endif
 #endif
 
 #if defined __NVCC__
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ForwardDeclarations.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ForwardDeclarations.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ForwardDeclarations.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ForwardDeclarations.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/IndexedViewHelper.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/IndexedViewHelper.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/IndexedViewHelper.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/IndexedViewHelper.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/IntegralConstant.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/IntegralConstant.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/IntegralConstant.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/IntegralConstant.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Memory.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Memory.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Memory.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Memory.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Meta.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Meta.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Meta.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Meta.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReenableStupidWarnings.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReenableStupidWarnings.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReenableStupidWarnings.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReenableStupidWarnings.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReshapedHelper.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReshapedHelper.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReshapedHelper.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/ReshapedHelper.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/StaticAssert.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/StaticAssert.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/StaticAssert.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/StaticAssert.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/SymbolicIndex.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/SymbolicIndex.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/SymbolicIndex.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/SymbolicIndex.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/XprHelper.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/XprHelper.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexEigenSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexEigenSolver.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexEigenSolver.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexEigenSolver.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexSchur.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexSchur.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexSchur.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/ComplexSchur.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/EigenSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/EigenSolver.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/EigenSolver.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/EigenSolver.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/HessenbergDecomposition.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/HessenbergDecomposition.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/HessenbergDecomposition.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/HessenbergDecomposition.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealQZ.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealQZ.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealQZ.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealQZ.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealSchur.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealSchur.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/Tridiagonalization.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/Tridiagonalization.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/Tridiagonalization.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/Tridiagonalization.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/BlockHouseholder.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/BlockHouseholder.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Householder/BlockHouseholder.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/BlockHouseholder.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/Householder.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/Householder.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Householder/Householder.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/Householder.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/HouseholderSequence.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/HouseholderSequence.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Householder/HouseholderSequence.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Householder/HouseholderSequence.h
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h
new file mode 100644
index 0000000..a117fc1
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h
@@ -0,0 +1,226 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2011-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_BASIC_PRECONDITIONERS_H
+#define EIGEN_BASIC_PRECONDITIONERS_H
+
+namespace Eigen {
+
+/** \ingroup IterativeLinearSolvers_Module
+  * \brief A preconditioner based on the digonal entries
+  *
+  * This class allows to approximately solve for A.x = b problems assuming A is a diagonal matrix.
+  * In other words, this preconditioner neglects all off diagonal entries and, in Eigen's language, solves for:
+    \code
+    A.diagonal().asDiagonal() . x = b
+    \endcode
+  *
+  * \tparam _Scalar the type of the scalar.
+  *
+  * \implsparsesolverconcept
+  *
+  * This preconditioner is suitable for both selfadjoint and general problems.
+  * The diagonal entries are pre-inverted and stored into a dense vector.
+  *
+  * \note A variant that has yet to be implemented would attempt to preserve the norm of each column.
+  *
+  * \sa class LeastSquareDiagonalPreconditioner, class ConjugateGradient
+  */
+template <typename _Scalar>
+class DiagonalPreconditioner
+{
+    typedef _Scalar Scalar;
+    typedef Matrix<Scalar,Dynamic,1> Vector;
+  public:
+    typedef typename Vector::StorageIndex StorageIndex;
+    enum {
+      ColsAtCompileTime = Dynamic,
+      MaxColsAtCompileTime = Dynamic
+    };
+
+    DiagonalPreconditioner() : m_isInitialized(false) {}
+
+    template<typename MatType>
+    explicit DiagonalPreconditioner(const MatType& mat) : m_invdiag(mat.cols())
+    {
+      compute(mat);
+    }
+
+    EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_invdiag.size(); }
+    EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_invdiag.size(); }
+
+    template<typename MatType>
+    DiagonalPreconditioner& analyzePattern(const MatType& )
+    {
+      return *this;
+    }
+
+    template<typename MatType>
+    DiagonalPreconditioner& factorize(const MatType& mat)
+    {
+      m_invdiag.resize(mat.cols());
+      for(int j=0; j<mat.outerSize(); ++j)
+      {
+        typename MatType::InnerIterator it(mat,j);
+        while(it && it.index()!=j) ++it;
+        if(it && it.index()==j && it.value()!=Scalar(0))
+          m_invdiag(j) = Scalar(1)/it.value();
+        else
+          m_invdiag(j) = Scalar(1);
+      }
+      m_isInitialized = true;
+      return *this;
+    }
+
+    template<typename MatType>
+    DiagonalPreconditioner& compute(const MatType& mat)
+    {
+      return factorize(mat);
+    }
+
+    /** \internal */
+    template<typename Rhs, typename Dest>
+    void _solve_impl(const Rhs& b, Dest& x) const
+    {
+      x = m_invdiag.array() * b.array() ;
+    }
+
+    template<typename Rhs> inline const Solve<DiagonalPreconditioner, Rhs>
+    solve(const MatrixBase<Rhs>& b) const
+    {
+      eigen_assert(m_isInitialized && "DiagonalPreconditioner is not initialized.");
+      eigen_assert(m_invdiag.size()==b.rows()
+                && "DiagonalPreconditioner::solve(): invalid number of rows of the right hand side matrix b");
+      return Solve<DiagonalPreconditioner, Rhs>(*this, b.derived());
+    }
+
+    ComputationInfo info() { return Success; }
+
+  protected:
+    Vector m_invdiag;
+    bool m_isInitialized;
+};
+
+/** \ingroup IterativeLinearSolvers_Module
+  * \brief Jacobi preconditioner for LeastSquaresConjugateGradient
+  *
+  * This class allows to approximately solve for A' A x  = A' b problems assuming A' A is a diagonal matrix.
+  * In other words, this preconditioner neglects all off diagonal entries and, in Eigen's language, solves for:
+    \code
+    (A.adjoint() * A).diagonal().asDiagonal() * x = b
+    \endcode
+  *
+  * \tparam _Scalar the type of the scalar.
+  *
+  * \implsparsesolverconcept
+  *
+  * The diagonal entries are pre-inverted and stored into a dense vector.
+  *
+  * \sa class LeastSquaresConjugateGradient, class DiagonalPreconditioner
+  */
+template <typename _Scalar>
+class LeastSquareDiagonalPreconditioner : public DiagonalPreconditioner<_Scalar>
+{
+    typedef _Scalar Scalar;
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+    typedef DiagonalPreconditioner<_Scalar> Base;
+    using Base::m_invdiag;
+  public:
+
+    LeastSquareDiagonalPreconditioner() : Base() {}
+
+    template<typename MatType>
+    explicit LeastSquareDiagonalPreconditioner(const MatType& mat) : Base()
+    {
+      compute(mat);
+    }
+
+    template<typename MatType>
+    LeastSquareDiagonalPreconditioner& analyzePattern(const MatType& )
+    {
+      return *this;
+    }
+
+    template<typename MatType>
+    LeastSquareDiagonalPreconditioner& factorize(const MatType& mat)
+    {
+      // Compute the inverse squared-norm of each column of mat
+      m_invdiag.resize(mat.cols());
+      if(MatType::IsRowMajor)
+      {
+        m_invdiag.setZero();
+        for(Index j=0; j<mat.outerSize(); ++j)
+        {
+          for(typename MatType::InnerIterator it(mat,j); it; ++it)
+            m_invdiag(it.index()) += numext::abs2(it.value());
+        }
+        for(Index j=0; j<mat.cols(); ++j)
+          if(numext::real(m_invdiag(j))>RealScalar(0))
+            m_invdiag(j) = RealScalar(1)/numext::real(m_invdiag(j));
+      }
+      else
+      {
+        for(Index j=0; j<mat.outerSize(); ++j)
+        {
+          RealScalar sum = mat.col(j).squaredNorm();
+          if(sum>RealScalar(0))
+            m_invdiag(j) = RealScalar(1)/sum;
+          else
+            m_invdiag(j) = RealScalar(1);
+        }
+      }
+      Base::m_isInitialized = true;
+      return *this;
+    }
+
+    template<typename MatType>
+    LeastSquareDiagonalPreconditioner& compute(const MatType& mat)
+    {
+      return factorize(mat);
+    }
+
+    ComputationInfo info() { return Success; }
+
+  protected:
+};
+
+/** \ingroup IterativeLinearSolvers_Module
+  * \brief A naive preconditioner which approximates any matrix as the identity matrix
+  *
+  * \implsparsesolverconcept
+  *
+  * \sa class DiagonalPreconditioner
+  */
+class IdentityPreconditioner
+{
+  public:
+
+    IdentityPreconditioner() {}
+
+    template<typename MatrixType>
+    explicit IdentityPreconditioner(const MatrixType& ) {}
+
+    template<typename MatrixType>
+    IdentityPreconditioner& analyzePattern(const MatrixType& ) { return *this; }
+
+    template<typename MatrixType>
+    IdentityPreconditioner& factorize(const MatrixType& ) { return *this; }
+
+    template<typename MatrixType>
+    IdentityPreconditioner& compute(const MatrixType& ) { return *this; }
+
+    template<typename Rhs>
+    inline const Rhs& solve(const Rhs& b) const { return b; }
+
+    ComputationInfo info() { return Success; }
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_BASIC_PRECONDITIONERS_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h
new file mode 100644
index 0000000..153acef
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h
@@ -0,0 +1,212 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2011-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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_BICGSTAB_H
+#define EIGEN_BICGSTAB_H
+
+namespace Eigen { 
+
+namespace internal {
+
+/** \internal Low-level bi conjugate gradient stabilized algorithm
+  * \param mat The matrix A
+  * \param rhs The right hand side vector b
+  * \param x On input and initial solution, on output the computed solution.
+  * \param precond A preconditioner being able to efficiently solve for an
+  *                approximation of Ax=b (regardless of b)
+  * \param iters On input the max number of iteration, on output the number of performed iterations.
+  * \param tol_error On input the tolerance error, on output an estimation of the relative error.
+  * \return false in the case of numerical issue, for example a break down of BiCGSTAB. 
+  */
+template<typename MatrixType, typename Rhs, typename Dest, typename Preconditioner>
+bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x,
+              const Preconditioner& precond, Index& iters,
+              typename Dest::RealScalar& tol_error)
+{
+  using std::sqrt;
+  using std::abs;
+  typedef typename Dest::RealScalar RealScalar;
+  typedef typename Dest::Scalar Scalar;
+  typedef Matrix<Scalar,Dynamic,1> VectorType;
+  RealScalar tol = tol_error;
+  Index maxIters = iters;
+
+  Index n = mat.cols();
+  VectorType r  = rhs - mat * x;
+  VectorType r0 = r;
+  
+  RealScalar r0_sqnorm = r0.squaredNorm();
+  RealScalar rhs_sqnorm = rhs.squaredNorm();
+  if(rhs_sqnorm == 0)
+  {
+    x.setZero();
+    return true;
+  }
+  Scalar rho    = 1;
+  Scalar alpha  = 1;
+  Scalar w      = 1;
+  
+  VectorType v = VectorType::Zero(n), p = VectorType::Zero(n);
+  VectorType y(n),  z(n);
+  VectorType kt(n), ks(n);
+
+  VectorType s(n), t(n);
+
+  RealScalar tol2 = tol*tol*rhs_sqnorm;
+  RealScalar eps2 = NumTraits<Scalar>::epsilon()*NumTraits<Scalar>::epsilon();
+  Index i = 0;
+  Index restarts = 0;
+
+  while ( r.squaredNorm() > tol2 && i<maxIters )
+  {
+    Scalar rho_old = rho;
+
+    rho = r0.dot(r);
+    if (abs(rho) < eps2*r0_sqnorm)
+    {
+      // The new residual vector became too orthogonal to the arbitrarily chosen direction r0
+      // Let's restart with a new r0:
+      r  = rhs - mat * x;
+      r0 = r;
+      rho = r0_sqnorm = r.squaredNorm();
+      if(restarts++ == 0)
+        i = 0;
+    }
+    Scalar beta = (rho/rho_old) * (alpha / w);
+    p = r + beta * (p - w * v);
+    
+    y = precond.solve(p);
+    
+    v.noalias() = mat * y;
+
+    alpha = rho / r0.dot(v);
+    s = r - alpha * v;
+
+    z = precond.solve(s);
+    t.noalias() = mat * z;
+
+    RealScalar tmp = t.squaredNorm();
+    if(tmp>RealScalar(0))
+      w = t.dot(s) / tmp;
+    else
+      w = Scalar(0);
+    x += alpha * y + w * z;
+    r = s - w * t;
+    ++i;
+  }
+  tol_error = sqrt(r.squaredNorm()/rhs_sqnorm);
+  iters = i;
+  return true; 
+}
+
+}
+
+template< typename _MatrixType,
+          typename _Preconditioner = DiagonalPreconditioner<typename _MatrixType::Scalar> >
+class BiCGSTAB;
+
+namespace internal {
+
+template< typename _MatrixType, typename _Preconditioner>
+struct traits<BiCGSTAB<_MatrixType,_Preconditioner> >
+{
+  typedef _MatrixType MatrixType;
+  typedef _Preconditioner Preconditioner;
+};
+
+}
+
+/** \ingroup IterativeLinearSolvers_Module
+  * \brief A bi conjugate gradient stabilized solver for sparse square problems
+  *
+  * This class allows to solve for A.x = b sparse linear problems using a bi conjugate gradient
+  * stabilized algorithm. The vectors x and b can be either dense or sparse.
+  *
+  * \tparam _MatrixType the type of the sparse matrix A, can be a dense or a sparse matrix.
+  * \tparam _Preconditioner the type of the preconditioner. Default is DiagonalPreconditioner
+  *
+  * \implsparsesolverconcept
+  *
+  * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations()
+  * and setTolerance() methods. The defaults are the size of the problem for the maximal number of iterations
+  * and NumTraits<Scalar>::epsilon() for the tolerance.
+  * 
+  * The tolerance corresponds to the relative residual error: |Ax-b|/|b|
+  * 
+  * \b Performance: when using sparse matrices, best performance is achied for a row-major sparse matrix format.
+  * Moreover, in this case multi-threading can be exploited if the user code is compiled with OpenMP enabled.
+  * See \ref TopicMultiThreading for details.
+  * 
+  * This class can be used as the direct solver classes. Here is a typical usage example:
+  * \include BiCGSTAB_simple.cpp
+  * 
+  * By default the iterations start with x=0 as an initial guess of the solution.
+  * One can control the start using the solveWithGuess() method.
+  * 
+  * BiCGSTAB can also be used in a matrix-free context, see the following \link MatrixfreeSolverExample example \endlink.
+  *
+  * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
+  */
+template< typename _MatrixType, typename _Preconditioner>
+class BiCGSTAB : public IterativeSolverBase<BiCGSTAB<_MatrixType,_Preconditioner> >
+{
+  typedef IterativeSolverBase<BiCGSTAB> Base;
+  using Base::matrix;
+  using Base::m_error;
+  using Base::m_iterations;
+  using Base::m_info;
+  using Base::m_isInitialized;
+public:
+  typedef _MatrixType MatrixType;
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::RealScalar RealScalar;
+  typedef _Preconditioner Preconditioner;
+
+public:
+
+  /** Default constructor. */
+  BiCGSTAB() : Base() {}
+
+  /** Initialize the solver with matrix \a A for further \c Ax=b solving.
+    * 
+    * This constructor is a shortcut for the default constructor followed
+    * by a call to compute().
+    * 
+    * \warning this class stores a reference to the matrix A as well as some
+    * precomputed values that depend on it. Therefore, if \a A is changed
+    * this class becomes invalid. Call compute() to update it with the new
+    * matrix A, or modify a copy of A.
+    */
+  template<typename MatrixDerived>
+  explicit BiCGSTAB(const EigenBase<MatrixDerived>& A) : Base(A.derived()) {}
+
+  ~BiCGSTAB() {}
+
+  /** \internal */
+  template<typename Rhs,typename Dest>
+  void _solve_vector_with_guess_impl(const Rhs& b, Dest& x) const
+  {    
+    m_iterations = Base::maxIterations();
+    m_error = Base::m_tolerance;
+    
+    bool ret = internal::bicgstab(matrix(), b, x, Base::m_preconditioner, m_iterations, m_error);
+
+    m_info = (!ret) ? NumericalIssue
+           : m_error <= Base::m_tolerance ? Success
+           : NoConvergence;
+  }
+
+protected:
+
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_BICGSTAB_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
new file mode 100644
index 0000000..5d8c6b4
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
@@ -0,0 +1,229 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2011-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_CONJUGATE_GRADIENT_H
+#define EIGEN_CONJUGATE_GRADIENT_H
+
+namespace Eigen { 
+
+namespace internal {
+
+/** \internal Low-level conjugate gradient algorithm
+  * \param mat The matrix A
+  * \param rhs The right hand side vector b
+  * \param x On input and initial solution, on output the computed solution.
+  * \param precond A preconditioner being able to efficiently solve for an
+  *                approximation of Ax=b (regardless of b)
+  * \param iters On input the max number of iteration, on output the number of performed iterations.
+  * \param tol_error On input the tolerance error, on output an estimation of the relative error.
+  */
+template<typename MatrixType, typename Rhs, typename Dest, typename Preconditioner>
+EIGEN_DONT_INLINE
+void conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& x,
+                        const Preconditioner& precond, Index& iters,
+                        typename Dest::RealScalar& tol_error)
+{
+  using std::sqrt;
+  using std::abs;
+  typedef typename Dest::RealScalar RealScalar;
+  typedef typename Dest::Scalar Scalar;
+  typedef Matrix<Scalar,Dynamic,1> VectorType;
+  
+  RealScalar tol = tol_error;
+  Index maxIters = iters;
+  
+  Index n = mat.cols();
+
+  VectorType residual = rhs - mat * x; //initial residual
+
+  RealScalar rhsNorm2 = rhs.squaredNorm();
+  if(rhsNorm2 == 0) 
+  {
+    x.setZero();
+    iters = 0;
+    tol_error = 0;
+    return;
+  }
+  const RealScalar considerAsZero = (std::numeric_limits<RealScalar>::min)();
+  RealScalar threshold = numext::maxi(RealScalar(tol*tol*rhsNorm2),considerAsZero);
+  RealScalar residualNorm2 = residual.squaredNorm();
+  if (residualNorm2 < threshold)
+  {
+    iters = 0;
+    tol_error = sqrt(residualNorm2 / rhsNorm2);
+    return;
+  }
+
+  VectorType p(n);
+  p = precond.solve(residual);      // initial search direction
+
+  VectorType z(n), tmp(n);
+  RealScalar absNew = numext::real(residual.dot(p));  // the square of the absolute value of r scaled by invM
+  Index i = 0;
+  while(i < maxIters)
+  {
+    tmp.noalias() = mat * p;                    // the bottleneck of the algorithm
+
+    Scalar alpha = absNew / p.dot(tmp);         // the amount we travel on dir
+    x += alpha * p;                             // update solution
+    residual -= alpha * tmp;                    // update residual
+    
+    residualNorm2 = residual.squaredNorm();
+    if(residualNorm2 < threshold)
+      break;
+    
+    z = precond.solve(residual);                // approximately solve for "A z = residual"
+
+    RealScalar absOld = absNew;
+    absNew = numext::real(residual.dot(z));     // update the absolute value of r
+    RealScalar beta = absNew / absOld;          // calculate the Gram-Schmidt value used to create the new search direction
+    p = z + beta * p;                           // update search direction
+    i++;
+  }
+  tol_error = sqrt(residualNorm2 / rhsNorm2);
+  iters = i;
+}
+
+}
+
+template< typename _MatrixType, int _UpLo=Lower,
+          typename _Preconditioner = DiagonalPreconditioner<typename _MatrixType::Scalar> >
+class ConjugateGradient;
+
+namespace internal {
+
+template< typename _MatrixType, int _UpLo, typename _Preconditioner>
+struct traits<ConjugateGradient<_MatrixType,_UpLo,_Preconditioner> >
+{
+  typedef _MatrixType MatrixType;
+  typedef _Preconditioner Preconditioner;
+};
+
+}
+
+/** \ingroup IterativeLinearSolvers_Module
+  * \brief A conjugate gradient solver for sparse (or dense) self-adjoint problems
+  *
+  * This class allows to solve for A.x = b linear problems using an iterative conjugate gradient algorithm.
+  * The matrix A must be selfadjoint. The matrix A and the vectors x and b can be either dense or sparse.
+  *
+  * \tparam _MatrixType the type of the matrix A, can be a dense or a sparse matrix.
+  * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower,
+  *               \c Upper, or \c Lower|Upper in which the full matrix entries will be considered.
+  *               Default is \c Lower, best performance is \c Lower|Upper.
+  * \tparam _Preconditioner the type of the preconditioner. Default is DiagonalPreconditioner
+  *
+  * \implsparsesolverconcept
+  *
+  * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations()
+  * and setTolerance() methods. The defaults are the size of the problem for the maximal number of iterations
+  * and NumTraits<Scalar>::epsilon() for the tolerance.
+  * 
+  * The tolerance corresponds to the relative residual error: |Ax-b|/|b|
+  * 
+  * \b Performance: Even though the default value of \c _UpLo is \c Lower, significantly higher performance is
+  * achieved when using a complete matrix and \b Lower|Upper as the \a _UpLo template parameter. Moreover, in this
+  * case multi-threading can be exploited if the user code is compiled with OpenMP enabled.
+  * See \ref TopicMultiThreading for details.
+  * 
+  * This class can be used as the direct solver classes. Here is a typical usage example:
+    \code
+    int n = 10000;
+    VectorXd x(n), b(n);
+    SparseMatrix<double> A(n,n);
+    // fill A and b
+    ConjugateGradient<SparseMatrix<double>, Lower|Upper> cg;
+    cg.compute(A);
+    x = cg.solve(b);
+    std::cout << "#iterations:     " << cg.iterations() << std::endl;
+    std::cout << "estimated error: " << cg.error()      << std::endl;
+    // update b, and solve again
+    x = cg.solve(b);
+    \endcode
+  * 
+  * By default the iterations start with x=0 as an initial guess of the solution.
+  * One can control the start using the solveWithGuess() method.
+  * 
+  * ConjugateGradient can also be used in a matrix-free context, see the following \link MatrixfreeSolverExample example \endlink.
+  *
+  * \sa class LeastSquaresConjugateGradient, class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
+  */
+template< typename _MatrixType, int _UpLo, typename _Preconditioner>
+class ConjugateGradient : public IterativeSolverBase<ConjugateGradient<_MatrixType,_UpLo,_Preconditioner> >
+{
+  typedef IterativeSolverBase<ConjugateGradient> Base;
+  using Base::matrix;
+  using Base::m_error;
+  using Base::m_iterations;
+  using Base::m_info;
+  using Base::m_isInitialized;
+public:
+  typedef _MatrixType MatrixType;
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::RealScalar RealScalar;
+  typedef _Preconditioner Preconditioner;
+
+  enum {
+    UpLo = _UpLo
+  };
+
+public:
+
+  /** Default constructor. */
+  ConjugateGradient() : Base() {}
+
+  /** Initialize the solver with matrix \a A for further \c Ax=b solving.
+    * 
+    * This constructor is a shortcut for the default constructor followed
+    * by a call to compute().
+    * 
+    * \warning this class stores a reference to the matrix A as well as some
+    * precomputed values that depend on it. Therefore, if \a A is changed
+    * this class becomes invalid. Call compute() to update it with the new
+    * matrix A, or modify a copy of A.
+    */
+  template<typename MatrixDerived>
+  explicit ConjugateGradient(const EigenBase<MatrixDerived>& A) : Base(A.derived()) {}
+
+  ~ConjugateGradient() {}
+
+  /** \internal */
+  template<typename Rhs,typename Dest>
+  void _solve_vector_with_guess_impl(const Rhs& b, Dest& x) const
+  {
+    typedef typename Base::MatrixWrapper MatrixWrapper;
+    typedef typename Base::ActualMatrixType ActualMatrixType;
+    enum {
+      TransposeInput  =   (!MatrixWrapper::MatrixFree)
+                      &&  (UpLo==(Lower|Upper))
+                      &&  (!MatrixType::IsRowMajor)
+                      &&  (!NumTraits<Scalar>::IsComplex)
+    };
+    typedef typename internal::conditional<TransposeInput,Transpose<const ActualMatrixType>, ActualMatrixType const&>::type RowMajorWrapper;
+    EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(MatrixWrapper::MatrixFree,UpLo==(Lower|Upper)),MATRIX_FREE_CONJUGATE_GRADIENT_IS_COMPATIBLE_WITH_UPPER_UNION_LOWER_MODE_ONLY);
+    typedef typename internal::conditional<UpLo==(Lower|Upper),
+                                           RowMajorWrapper,
+                                           typename MatrixWrapper::template ConstSelfAdjointViewReturnType<UpLo>::Type
+                                          >::type SelfAdjointWrapper;
+
+    m_iterations = Base::maxIterations();
+    m_error = Base::m_tolerance;
+
+    RowMajorWrapper row_mat(matrix());
+    internal::conjugate_gradient(SelfAdjointWrapper(row_mat), b, x, Base::m_preconditioner, m_iterations, m_error);
+    m_info = m_error <= Base::m_tolerance ? Success : NoConvergence;
+  }
+
+protected:
+
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_CONJUGATE_GRADIENT_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h
new file mode 100644
index 0000000..7803fd8
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h
@@ -0,0 +1,394 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_INCOMPLETE_CHOlESKY_H
+#define EIGEN_INCOMPLETE_CHOlESKY_H
+
+#include <vector>
+#include <list>
+
+namespace Eigen {
+/**
+  * \brief Modified Incomplete Cholesky with dual threshold
+  *
+  * References : C-J. Lin and J. J. Moré, Incomplete Cholesky Factorizations with
+  *              Limited memory, SIAM J. Sci. Comput.  21(1), pp. 24-45, 1999
+  *
+  * \tparam Scalar the scalar type of the input matrices
+  * \tparam _UpLo The triangular part that will be used for the computations. It can be Lower
+    *               or Upper. Default is Lower.
+  * \tparam _OrderingType The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<int>,
+  *                       unless EIGEN_MPL2_ONLY is defined, in which case the default is NaturalOrdering<int>.
+  *
+  * \implsparsesolverconcept
+  *
+  * It performs the following incomplete factorization: \f$ S P A P' S \approx L L' \f$
+  * where L is a lower triangular factor, S is a diagonal scaling matrix, and P is a
+  * fill-in reducing permutation as computed by the ordering method.
+  *
+  * \b Shifting \b strategy: Let \f$ B = S P A P' S \f$  be the scaled matrix on which the factorization is carried out,
+  * and \f$ \beta \f$ be the minimum value of the diagonal. If \f$ \beta > 0 \f$ then, the factorization is directly performed
+  * on the matrix B. Otherwise, the factorization is performed on the shifted matrix \f$ B + (\sigma+|\beta| I \f$ where
+  * \f$ \sigma \f$ is the initial shift value as returned and set by setInitialShift() method. The default value is \f$ \sigma = 10^{-3} \f$.
+  * If the factorization fails, then the shift in doubled until it succeed or a maximum of ten attempts. If it still fails, as returned by
+  * the info() method, then you can either increase the initial shift, or better use another preconditioning technique.
+  *
+  */
+template <typename Scalar, int _UpLo = Lower, typename _OrderingType = AMDOrdering<int> >
+class IncompleteCholesky : public SparseSolverBase<IncompleteCholesky<Scalar,_UpLo,_OrderingType> >
+{
+  protected:
+    typedef SparseSolverBase<IncompleteCholesky<Scalar,_UpLo,_OrderingType> > Base;
+    using Base::m_isInitialized;
+  public:
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+    typedef _OrderingType OrderingType;
+    typedef typename OrderingType::PermutationType PermutationType;
+    typedef typename PermutationType::StorageIndex StorageIndex;
+    typedef SparseMatrix<Scalar,ColMajor,StorageIndex> FactorType;
+    typedef Matrix<Scalar,Dynamic,1> VectorSx;
+    typedef Matrix<RealScalar,Dynamic,1> VectorRx;
+    typedef Matrix<StorageIndex,Dynamic, 1> VectorIx;
+    typedef std::vector<std::list<StorageIndex> > VectorList;
+    enum { UpLo = _UpLo };
+    enum {
+      ColsAtCompileTime = Dynamic,
+      MaxColsAtCompileTime = Dynamic
+    };
+  public:
+
+    /** Default constructor leaving the object in a partly non-initialized stage.
+      *
+      * You must call compute() or the pair analyzePattern()/factorize() to make it valid.
+      *
+      * \sa IncompleteCholesky(const MatrixType&)
+      */
+    IncompleteCholesky() : m_initialShift(1e-3),m_analysisIsOk(false),m_factorizationIsOk(false) {}
+
+    /** Constructor computing the incomplete factorization for the given matrix \a matrix.
+      */
+    template<typename MatrixType>
+    IncompleteCholesky(const MatrixType& matrix) : m_initialShift(1e-3),m_analysisIsOk(false),m_factorizationIsOk(false)
+    {
+      compute(matrix);
+    }
+
+    /** \returns number of rows of the factored matrix */
+    EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_L.rows(); }
+
+    /** \returns number of columns of the factored matrix */
+    EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_L.cols(); }
+
+
+    /** \brief Reports whether previous computation was successful.
+      *
+      * It triggers an assertion if \c *this has not been initialized through the respective constructor,
+      * or a call to compute() or analyzePattern().
+      *
+      * \returns \c Success if computation was successful,
+      *          \c NumericalIssue if the matrix appears to be negative.
+      */
+    ComputationInfo info() const
+    {
+      eigen_assert(m_isInitialized && "IncompleteCholesky is not initialized.");
+      return m_info;
+    }
+
+    /** \brief Set the initial shift parameter \f$ \sigma \f$.
+      */
+    void setInitialShift(RealScalar shift) { m_initialShift = shift; }
+
+    /** \brief Computes the fill reducing permutation vector using the sparsity pattern of \a mat
+      */
+    template<typename MatrixType>
+    void analyzePattern(const MatrixType& mat)
+    {
+      OrderingType ord;
+      PermutationType pinv;
+      ord(mat.template selfadjointView<UpLo>(), pinv);
+      if(pinv.size()>0) m_perm = pinv.inverse();
+      else              m_perm.resize(0);
+      m_L.resize(mat.rows(), mat.cols());
+      m_analysisIsOk = true;
+      m_isInitialized = true;
+      m_info = Success;
+    }
+
+    /** \brief Performs the numerical factorization of the input matrix \a mat
+      *
+      * The method analyzePattern() or compute() must have been called beforehand
+      * with a matrix having the same pattern.
+      *
+      * \sa compute(), analyzePattern()
+      */
+    template<typename MatrixType>
+    void factorize(const MatrixType& mat);
+
+    /** Computes or re-computes the incomplete Cholesky factorization of the input matrix \a mat
+      *
+      * It is a shortcut for a sequential call to the analyzePattern() and factorize() methods.
+      *
+      * \sa analyzePattern(), factorize()
+      */
+    template<typename MatrixType>
+    void compute(const MatrixType& mat)
+    {
+      analyzePattern(mat);
+      factorize(mat);
+    }
+
+    // internal
+    template<typename Rhs, typename Dest>
+    void _solve_impl(const Rhs& b, Dest& x) const
+    {
+      eigen_assert(m_factorizationIsOk && "factorize() should be called first");
+      if (m_perm.rows() == b.rows())  x = m_perm * b;
+      else                            x = b;
+      x = m_scale.asDiagonal() * x;
+      x = m_L.template triangularView<Lower>().solve(x);
+      x = m_L.adjoint().template triangularView<Upper>().solve(x);
+      x = m_scale.asDiagonal() * x;
+      if (m_perm.rows() == b.rows())
+        x = m_perm.inverse() * x;
+    }
+
+    /** \returns the sparse lower triangular factor L */
+    const FactorType& matrixL() const { eigen_assert("m_factorizationIsOk"); return m_L; }
+
+    /** \returns a vector representing the scaling factor S */
+    const VectorRx& scalingS() const { eigen_assert("m_factorizationIsOk"); return m_scale; }
+
+    /** \returns the fill-in reducing permutation P (can be empty for a natural ordering) */
+    const PermutationType& permutationP() const { eigen_assert("m_analysisIsOk"); return m_perm; }
+
+  protected:
+    FactorType m_L;              // The lower part stored in CSC
+    VectorRx m_scale;            // The vector for scaling the matrix
+    RealScalar m_initialShift;   // The initial shift parameter
+    bool m_analysisIsOk;
+    bool m_factorizationIsOk;
+    ComputationInfo m_info;
+    PermutationType m_perm;
+
+  private:
+    inline void updateList(Ref<const VectorIx> colPtr, Ref<VectorIx> rowIdx, Ref<VectorSx> vals, const Index& col, const Index& jk, VectorIx& firstElt, VectorList& listCol);
+};
+
+// Based on the following paper:
+//   C-J. Lin and J. J. Moré, Incomplete Cholesky Factorizations with
+//   Limited memory, SIAM J. Sci. Comput.  21(1), pp. 24-45, 1999
+//   http://ftp.mcs.anl.gov/pub/tech_reports/reports/P682.pdf
+template<typename Scalar, int _UpLo, typename OrderingType>
+template<typename _MatrixType>
+void IncompleteCholesky<Scalar,_UpLo, OrderingType>::factorize(const _MatrixType& mat)
+{
+  using std::sqrt;
+  eigen_assert(m_analysisIsOk && "analyzePattern() should be called first");
+
+  // Dropping strategy : Keep only the p largest elements per column, where p is the number of elements in the column of the original matrix. Other strategies will be added
+
+  // Apply the fill-reducing permutation computed in analyzePattern()
+  if (m_perm.rows() == mat.rows() ) // To detect the null permutation
+  {
+    // The temporary is needed to make sure that the diagonal entry is properly sorted
+    FactorType tmp(mat.rows(), mat.cols());
+    tmp = mat.template selfadjointView<_UpLo>().twistedBy(m_perm);
+    m_L.template selfadjointView<Lower>() = tmp.template selfadjointView<Lower>();
+  }
+  else
+  {
+    m_L.template selfadjointView<Lower>() = mat.template selfadjointView<_UpLo>();
+  }
+
+  Index n = m_L.cols();
+  Index nnz = m_L.nonZeros();
+  Map<VectorSx> vals(m_L.valuePtr(), nnz);         //values
+  Map<VectorIx> rowIdx(m_L.innerIndexPtr(), nnz);  //Row indices
+  Map<VectorIx> colPtr( m_L.outerIndexPtr(), n+1); // Pointer to the beginning of each row
+  VectorIx firstElt(n-1); // for each j, points to the next entry in vals that will be used in the factorization
+  VectorList listCol(n);  // listCol(j) is a linked list of columns to update column j
+  VectorSx col_vals(n);   // Store a  nonzero values in each column
+  VectorIx col_irow(n);   // Row indices of nonzero elements in each column
+  VectorIx col_pattern(n);
+  col_pattern.fill(-1);
+  StorageIndex col_nnz;
+
+
+  // Computes the scaling factors
+  m_scale.resize(n);
+  m_scale.setZero();
+  for (Index j = 0; j < n; j++)
+    for (Index k = colPtr[j]; k < colPtr[j+1]; k++)
+    {
+      m_scale(j) += numext::abs2(vals(k));
+      if(rowIdx[k]!=j)
+        m_scale(rowIdx[k]) += numext::abs2(vals(k));
+    }
+
+  m_scale = m_scale.cwiseSqrt().cwiseSqrt();
+
+  for (Index j = 0; j < n; ++j)
+    if(m_scale(j)>(std::numeric_limits<RealScalar>::min)())
+      m_scale(j) = RealScalar(1)/m_scale(j);
+    else
+      m_scale(j) = 1;
+
+  // TODO disable scaling if not needed, i.e., if it is roughly uniform? (this will make solve() faster)
+
+  // Scale and compute the shift for the matrix
+  RealScalar mindiag = NumTraits<RealScalar>::highest();
+  for (Index j = 0; j < n; j++)
+  {
+    for (Index k = colPtr[j]; k < colPtr[j+1]; k++)
+      vals[k] *= (m_scale(j)*m_scale(rowIdx[k]));
+    eigen_internal_assert(rowIdx[colPtr[j]]==j && "IncompleteCholesky: only the lower triangular part must be stored");
+    mindiag = numext::mini(numext::real(vals[colPtr[j]]), mindiag);
+  }
+
+  FactorType L_save = m_L;
+
+  RealScalar shift = 0;
+  if(mindiag <= RealScalar(0.))
+    shift = m_initialShift - mindiag;
+
+  m_info = NumericalIssue;
+
+  // Try to perform the incomplete factorization using the current shift
+  int iter = 0;
+  do
+  {
+    // Apply the shift to the diagonal elements of the matrix
+    for (Index j = 0; j < n; j++)
+      vals[colPtr[j]] += shift;
+
+    // jki version of the Cholesky factorization
+    Index j=0;
+    for (; j < n; ++j)
+    {
+      // Left-looking factorization of the j-th column
+      // First, load the j-th column into col_vals
+      Scalar diag = vals[colPtr[j]];  // It is assumed that only the lower part is stored
+      col_nnz = 0;
+      for (Index i = colPtr[j] + 1; i < colPtr[j+1]; i++)
+      {
+        StorageIndex l = rowIdx[i];
+        col_vals(col_nnz) = vals[i];
+        col_irow(col_nnz) = l;
+        col_pattern(l) = col_nnz;
+        col_nnz++;
+      }
+      {
+        typename std::list<StorageIndex>::iterator k;
+        // Browse all previous columns that will update column j
+        for(k = listCol[j].begin(); k != listCol[j].end(); k++)
+        {
+          Index jk = firstElt(*k); // First element to use in the column
+          eigen_internal_assert(rowIdx[jk]==j);
+          Scalar v_j_jk = numext::conj(vals[jk]);
+
+          jk += 1;
+          for (Index i = jk; i < colPtr[*k+1]; i++)
+          {
+            StorageIndex l = rowIdx[i];
+            if(col_pattern[l]<0)
+            {
+              col_vals(col_nnz) = vals[i] * v_j_jk;
+              col_irow[col_nnz] = l;
+              col_pattern(l) = col_nnz;
+              col_nnz++;
+            }
+            else
+              col_vals(col_pattern[l]) -= vals[i] * v_j_jk;
+          }
+          updateList(colPtr,rowIdx,vals, *k, jk, firstElt, listCol);
+        }
+      }
+
+      // Scale the current column
+      if(numext::real(diag) <= 0)
+      {
+        if(++iter>=10)
+          return;
+
+        // increase shift
+        shift = numext::maxi(m_initialShift,RealScalar(2)*shift);
+        // restore m_L, col_pattern, and listCol
+        vals = Map<const VectorSx>(L_save.valuePtr(), nnz);
+        rowIdx = Map<const VectorIx>(L_save.innerIndexPtr(), nnz);
+        colPtr = Map<const VectorIx>(L_save.outerIndexPtr(), n+1);
+        col_pattern.fill(-1);
+        for(Index i=0; i<n; ++i)
+          listCol[i].clear();
+
+        break;
+      }
+
+      RealScalar rdiag = sqrt(numext::real(diag));
+      vals[colPtr[j]] = rdiag;
+      for (Index k = 0; k<col_nnz; ++k)
+      {
+        Index i = col_irow[k];
+        //Scale
+        col_vals(k) /= rdiag;
+        //Update the remaining diagonals with col_vals
+        vals[colPtr[i]] -= numext::abs2(col_vals(k));
+      }
+      // Select the largest p elements
+      // p is the original number of elements in the column (without the diagonal)
+      Index p = colPtr[j+1] - colPtr[j] - 1 ;
+      Ref<VectorSx> cvals = col_vals.head(col_nnz);
+      Ref<VectorIx> cirow = col_irow.head(col_nnz);
+      internal::QuickSplit(cvals,cirow, p);
+      // Insert the largest p elements in the matrix
+      Index cpt = 0;
+      for (Index i = colPtr[j]+1; i < colPtr[j+1]; i++)
+      {
+        vals[i] = col_vals(cpt);
+        rowIdx[i] = col_irow(cpt);
+        // restore col_pattern:
+        col_pattern(col_irow(cpt)) = -1;
+        cpt++;
+      }
+      // Get the first smallest row index and put it after the diagonal element
+      Index jk = colPtr(j)+1;
+      updateList(colPtr,rowIdx,vals,j,jk,firstElt,listCol);
+    }
+
+    if(j==n)
+    {
+      m_factorizationIsOk = true;
+      m_info = Success;
+    }
+  } while(m_info!=Success);
+}
+
+template<typename Scalar, int _UpLo, typename OrderingType>
+inline void IncompleteCholesky<Scalar,_UpLo, OrderingType>::updateList(Ref<const VectorIx> colPtr, Ref<VectorIx> rowIdx, Ref<VectorSx> vals, const Index& col, const Index& jk, VectorIx& firstElt, VectorList& listCol)
+{
+  if (jk < colPtr(col+1) )
+  {
+    Index p = colPtr(col+1) - jk;
+    Index minpos;
+    rowIdx.segment(jk,p).minCoeff(&minpos);
+    minpos += jk;
+    if (rowIdx(minpos) != rowIdx(jk))
+    {
+      //Swap
+      std::swap(rowIdx(jk),rowIdx(minpos));
+      std::swap(vals(jk),vals(minpos));
+    }
+    firstElt(col) = internal::convert_index<StorageIndex,Index>(jk);
+    listCol[rowIdx(jk)].push_back(internal::convert_index<StorageIndex,Index>(col));
+  }
+}
+
+} // end namespace Eigen
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h
new file mode 100644
index 0000000..cdcf709
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h
@@ -0,0 +1,453 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_INCOMPLETE_LUT_H
+#define EIGEN_INCOMPLETE_LUT_H
+
+
+namespace Eigen {
+
+namespace internal {
+
+/** \internal
+  * Compute a quick-sort split of a vector
+  * On output, the vector row is permuted such that its elements satisfy
+  * abs(row(i)) >= abs(row(ncut)) if i<ncut
+  * abs(row(i)) <= abs(row(ncut)) if i>ncut
+  * \param row The vector of values
+  * \param ind The array of index for the elements in @p row
+  * \param ncut  The number of largest elements to keep
+  **/
+template <typename VectorV, typename VectorI>
+Index QuickSplit(VectorV &row, VectorI &ind, Index ncut)
+{
+  typedef typename VectorV::RealScalar RealScalar;
+  using std::swap;
+  using std::abs;
+  Index mid;
+  Index n = row.size(); /* length of the vector */
+  Index first, last ;
+
+  ncut--; /* to fit the zero-based indices */
+  first = 0;
+  last = n-1;
+  if (ncut < first || ncut > last ) return 0;
+
+  do {
+    mid = first;
+    RealScalar abskey = abs(row(mid));
+    for (Index j = first + 1; j <= last; j++) {
+      if ( abs(row(j)) > abskey) {
+        ++mid;
+        swap(row(mid), row(j));
+        swap(ind(mid), ind(j));
+      }
+    }
+    /* Interchange for the pivot element */
+    swap(row(mid), row(first));
+    swap(ind(mid), ind(first));
+
+    if (mid > ncut) last = mid - 1;
+    else if (mid < ncut ) first = mid + 1;
+  } while (mid != ncut );
+
+  return 0; /* mid is equal to ncut */
+}
+
+}// end namespace internal
+
+/** \ingroup IterativeLinearSolvers_Module
+  * \class IncompleteLUT
+  * \brief Incomplete LU factorization with dual-threshold strategy
+  *
+  * \implsparsesolverconcept
+  *
+  * During the numerical factorization, two dropping rules are used :
+  *  1) any element whose magnitude is less than some tolerance is dropped.
+  *    This tolerance is obtained by multiplying the input tolerance @p droptol
+  *    by the average magnitude of all the original elements in the current row.
+  *  2) After the elimination of the row, only the @p fill largest elements in
+  *    the L part and the @p fill largest elements in the U part are kept
+  *    (in addition to the diagonal element ). Note that @p fill is computed from
+  *    the input parameter @p fillfactor which is used the ratio to control the fill_in
+  *    relatively to the initial number of nonzero elements.
+  *
+  * The two extreme cases are when @p droptol=0 (to keep all the @p fill*2 largest elements)
+  * and when @p fill=n/2 with @p droptol being different to zero.
+  *
+  * References : Yousef Saad, ILUT: A dual threshold incomplete LU factorization,
+  *              Numerical Linear Algebra with Applications, 1(4), pp 387-402, 1994.
+  *
+  * NOTE : The following implementation is derived from the ILUT implementation
+  * in the SPARSKIT package, Copyright (C) 2005, the Regents of the University of Minnesota
+  *  released under the terms of the GNU LGPL:
+  *    http://www-users.cs.umn.edu/~saad/software/SPARSKIT/README
+  * However, Yousef Saad gave us permission to relicense his ILUT code to MPL2.
+  * See the Eigen mailing list archive, thread: ILUT, date: July 8, 2012:
+  *   http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2012/07/msg00064.html
+  * alternatively, on GMANE:
+  *   http://comments.gmane.org/gmane.comp.lib.eigen/3302
+  */
+template <typename _Scalar, typename _StorageIndex = int>
+class IncompleteLUT : public SparseSolverBase<IncompleteLUT<_Scalar, _StorageIndex> >
+{
+  protected:
+    typedef SparseSolverBase<IncompleteLUT> Base;
+    using Base::m_isInitialized;
+  public:
+    typedef _Scalar Scalar;
+    typedef _StorageIndex StorageIndex;
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+    typedef Matrix<Scalar,Dynamic,1> Vector;
+    typedef Matrix<StorageIndex,Dynamic,1> VectorI;
+    typedef SparseMatrix<Scalar,RowMajor,StorageIndex> FactorType;
+
+    enum {
+      ColsAtCompileTime = Dynamic,
+      MaxColsAtCompileTime = Dynamic
+    };
+
+  public:
+
+    IncompleteLUT()
+      : m_droptol(NumTraits<Scalar>::dummy_precision()), m_fillfactor(10),
+        m_analysisIsOk(false), m_factorizationIsOk(false)
+    {}
+
+    template<typename MatrixType>
+    explicit IncompleteLUT(const MatrixType& mat, const RealScalar& droptol=NumTraits<Scalar>::dummy_precision(), int fillfactor = 10)
+      : m_droptol(droptol),m_fillfactor(fillfactor),
+        m_analysisIsOk(false),m_factorizationIsOk(false)
+    {
+      eigen_assert(fillfactor != 0);
+      compute(mat);
+    }
+
+    EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_lu.rows(); }
+
+    EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_lu.cols(); }
+
+    /** \brief Reports whether previous computation was successful.
+      *
+      * \returns \c Success if computation was successful,
+      *          \c NumericalIssue if the matrix.appears to be negative.
+      */
+    ComputationInfo info() const
+    {
+      eigen_assert(m_isInitialized && "IncompleteLUT is not initialized.");
+      return m_info;
+    }
+
+    template<typename MatrixType>
+    void analyzePattern(const MatrixType& amat);
+
+    template<typename MatrixType>
+    void factorize(const MatrixType& amat);
+
+    /**
+      * Compute an incomplete LU factorization with dual threshold on the matrix mat
+      * No pivoting is done in this version
+      *
+      **/
+    template<typename MatrixType>
+    IncompleteLUT& compute(const MatrixType& amat)
+    {
+      analyzePattern(amat);
+      factorize(amat);
+      return *this;
+    }
+
+    void setDroptol(const RealScalar& droptol);
+    void setFillfactor(int fillfactor);
+
+    template<typename Rhs, typename Dest>
+    void _solve_impl(const Rhs& b, Dest& x) const
+    {
+      x = m_Pinv * b;
+      x = m_lu.template triangularView<UnitLower>().solve(x);
+      x = m_lu.template triangularView<Upper>().solve(x);
+      x = m_P * x;
+    }
+
+protected:
+
+    /** keeps off-diagonal entries; drops diagonal entries */
+    struct keep_diag {
+      inline bool operator() (const Index& row, const Index& col, const Scalar&) const
+      {
+        return row!=col;
+      }
+    };
+
+protected:
+
+    FactorType m_lu;
+    RealScalar m_droptol;
+    int m_fillfactor;
+    bool m_analysisIsOk;
+    bool m_factorizationIsOk;
+    ComputationInfo m_info;
+    PermutationMatrix<Dynamic,Dynamic,StorageIndex> m_P;     // Fill-reducing permutation
+    PermutationMatrix<Dynamic,Dynamic,StorageIndex> m_Pinv;  // Inverse permutation
+};
+
+/**
+ * Set control parameter droptol
+ *  \param droptol   Drop any element whose magnitude is less than this tolerance
+ **/
+template<typename Scalar, typename StorageIndex>
+void IncompleteLUT<Scalar,StorageIndex>::setDroptol(const RealScalar& droptol)
+{
+  this->m_droptol = droptol;
+}
+
+/**
+ * Set control parameter fillfactor
+ * \param fillfactor  This is used to compute the  number @p fill_in of largest elements to keep on each row.
+ **/
+template<typename Scalar, typename StorageIndex>
+void IncompleteLUT<Scalar,StorageIndex>::setFillfactor(int fillfactor)
+{
+  this->m_fillfactor = fillfactor;
+}
+
+template <typename Scalar, typename StorageIndex>
+template<typename _MatrixType>
+void IncompleteLUT<Scalar,StorageIndex>::analyzePattern(const _MatrixType& amat)
+{
+  // Compute the Fill-reducing permutation
+  // Since ILUT does not perform any numerical pivoting,
+  // it is highly preferable to keep the diagonal through symmetric permutations.
+  // To this end, let's symmetrize the pattern and perform AMD on it.
+  SparseMatrix<Scalar,ColMajor, StorageIndex> mat1 = amat;
+  SparseMatrix<Scalar,ColMajor, StorageIndex> mat2 = amat.transpose();
+  // FIXME for a matrix with nearly symmetric pattern, mat2+mat1 is the appropriate choice.
+  //       on the other hand for a really non-symmetric pattern, mat2*mat1 should be preferred...
+  SparseMatrix<Scalar,ColMajor, StorageIndex> AtA = mat2 + mat1;
+  AMDOrdering<StorageIndex> ordering;
+  ordering(AtA,m_P);
+  m_Pinv  = m_P.inverse(); // cache the inverse permutation
+  m_analysisIsOk = true;
+  m_factorizationIsOk = false;
+  m_isInitialized = true;
+}
+
+template <typename Scalar, typename StorageIndex>
+template<typename _MatrixType>
+void IncompleteLUT<Scalar,StorageIndex>::factorize(const _MatrixType& amat)
+{
+  using std::sqrt;
+  using std::swap;
+  using std::abs;
+  using internal::convert_index;
+
+  eigen_assert((amat.rows() == amat.cols()) && "The factorization should be done on a square matrix");
+  Index n = amat.cols();  // Size of the matrix
+  m_lu.resize(n,n);
+  // Declare Working vectors and variables
+  Vector u(n) ;     // real values of the row -- maximum size is n --
+  VectorI ju(n);   // column position of the values in u -- maximum size  is n
+  VectorI jr(n);   // Indicate the position of the nonzero elements in the vector u -- A zero location is indicated by -1
+
+  // Apply the fill-reducing permutation
+  eigen_assert(m_analysisIsOk && "You must first call analyzePattern()");
+  SparseMatrix<Scalar,RowMajor, StorageIndex> mat;
+  mat = amat.twistedBy(m_Pinv);
+
+  // Initialization
+  jr.fill(-1);
+  ju.fill(0);
+  u.fill(0);
+
+  // number of largest elements to keep in each row:
+  Index fill_in = (amat.nonZeros()*m_fillfactor)/n + 1;
+  if (fill_in > n) fill_in = n;
+
+  // number of largest nonzero elements to keep in the L and the U part of the current row:
+  Index nnzL = fill_in/2;
+  Index nnzU = nnzL;
+  m_lu.reserve(n * (nnzL + nnzU + 1));
+
+  // global loop over the rows of the sparse matrix
+  for (Index ii = 0; ii < n; ii++)
+  {
+    // 1 - copy the lower and the upper part of the row i of mat in the working vector u
+
+    Index sizeu = 1; // number of nonzero elements in the upper part of the current row
+    Index sizel = 0; // number of nonzero elements in the lower part of the current row
+    ju(ii)    = convert_index<StorageIndex>(ii);
+    u(ii)     = 0;
+    jr(ii)    = convert_index<StorageIndex>(ii);
+    RealScalar rownorm = 0;
+
+    typename FactorType::InnerIterator j_it(mat, ii); // Iterate through the current row ii
+    for (; j_it; ++j_it)
+    {
+      Index k = j_it.index();
+      if (k < ii)
+      {
+        // copy the lower part
+        ju(sizel) = convert_index<StorageIndex>(k);
+        u(sizel) = j_it.value();
+        jr(k) = convert_index<StorageIndex>(sizel);
+        ++sizel;
+      }
+      else if (k == ii)
+      {
+        u(ii) = j_it.value();
+      }
+      else
+      {
+        // copy the upper part
+        Index jpos = ii + sizeu;
+        ju(jpos) = convert_index<StorageIndex>(k);
+        u(jpos) = j_it.value();
+        jr(k) = convert_index<StorageIndex>(jpos);
+        ++sizeu;
+      }
+      rownorm += numext::abs2(j_it.value());
+    }
+
+    // 2 - detect possible zero row
+    if(rownorm==0)
+    {
+      m_info = NumericalIssue;
+      return;
+    }
+    // Take the 2-norm of the current row as a relative tolerance
+    rownorm = sqrt(rownorm);
+
+    // 3 - eliminate the previous nonzero rows
+    Index jj = 0;
+    Index len = 0;
+    while (jj < sizel)
+    {
+      // In order to eliminate in the correct order,
+      // we must select first the smallest column index among  ju(jj:sizel)
+      Index k;
+      Index minrow = ju.segment(jj,sizel-jj).minCoeff(&k); // k is relative to the segment
+      k += jj;
+      if (minrow != ju(jj))
+      {
+        // swap the two locations
+        Index j = ju(jj);
+        swap(ju(jj), ju(k));
+        jr(minrow) = convert_index<StorageIndex>(jj);
+        jr(j) = convert_index<StorageIndex>(k);
+        swap(u(jj), u(k));
+      }
+      // Reset this location
+      jr(minrow) = -1;
+
+      // Start elimination
+      typename FactorType::InnerIterator ki_it(m_lu, minrow);
+      while (ki_it && ki_it.index() < minrow) ++ki_it;
+      eigen_internal_assert(ki_it && ki_it.col()==minrow);
+      Scalar fact = u(jj) / ki_it.value();
+
+      // drop too small elements
+      if(abs(fact) <= m_droptol)
+      {
+        jj++;
+        continue;
+      }
+
+      // linear combination of the current row ii and the row minrow
+      ++ki_it;
+      for (; ki_it; ++ki_it)
+      {
+        Scalar prod = fact * ki_it.value();
+        Index j     = ki_it.index();
+        Index jpos  = jr(j);
+        if (jpos == -1) // fill-in element
+        {
+          Index newpos;
+          if (j >= ii) // dealing with the upper part
+          {
+            newpos = ii + sizeu;
+            sizeu++;
+            eigen_internal_assert(sizeu<=n);
+          }
+          else // dealing with the lower part
+          {
+            newpos = sizel;
+            sizel++;
+            eigen_internal_assert(sizel<=ii);
+          }
+          ju(newpos) = convert_index<StorageIndex>(j);
+          u(newpos) = -prod;
+          jr(j) = convert_index<StorageIndex>(newpos);
+        }
+        else
+          u(jpos) -= prod;
+      }
+      // store the pivot element
+      u(len)  = fact;
+      ju(len) = convert_index<StorageIndex>(minrow);
+      ++len;
+
+      jj++;
+    } // end of the elimination on the row ii
+
+    // reset the upper part of the pointer jr to zero
+    for(Index k = 0; k <sizeu; k++) jr(ju(ii+k)) = -1;
+
+    // 4 - partially sort and insert the elements in the m_lu matrix
+
+    // sort the L-part of the row
+    sizel = len;
+    len = (std::min)(sizel, nnzL);
+    typename Vector::SegmentReturnType ul(u.segment(0, sizel));
+    typename VectorI::SegmentReturnType jul(ju.segment(0, sizel));
+    internal::QuickSplit(ul, jul, len);
+
+    // store the largest m_fill elements of the L part
+    m_lu.startVec(ii);
+    for(Index k = 0; k < len; k++)
+      m_lu.insertBackByOuterInnerUnordered(ii,ju(k)) = u(k);
+
+    // store the diagonal element
+    // apply a shifting rule to avoid zero pivots (we are doing an incomplete factorization)
+    if (u(ii) == Scalar(0))
+      u(ii) = sqrt(m_droptol) * rownorm;
+    m_lu.insertBackByOuterInnerUnordered(ii, ii) = u(ii);
+
+    // sort the U-part of the row
+    // apply the dropping rule first
+    len = 0;
+    for(Index k = 1; k < sizeu; k++)
+    {
+      if(abs(u(ii+k)) > m_droptol * rownorm )
+      {
+        ++len;
+        u(ii + len)  = u(ii + k);
+        ju(ii + len) = ju(ii + k);
+      }
+    }
+    sizeu = len + 1; // +1 to take into account the diagonal element
+    len = (std::min)(sizeu, nnzU);
+    typename Vector::SegmentReturnType uu(u.segment(ii+1, sizeu-1));
+    typename VectorI::SegmentReturnType juu(ju.segment(ii+1, sizeu-1));
+    internal::QuickSplit(uu, juu, len);
+
+    // store the largest elements of the U part
+    for(Index k = ii + 1; k < ii + len; k++)
+      m_lu.insertBackByOuterInnerUnordered(ii,ju(k)) = u(k);
+  }
+  m_lu.finalize();
+  m_lu.makeCompressed();
+
+  m_factorizationIsOk = true;
+  m_info = Success;
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_INCOMPLETE_LUT_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h
new file mode 100644
index 0000000..28a0c51
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h
@@ -0,0 +1,444 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2011-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_ITERATIVE_SOLVER_BASE_H
+#define EIGEN_ITERATIVE_SOLVER_BASE_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename MatrixType>
+struct is_ref_compatible_impl
+{
+private:
+  template <typename T0>
+  struct any_conversion
+  {
+    template <typename T> any_conversion(const volatile T&);
+    template <typename T> any_conversion(T&);
+  };
+  struct yes {int a[1];};
+  struct no  {int a[2];};
+
+  template<typename T>
+  static yes test(const Ref<const T>&, int);
+  template<typename T>
+  static no  test(any_conversion<T>, ...);
+
+public:
+  static MatrixType ms_from;
+  enum { value = sizeof(test<MatrixType>(ms_from, 0))==sizeof(yes) };
+};
+
+template<typename MatrixType>
+struct is_ref_compatible
+{
+  enum { value = is_ref_compatible_impl<typename remove_all<MatrixType>::type>::value };
+};
+
+template<typename MatrixType, bool MatrixFree = !internal::is_ref_compatible<MatrixType>::value>
+class generic_matrix_wrapper;
+
+// We have an explicit matrix at hand, compatible with Ref<>
+template<typename MatrixType>
+class generic_matrix_wrapper<MatrixType,false>
+{
+public:
+  typedef Ref<const MatrixType> ActualMatrixType;
+  template<int UpLo> struct ConstSelfAdjointViewReturnType {
+    typedef typename ActualMatrixType::template ConstSelfAdjointViewReturnType<UpLo>::Type Type;
+  };
+
+  enum {
+    MatrixFree = false
+  };
+
+  generic_matrix_wrapper()
+    : m_dummy(0,0), m_matrix(m_dummy)
+  {}
+
+  template<typename InputType>
+  generic_matrix_wrapper(const InputType &mat)
+    : m_matrix(mat)
+  {}
+
+  const ActualMatrixType& matrix() const
+  {
+    return m_matrix;
+  }
+
+  template<typename MatrixDerived>
+  void grab(const EigenBase<MatrixDerived> &mat)
+  {
+    m_matrix.~Ref<const MatrixType>();
+    ::new (&m_matrix) Ref<const MatrixType>(mat.derived());
+  }
+
+  void grab(const Ref<const MatrixType> &mat)
+  {
+    if(&(mat.derived()) != &m_matrix)
+    {
+      m_matrix.~Ref<const MatrixType>();
+      ::new (&m_matrix) Ref<const MatrixType>(mat);
+    }
+  }
+
+protected:
+  MatrixType m_dummy; // used to default initialize the Ref<> object
+  ActualMatrixType m_matrix;
+};
+
+// MatrixType is not compatible with Ref<> -> matrix-free wrapper
+template<typename MatrixType>
+class generic_matrix_wrapper<MatrixType,true>
+{
+public:
+  typedef MatrixType ActualMatrixType;
+  template<int UpLo> struct ConstSelfAdjointViewReturnType
+  {
+    typedef ActualMatrixType Type;
+  };
+
+  enum {
+    MatrixFree = true
+  };
+
+  generic_matrix_wrapper()
+    : mp_matrix(0)
+  {}
+
+  generic_matrix_wrapper(const MatrixType &mat)
+    : mp_matrix(&mat)
+  {}
+
+  const ActualMatrixType& matrix() const
+  {
+    return *mp_matrix;
+  }
+
+  void grab(const MatrixType &mat)
+  {
+    mp_matrix = &mat;
+  }
+
+protected:
+  const ActualMatrixType *mp_matrix;
+};
+
+}
+
+/** \ingroup IterativeLinearSolvers_Module
+  * \brief Base class for linear iterative solvers
+  *
+  * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
+  */
+template< typename Derived>
+class IterativeSolverBase : public SparseSolverBase<Derived>
+{
+protected:
+  typedef SparseSolverBase<Derived> Base;
+  using Base::m_isInitialized;
+
+public:
+  typedef typename internal::traits<Derived>::MatrixType MatrixType;
+  typedef typename internal::traits<Derived>::Preconditioner Preconditioner;
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  typedef typename MatrixType::RealScalar RealScalar;
+
+  enum {
+    ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+  };
+
+public:
+
+  using Base::derived;
+
+  /** Default constructor. */
+  IterativeSolverBase()
+  {
+    init();
+  }
+
+  /** Initialize the solver with matrix \a A for further \c Ax=b solving.
+    *
+    * This constructor is a shortcut for the default constructor followed
+    * by a call to compute().
+    *
+    * \warning this class stores a reference to the matrix A as well as some
+    * precomputed values that depend on it. Therefore, if \a A is changed
+    * this class becomes invalid. Call compute() to update it with the new
+    * matrix A, or modify a copy of A.
+    */
+  template<typename MatrixDerived>
+  explicit IterativeSolverBase(const EigenBase<MatrixDerived>& A)
+    : m_matrixWrapper(A.derived())
+  {
+    init();
+    compute(matrix());
+  }
+
+  ~IterativeSolverBase() {}
+
+  /** Initializes the iterative solver for the sparsity pattern of the matrix \a A for further solving \c Ax=b problems.
+    *
+    * Currently, this function mostly calls analyzePattern on the preconditioner. In the future
+    * we might, for instance, implement column reordering for faster matrix vector products.
+    */
+  template<typename MatrixDerived>
+  Derived& analyzePattern(const EigenBase<MatrixDerived>& A)
+  {
+    grab(A.derived());
+    m_preconditioner.analyzePattern(matrix());
+    m_isInitialized = true;
+    m_analysisIsOk = true;
+    m_info = m_preconditioner.info();
+    return derived();
+  }
+
+  /** Initializes the iterative solver with the numerical values of the matrix \a A for further solving \c Ax=b problems.
+    *
+    * Currently, this function mostly calls factorize on the preconditioner.
+    *
+    * \warning this class stores a reference to the matrix A as well as some
+    * precomputed values that depend on it. Therefore, if \a A is changed
+    * this class becomes invalid. Call compute() to update it with the new
+    * matrix A, or modify a copy of A.
+    */
+  template<typename MatrixDerived>
+  Derived& factorize(const EigenBase<MatrixDerived>& A)
+  {
+    eigen_assert(m_analysisIsOk && "You must first call analyzePattern()");
+    grab(A.derived());
+    m_preconditioner.factorize(matrix());
+    m_factorizationIsOk = true;
+    m_info = m_preconditioner.info();
+    return derived();
+  }
+
+  /** Initializes the iterative solver with the matrix \a A for further solving \c Ax=b problems.
+    *
+    * Currently, this function mostly initializes/computes the preconditioner. In the future
+    * we might, for instance, implement column reordering for faster matrix vector products.
+    *
+    * \warning this class stores a reference to the matrix A as well as some
+    * precomputed values that depend on it. Therefore, if \a A is changed
+    * this class becomes invalid. Call compute() to update it with the new
+    * matrix A, or modify a copy of A.
+    */
+  template<typename MatrixDerived>
+  Derived& compute(const EigenBase<MatrixDerived>& A)
+  {
+    grab(A.derived());
+    m_preconditioner.compute(matrix());
+    m_isInitialized = true;
+    m_analysisIsOk = true;
+    m_factorizationIsOk = true;
+    m_info = m_preconditioner.info();
+    return derived();
+  }
+
+  /** \internal */
+  EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return matrix().rows(); }
+
+  /** \internal */
+  EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return matrix().cols(); }
+
+  /** \returns the tolerance threshold used by the stopping criteria.
+    * \sa setTolerance()
+    */
+  RealScalar tolerance() const { return m_tolerance; }
+
+  /** Sets the tolerance threshold used by the stopping criteria.
+    *
+    * This value is used as an upper bound to the relative residual error: |Ax-b|/|b|.
+    * The default value is the machine precision given by NumTraits<Scalar>::epsilon()
+    */
+  Derived& setTolerance(const RealScalar& tolerance)
+  {
+    m_tolerance = tolerance;
+    return derived();
+  }
+
+  /** \returns a read-write reference to the preconditioner for custom configuration. */
+  Preconditioner& preconditioner() { return m_preconditioner; }
+
+  /** \returns a read-only reference to the preconditioner. */
+  const Preconditioner& preconditioner() const { return m_preconditioner; }
+
+  /** \returns the max number of iterations.
+    * It is either the value set by setMaxIterations or, by default,
+    * twice the number of columns of the matrix.
+    */
+  Index maxIterations() const
+  {
+    return (m_maxIterations<0) ? 2*matrix().cols() : m_maxIterations;
+  }
+
+  /** Sets the max number of iterations.
+    * Default is twice the number of columns of the matrix.
+    */
+  Derived& setMaxIterations(Index maxIters)
+  {
+    m_maxIterations = maxIters;
+    return derived();
+  }
+
+  /** \returns the number of iterations performed during the last solve */
+  Index iterations() const
+  {
+    eigen_assert(m_isInitialized && "ConjugateGradient is not initialized.");
+    return m_iterations;
+  }
+
+  /** \returns the tolerance error reached during the last solve.
+    * It is a close approximation of the true relative residual error |Ax-b|/|b|.
+    */
+  RealScalar error() const
+  {
+    eigen_assert(m_isInitialized && "ConjugateGradient is not initialized.");
+    return m_error;
+  }
+
+  /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A
+    * and \a x0 as an initial solution.
+    *
+    * \sa solve(), compute()
+    */
+  template<typename Rhs,typename Guess>
+  inline const SolveWithGuess<Derived, Rhs, Guess>
+  solveWithGuess(const MatrixBase<Rhs>& b, const Guess& x0) const
+  {
+    eigen_assert(m_isInitialized && "Solver is not initialized.");
+    eigen_assert(derived().rows()==b.rows() && "solve(): invalid number of rows of the right hand side matrix b");
+    return SolveWithGuess<Derived, Rhs, Guess>(derived(), b.derived(), x0);
+  }
+
+  /** \returns Success if the iterations converged, and NoConvergence otherwise. */
+  ComputationInfo info() const
+  {
+    eigen_assert(m_isInitialized && "IterativeSolverBase is not initialized.");
+    return m_info;
+  }
+
+  /** \internal */
+  template<typename Rhs, typename DestDerived>
+  void _solve_with_guess_impl(const Rhs& b, SparseMatrixBase<DestDerived> &aDest) const
+  {
+    eigen_assert(rows()==b.rows());
+
+    Index rhsCols = b.cols();
+    Index size = b.rows();
+    DestDerived& dest(aDest.derived());
+    typedef typename DestDerived::Scalar DestScalar;
+    Eigen::Matrix<DestScalar,Dynamic,1> tb(size);
+    Eigen::Matrix<DestScalar,Dynamic,1> tx(cols());
+    // We do not directly fill dest because sparse expressions have to be free of aliasing issue.
+    // For non square least-square problems, b and dest might not have the same size whereas they might alias each-other.
+    typename DestDerived::PlainObject tmp(cols(),rhsCols);
+    ComputationInfo global_info = Success;
+    for(Index k=0; k<rhsCols; ++k)
+    {
+      tb = b.col(k);
+      tx = dest.col(k);
+      derived()._solve_vector_with_guess_impl(tb,tx);
+      tmp.col(k) = tx.sparseView(0);
+
+      // The call to _solve_vector_with_guess_impl updates m_info, so if it failed for a previous column
+      // we need to restore it to the worst value.
+      if(m_info==NumericalIssue)
+        global_info = NumericalIssue;
+      else if(m_info==NoConvergence)
+        global_info = NoConvergence;
+    }
+    m_info = global_info;
+    dest.swap(tmp);
+  }
+
+  template<typename Rhs, typename DestDerived>
+  typename internal::enable_if<Rhs::ColsAtCompileTime!=1 && DestDerived::ColsAtCompileTime!=1>::type
+  _solve_with_guess_impl(const Rhs& b, MatrixBase<DestDerived> &aDest) const
+  {
+    eigen_assert(rows()==b.rows());
+
+    Index rhsCols = b.cols();
+    DestDerived& dest(aDest.derived());
+    ComputationInfo global_info = Success;
+    for(Index k=0; k<rhsCols; ++k)
+    {
+      typename DestDerived::ColXpr xk(dest,k);
+      typename Rhs::ConstColXpr bk(b,k);
+      derived()._solve_vector_with_guess_impl(bk,xk);
+
+      // The call to _solve_vector_with_guess updates m_info, so if it failed for a previous column
+      // we need to restore it to the worst value.
+      if(m_info==NumericalIssue)
+        global_info = NumericalIssue;
+      else if(m_info==NoConvergence)
+        global_info = NoConvergence;
+    }
+    m_info = global_info;
+  }
+
+  template<typename Rhs, typename DestDerived>
+  typename internal::enable_if<Rhs::ColsAtCompileTime==1 || DestDerived::ColsAtCompileTime==1>::type
+  _solve_with_guess_impl(const Rhs& b, MatrixBase<DestDerived> &dest) const
+  {
+    derived()._solve_vector_with_guess_impl(b,dest.derived());
+  }
+
+  /** \internal default initial guess = 0 */
+  template<typename Rhs,typename Dest>
+  void _solve_impl(const Rhs& b, Dest& x) const
+  {
+    x.setZero();
+    derived()._solve_with_guess_impl(b,x);
+  }
+
+protected:
+  void init()
+  {
+    m_isInitialized = false;
+    m_analysisIsOk = false;
+    m_factorizationIsOk = false;
+    m_maxIterations = -1;
+    m_tolerance = NumTraits<Scalar>::epsilon();
+  }
+
+  typedef internal::generic_matrix_wrapper<MatrixType> MatrixWrapper;
+  typedef typename MatrixWrapper::ActualMatrixType ActualMatrixType;
+
+  const ActualMatrixType& matrix() const
+  {
+    return m_matrixWrapper.matrix();
+  }
+
+  template<typename InputType>
+  void grab(const InputType &A)
+  {
+    m_matrixWrapper.grab(A);
+  }
+
+  MatrixWrapper m_matrixWrapper;
+  Preconditioner m_preconditioner;
+
+  Index m_maxIterations;
+  RealScalar m_tolerance;
+
+  mutable RealScalar m_error;
+  mutable Index m_iterations;
+  mutable ComputationInfo m_info;
+  mutable bool m_analysisIsOk, m_factorizationIsOk;
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_ITERATIVE_SOLVER_BASE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h
new file mode 100644
index 0000000..203fd0e
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h
@@ -0,0 +1,198 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_LEAST_SQUARE_CONJUGATE_GRADIENT_H
+#define EIGEN_LEAST_SQUARE_CONJUGATE_GRADIENT_H
+
+namespace Eigen { 
+
+namespace internal {
+
+/** \internal Low-level conjugate gradient algorithm for least-square problems
+  * \param mat The matrix A
+  * \param rhs The right hand side vector b
+  * \param x On input and initial solution, on output the computed solution.
+  * \param precond A preconditioner being able to efficiently solve for an
+  *                approximation of A'Ax=b (regardless of b)
+  * \param iters On input the max number of iteration, on output the number of performed iterations.
+  * \param tol_error On input the tolerance error, on output an estimation of the relative error.
+  */
+template<typename MatrixType, typename Rhs, typename Dest, typename Preconditioner>
+EIGEN_DONT_INLINE
+void least_square_conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& x,
+                                     const Preconditioner& precond, Index& iters,
+                                     typename Dest::RealScalar& tol_error)
+{
+  using std::sqrt;
+  using std::abs;
+  typedef typename Dest::RealScalar RealScalar;
+  typedef typename Dest::Scalar Scalar;
+  typedef Matrix<Scalar,Dynamic,1> VectorType;
+  
+  RealScalar tol = tol_error;
+  Index maxIters = iters;
+  
+  Index m = mat.rows(), n = mat.cols();
+
+  VectorType residual        = rhs - mat * x;
+  VectorType normal_residual = mat.adjoint() * residual;
+
+  RealScalar rhsNorm2 = (mat.adjoint()*rhs).squaredNorm();
+  if(rhsNorm2 == 0) 
+  {
+    x.setZero();
+    iters = 0;
+    tol_error = 0;
+    return;
+  }
+  RealScalar threshold = tol*tol*rhsNorm2;
+  RealScalar residualNorm2 = normal_residual.squaredNorm();
+  if (residualNorm2 < threshold)
+  {
+    iters = 0;
+    tol_error = sqrt(residualNorm2 / rhsNorm2);
+    return;
+  }
+  
+  VectorType p(n);
+  p = precond.solve(normal_residual);                         // initial search direction
+
+  VectorType z(n), tmp(m);
+  RealScalar absNew = numext::real(normal_residual.dot(p));  // the square of the absolute value of r scaled by invM
+  Index i = 0;
+  while(i < maxIters)
+  {
+    tmp.noalias() = mat * p;
+
+    Scalar alpha = absNew / tmp.squaredNorm();      // the amount we travel on dir
+    x += alpha * p;                                 // update solution
+    residual -= alpha * tmp;                        // update residual
+    normal_residual = mat.adjoint() * residual;     // update residual of the normal equation
+    
+    residualNorm2 = normal_residual.squaredNorm();
+    if(residualNorm2 < threshold)
+      break;
+    
+    z = precond.solve(normal_residual);             // approximately solve for "A'A z = normal_residual"
+
+    RealScalar absOld = absNew;
+    absNew = numext::real(normal_residual.dot(z));  // update the absolute value of r
+    RealScalar beta = absNew / absOld;              // calculate the Gram-Schmidt value used to create the new search direction
+    p = z + beta * p;                               // update search direction
+    i++;
+  }
+  tol_error = sqrt(residualNorm2 / rhsNorm2);
+  iters = i;
+}
+
+}
+
+template< typename _MatrixType,
+          typename _Preconditioner = LeastSquareDiagonalPreconditioner<typename _MatrixType::Scalar> >
+class LeastSquaresConjugateGradient;
+
+namespace internal {
+
+template< typename _MatrixType, typename _Preconditioner>
+struct traits<LeastSquaresConjugateGradient<_MatrixType,_Preconditioner> >
+{
+  typedef _MatrixType MatrixType;
+  typedef _Preconditioner Preconditioner;
+};
+
+}
+
+/** \ingroup IterativeLinearSolvers_Module
+  * \brief A conjugate gradient solver for sparse (or dense) least-square problems
+  *
+  * This class allows to solve for A x = b linear problems using an iterative conjugate gradient algorithm.
+  * The matrix A can be non symmetric and rectangular, but the matrix A' A should be positive-definite to guaranty stability.
+  * Otherwise, the SparseLU or SparseQR classes might be preferable.
+  * The matrix A and the vectors x and b can be either dense or sparse.
+  *
+  * \tparam _MatrixType the type of the matrix A, can be a dense or a sparse matrix.
+  * \tparam _Preconditioner the type of the preconditioner. Default is LeastSquareDiagonalPreconditioner
+  *
+  * \implsparsesolverconcept
+  * 
+  * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations()
+  * and setTolerance() methods. The defaults are the size of the problem for the maximal number of iterations
+  * and NumTraits<Scalar>::epsilon() for the tolerance.
+  * 
+  * This class can be used as the direct solver classes. Here is a typical usage example:
+    \code
+    int m=1000000, n = 10000;
+    VectorXd x(n), b(m);
+    SparseMatrix<double> A(m,n);
+    // fill A and b
+    LeastSquaresConjugateGradient<SparseMatrix<double> > lscg;
+    lscg.compute(A);
+    x = lscg.solve(b);
+    std::cout << "#iterations:     " << lscg.iterations() << std::endl;
+    std::cout << "estimated error: " << lscg.error()      << std::endl;
+    // update b, and solve again
+    x = lscg.solve(b);
+    \endcode
+  * 
+  * By default the iterations start with x=0 as an initial guess of the solution.
+  * One can control the start using the solveWithGuess() method.
+  * 
+  * \sa class ConjugateGradient, SparseLU, SparseQR
+  */
+template< typename _MatrixType, typename _Preconditioner>
+class LeastSquaresConjugateGradient : public IterativeSolverBase<LeastSquaresConjugateGradient<_MatrixType,_Preconditioner> >
+{
+  typedef IterativeSolverBase<LeastSquaresConjugateGradient> Base;
+  using Base::matrix;
+  using Base::m_error;
+  using Base::m_iterations;
+  using Base::m_info;
+  using Base::m_isInitialized;
+public:
+  typedef _MatrixType MatrixType;
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::RealScalar RealScalar;
+  typedef _Preconditioner Preconditioner;
+
+public:
+
+  /** Default constructor. */
+  LeastSquaresConjugateGradient() : Base() {}
+
+  /** Initialize the solver with matrix \a A for further \c Ax=b solving.
+    * 
+    * This constructor is a shortcut for the default constructor followed
+    * by a call to compute().
+    * 
+    * \warning this class stores a reference to the matrix A as well as some
+    * precomputed values that depend on it. Therefore, if \a A is changed
+    * this class becomes invalid. Call compute() to update it with the new
+    * matrix A, or modify a copy of A.
+    */
+  template<typename MatrixDerived>
+  explicit LeastSquaresConjugateGradient(const EigenBase<MatrixDerived>& A) : Base(A.derived()) {}
+
+  ~LeastSquaresConjugateGradient() {}
+
+  /** \internal */
+  template<typename Rhs,typename Dest>
+  void _solve_vector_with_guess_impl(const Rhs& b, Dest& x) const
+  {
+    m_iterations = Base::maxIterations();
+    m_error = Base::m_tolerance;
+
+    internal::least_square_conjugate_gradient(matrix(), b, x, Base::m_preconditioner, m_iterations, m_error);
+    m_info = m_error <= Base::m_tolerance ? Success : NoConvergence;
+  }
+
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_LEAST_SQUARE_CONJUGATE_GRADIENT_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h
new file mode 100644
index 0000000..7b89657
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h
@@ -0,0 +1,117 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SOLVEWITHGUESS_H
+#define EIGEN_SOLVEWITHGUESS_H
+
+namespace Eigen {
+
+template<typename Decomposition, typename RhsType, typename GuessType> class SolveWithGuess;
+
+/** \class SolveWithGuess
+  * \ingroup IterativeLinearSolvers_Module
+  *
+  * \brief Pseudo expression representing a solving operation
+  *
+  * \tparam Decomposition the type of the matrix or decomposion object
+  * \tparam Rhstype the type of the right-hand side
+  *
+  * This class represents an expression of A.solve(B)
+  * and most of the time this is the only way it is used.
+  *
+  */
+namespace internal {
+
+
+template<typename Decomposition, typename RhsType, typename GuessType>
+struct traits<SolveWithGuess<Decomposition, RhsType, GuessType> >
+  : traits<Solve<Decomposition,RhsType> >
+{};
+
+}
+
+
+template<typename Decomposition, typename RhsType, typename GuessType>
+class SolveWithGuess : public internal::generic_xpr_base<SolveWithGuess<Decomposition,RhsType,GuessType>, MatrixXpr, typename internal::traits<RhsType>::StorageKind>::type
+{
+public:
+  typedef typename internal::traits<SolveWithGuess>::Scalar Scalar;
+  typedef typename internal::traits<SolveWithGuess>::PlainObject PlainObject;
+  typedef typename internal::generic_xpr_base<SolveWithGuess<Decomposition,RhsType,GuessType>, MatrixXpr, typename internal::traits<RhsType>::StorageKind>::type Base;
+  typedef typename internal::ref_selector<SolveWithGuess>::type Nested;
+
+  SolveWithGuess(const Decomposition &dec, const RhsType &rhs, const GuessType &guess)
+    : m_dec(dec), m_rhs(rhs), m_guess(guess)
+  {}
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+  Index rows() const EIGEN_NOEXCEPT { return m_dec.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+  Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); }
+
+  EIGEN_DEVICE_FUNC const Decomposition& dec()   const { return m_dec; }
+  EIGEN_DEVICE_FUNC const RhsType&       rhs()   const { return m_rhs; }
+  EIGEN_DEVICE_FUNC const GuessType&     guess() const { return m_guess; }
+
+protected:
+  const Decomposition &m_dec;
+  const RhsType       &m_rhs;
+  const GuessType     &m_guess;
+
+private:
+  Scalar coeff(Index row, Index col) const;
+  Scalar coeff(Index i) const;
+};
+
+namespace internal {
+
+// Evaluator of SolveWithGuess -> eval into a temporary
+template<typename Decomposition, typename RhsType, typename GuessType>
+struct evaluator<SolveWithGuess<Decomposition,RhsType, GuessType> >
+  : public evaluator<typename SolveWithGuess<Decomposition,RhsType,GuessType>::PlainObject>
+{
+  typedef SolveWithGuess<Decomposition,RhsType,GuessType> SolveType;
+  typedef typename SolveType::PlainObject PlainObject;
+  typedef evaluator<PlainObject> Base;
+
+  evaluator(const SolveType& solve)
+    : m_result(solve.rows(), solve.cols())
+  {
+    ::new (static_cast<Base*>(this)) Base(m_result);
+    m_result = solve.guess();
+    solve.dec()._solve_with_guess_impl(solve.rhs(), m_result);
+  }
+
+protected:
+  PlainObject m_result;
+};
+
+// Specialization for "dst = dec.solveWithGuess(rhs)"
+// NOTE we need to specialize it for Dense2Dense to avoid ambiguous specialization error and a Sparse2Sparse specialization must exist somewhere
+template<typename DstXprType, typename DecType, typename RhsType, typename GuessType, typename Scalar>
+struct Assignment<DstXprType, SolveWithGuess<DecType,RhsType,GuessType>, internal::assign_op<Scalar,Scalar>, Dense2Dense>
+{
+  typedef SolveWithGuess<DecType,RhsType,GuessType> SrcXprType;
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &)
+  {
+    Index dstRows = src.rows();
+    Index dstCols = src.cols();
+    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
+      dst.resize(dstRows, dstCols);
+
+    dst = src.guess();
+    src.dec()._solve_with_guess_impl(src.rhs(), dst/*, src.guess()*/);
+  }
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SOLVEWITHGUESS_H
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Jacobi/Jacobi.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Jacobi/Jacobi.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/Jacobi/Jacobi.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Jacobi/Jacobi.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/LU/Determinant.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/Determinant.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/LU/Determinant.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/Determinant.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/LU/FullPivLU.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/FullPivLU.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/LU/FullPivLU.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/FullPivLU.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/LU/InverseImpl.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/InverseImpl.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/LU/InverseImpl.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/InverseImpl.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/LU/PartialPivLU.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/PartialPivLU.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/LU/PartialPivLU.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/PartialPivLU.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/LU/arch/InverseSize4.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/arch/InverseSize4.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/LU/arch/InverseSize4.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/LU/arch/InverseSize4.h
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Amd.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Amd.h
new file mode 100644
index 0000000..7ca3f33
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Amd.h
@@ -0,0 +1,435 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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/.
+
+/*
+NOTE: this routine has been adapted from the CSparse library:
+
+Copyright (c) 2006, Timothy A. Davis.
+http://www.suitesparse.com
+
+The author of CSparse, Timothy A. Davis., has executed a license with Google LLC
+to permit distribution of this code and derivative works as part of Eigen under
+the Mozilla Public License v. 2.0, as stated at the top of this file.
+*/
+
+#ifndef EIGEN_SPARSE_AMD_H
+#define EIGEN_SPARSE_AMD_H
+
+namespace Eigen { 
+
+namespace internal {
+  
+template<typename T> inline T amd_flip(const T& i) { return -i-2; }
+template<typename T> inline T amd_unflip(const T& i) { return i<0 ? amd_flip(i) : i; }
+template<typename T0, typename T1> inline bool amd_marked(const T0* w, const T1& j) { return w[j]<0; }
+template<typename T0, typename T1> inline void amd_mark(const T0* w, const T1& j) { return w[j] = amd_flip(w[j]); }
+
+/* clear w */
+template<typename StorageIndex>
+static StorageIndex cs_wclear (StorageIndex mark, StorageIndex lemax, StorageIndex *w, StorageIndex n)
+{
+  StorageIndex k;
+  if(mark < 2 || (mark + lemax < 0))
+  {
+    for(k = 0; k < n; k++)
+      if(w[k] != 0)
+        w[k] = 1;
+    mark = 2;
+  }
+  return (mark);     /* at this point, w[0..n-1] < mark holds */
+}
+
+/* depth-first search and postorder of a tree rooted at node j */
+template<typename StorageIndex>
+StorageIndex cs_tdfs(StorageIndex j, StorageIndex k, StorageIndex *head, const StorageIndex *next, StorageIndex *post, StorageIndex *stack)
+{
+  StorageIndex i, p, top = 0;
+  if(!head || !next || !post || !stack) return (-1);    /* check inputs */
+  stack[0] = j;                 /* place j on the stack */
+  while (top >= 0)                /* while (stack is not empty) */
+  {
+    p = stack[top];           /* p = top of stack */
+    i = head[p];              /* i = youngest child of p */
+    if(i == -1)
+    {
+      top--;                 /* p has no unordered children left */
+      post[k++] = p;        /* node p is the kth postordered node */
+    }
+    else
+    {
+      head[p] = next[i];   /* remove i from children of p */
+      stack[++top] = i;     /* start dfs on child node i */
+    }
+  }
+  return k;
+}
+
+
+/** \internal
+  * \ingroup OrderingMethods_Module 
+  * Approximate minimum degree ordering algorithm.
+  *
+  * \param[in] C the input selfadjoint matrix stored in compressed column major format.
+  * \param[out] perm the permutation P reducing the fill-in of the input matrix \a C
+  *
+  * Note that the input matrix \a C must be complete, that is both the upper and lower parts have to be stored, as well as the diagonal entries.
+  * On exit the values of C are destroyed */
+template<typename Scalar, typename StorageIndex>
+void minimum_degree_ordering(SparseMatrix<Scalar,ColMajor,StorageIndex>& C, PermutationMatrix<Dynamic,Dynamic,StorageIndex>& perm)
+{
+  using std::sqrt;
+  
+  StorageIndex d, dk, dext, lemax = 0, e, elenk, eln, i, j, k, k1,
+                k2, k3, jlast, ln, dense, nzmax, mindeg = 0, nvi, nvj, nvk, mark, wnvi,
+                ok, nel = 0, p, p1, p2, p3, p4, pj, pk, pk1, pk2, pn, q, t, h;
+  
+  StorageIndex n = StorageIndex(C.cols());
+  dense = std::max<StorageIndex> (16, StorageIndex(10 * sqrt(double(n))));   /* find dense threshold */
+  dense = (std::min)(n-2, dense);
+  
+  StorageIndex cnz = StorageIndex(C.nonZeros());
+  perm.resize(n+1);
+  t = cnz + cnz/5 + 2*n;                 /* add elbow room to C */
+  C.resizeNonZeros(t);
+  
+  // get workspace
+  ei_declare_aligned_stack_constructed_variable(StorageIndex,W,8*(n+1),0);
+  StorageIndex* len     = W;
+  StorageIndex* nv      = W +   (n+1);
+  StorageIndex* next    = W + 2*(n+1);
+  StorageIndex* head    = W + 3*(n+1);
+  StorageIndex* elen    = W + 4*(n+1);
+  StorageIndex* degree  = W + 5*(n+1);
+  StorageIndex* w       = W + 6*(n+1);
+  StorageIndex* hhead   = W + 7*(n+1);
+  StorageIndex* last    = perm.indices().data();                              /* use P as workspace for last */
+  
+  /* --- Initialize quotient graph ---------------------------------------- */
+  StorageIndex* Cp = C.outerIndexPtr();
+  StorageIndex* Ci = C.innerIndexPtr();
+  for(k = 0; k < n; k++)
+    len[k] = Cp[k+1] - Cp[k];
+  len[n] = 0;
+  nzmax = t;
+  
+  for(i = 0; i <= n; i++)
+  {
+    head[i]   = -1;                     // degree list i is empty
+    last[i]   = -1;
+    next[i]   = -1;
+    hhead[i]  = -1;                     // hash list i is empty 
+    nv[i]     = 1;                      // node i is just one node
+    w[i]      = 1;                      // node i is alive
+    elen[i]   = 0;                      // Ek of node i is empty
+    degree[i] = len[i];                 // degree of node i
+  }
+  mark = internal::cs_wclear<StorageIndex>(0, 0, w, n);         /* clear w */
+  
+  /* --- Initialize degree lists ------------------------------------------ */
+  for(i = 0; i < n; i++)
+  {
+    bool has_diag = false;
+    for(p = Cp[i]; p<Cp[i+1]; ++p)
+      if(Ci[p]==i)
+      {
+        has_diag = true;
+        break;
+      }
+   
+    d = degree[i];
+    if(d == 1 && has_diag)           /* node i is empty */
+    {
+      elen[i] = -2;                 /* element i is dead */
+      nel++;
+      Cp[i] = -1;                   /* i is a root of assembly tree */
+      w[i] = 0;
+    }
+    else if(d > dense || !has_diag)  /* node i is dense or has no structural diagonal element */
+    {
+      nv[i] = 0;                    /* absorb i into element n */
+      elen[i] = -1;                 /* node i is dead */
+      nel++;
+      Cp[i] = amd_flip (n);
+      nv[n]++;
+    }
+    else
+    {
+      if(head[d] != -1) last[head[d]] = i;
+      next[i] = head[d];           /* put node i in degree list d */
+      head[d] = i;
+    }
+  }
+  
+  elen[n] = -2;                         /* n is a dead element */
+  Cp[n] = -1;                           /* n is a root of assembly tree */
+  w[n] = 0;                             /* n is a dead element */
+  
+  while (nel < n)                         /* while (selecting pivots) do */
+  {
+    /* --- Select node of minimum approximate degree -------------------- */
+    for(k = -1; mindeg < n && (k = head[mindeg]) == -1; mindeg++) {}
+    if(next[k] != -1) last[next[k]] = -1;
+    head[mindeg] = next[k];          /* remove k from degree list */
+    elenk = elen[k];                  /* elenk = |Ek| */
+    nvk = nv[k];                      /* # of nodes k represents */
+    nel += nvk;                        /* nv[k] nodes of A eliminated */
+    
+    /* --- Garbage collection ------------------------------------------- */
+    if(elenk > 0 && cnz + mindeg >= nzmax)
+    {
+      for(j = 0; j < n; j++)
+      {
+        if((p = Cp[j]) >= 0)      /* j is a live node or element */
+        {
+          Cp[j] = Ci[p];          /* save first entry of object */
+          Ci[p] = amd_flip (j);    /* first entry is now amd_flip(j) */
+        }
+      }
+      for(q = 0, p = 0; p < cnz; ) /* scan all of memory */
+      {
+        if((j = amd_flip (Ci[p++])) >= 0)  /* found object j */
+        {
+          Ci[q] = Cp[j];       /* restore first entry of object */
+          Cp[j] = q++;          /* new pointer to object j */
+          for(k3 = 0; k3 < len[j]-1; k3++) Ci[q++] = Ci[p++];
+        }
+      }
+      cnz = q;                       /* Ci[cnz...nzmax-1] now free */
+    }
+    
+    /* --- Construct new element ---------------------------------------- */
+    dk = 0;
+    nv[k] = -nvk;                     /* flag k as in Lk */
+    p = Cp[k];
+    pk1 = (elenk == 0) ? p : cnz;      /* do in place if elen[k] == 0 */
+    pk2 = pk1;
+    for(k1 = 1; k1 <= elenk + 1; k1++)
+    {
+      if(k1 > elenk)
+      {
+        e = k;                     /* search the nodes in k */
+        pj = p;                    /* list of nodes starts at Ci[pj]*/
+        ln = len[k] - elenk;      /* length of list of nodes in k */
+      }
+      else
+      {
+        e = Ci[p++];              /* search the nodes in e */
+        pj = Cp[e];
+        ln = len[e];              /* length of list of nodes in e */
+      }
+      for(k2 = 1; k2 <= ln; k2++)
+      {
+        i = Ci[pj++];
+        if((nvi = nv[i]) <= 0) continue; /* node i dead, or seen */
+        dk += nvi;                 /* degree[Lk] += size of node i */
+        nv[i] = -nvi;             /* negate nv[i] to denote i in Lk*/
+        Ci[pk2++] = i;            /* place i in Lk */
+        if(next[i] != -1) last[next[i]] = last[i];
+        if(last[i] != -1)         /* remove i from degree list */
+        {
+          next[last[i]] = next[i];
+        }
+        else
+        {
+          head[degree[i]] = next[i];
+        }
+      }
+      if(e != k)
+      {
+        Cp[e] = amd_flip (k);      /* absorb e into k */
+        w[e] = 0;                 /* e is now a dead element */
+      }
+    }
+    if(elenk != 0) cnz = pk2;         /* Ci[cnz...nzmax] is free */
+    degree[k] = dk;                   /* external degree of k - |Lk\i| */
+    Cp[k] = pk1;                      /* element k is in Ci[pk1..pk2-1] */
+    len[k] = pk2 - pk1;
+    elen[k] = -2;                     /* k is now an element */
+    
+    /* --- Find set differences ----------------------------------------- */
+    mark = internal::cs_wclear<StorageIndex>(mark, lemax, w, n);  /* clear w if necessary */
+    for(pk = pk1; pk < pk2; pk++)    /* scan 1: find |Le\Lk| */
+    {
+      i = Ci[pk];
+      if((eln = elen[i]) <= 0) continue;/* skip if elen[i] empty */
+      nvi = -nv[i];                      /* nv[i] was negated */
+      wnvi = mark - nvi;
+      for(p = Cp[i]; p <= Cp[i] + eln - 1; p++)  /* scan Ei */
+      {
+        e = Ci[p];
+        if(w[e] >= mark)
+        {
+          w[e] -= nvi;          /* decrement |Le\Lk| */
+        }
+        else if(w[e] != 0)        /* ensure e is a live element */
+        {
+          w[e] = degree[e] + wnvi; /* 1st time e seen in scan 1 */
+        }
+      }
+    }
+    
+    /* --- Degree update ------------------------------------------------ */
+    for(pk = pk1; pk < pk2; pk++)    /* scan2: degree update */
+    {
+      i = Ci[pk];                   /* consider node i in Lk */
+      p1 = Cp[i];
+      p2 = p1 + elen[i] - 1;
+      pn = p1;
+      for(h = 0, d = 0, p = p1; p <= p2; p++)    /* scan Ei */
+      {
+        e = Ci[p];
+        if(w[e] != 0)             /* e is an unabsorbed element */
+        {
+          dext = w[e] - mark;   /* dext = |Le\Lk| */
+          if(dext > 0)
+          {
+            d += dext;         /* sum up the set differences */
+            Ci[pn++] = e;     /* keep e in Ei */
+            h += e;            /* compute the hash of node i */
+          }
+          else
+          {
+            Cp[e] = amd_flip (k);  /* aggressive absorb. e->k */
+            w[e] = 0;             /* e is a dead element */
+          }
+        }
+      }
+      elen[i] = pn - p1 + 1;        /* elen[i] = |Ei| */
+      p3 = pn;
+      p4 = p1 + len[i];
+      for(p = p2 + 1; p < p4; p++) /* prune edges in Ai */
+      {
+        j = Ci[p];
+        if((nvj = nv[j]) <= 0) continue; /* node j dead or in Lk */
+        d += nvj;                  /* degree(i) += |j| */
+        Ci[pn++] = j;             /* place j in node list of i */
+        h += j;                    /* compute hash for node i */
+      }
+      if(d == 0)                     /* check for mass elimination */
+      {
+        Cp[i] = amd_flip (k);      /* absorb i into k */
+        nvi = -nv[i];
+        dk -= nvi;                 /* |Lk| -= |i| */
+        nvk += nvi;                /* |k| += nv[i] */
+        nel += nvi;
+        nv[i] = 0;
+        elen[i] = -1;             /* node i is dead */
+      }
+      else
+      {
+        degree[i] = std::min<StorageIndex> (degree[i], d);   /* update degree(i) */
+        Ci[pn] = Ci[p3];         /* move first node to end */
+        Ci[p3] = Ci[p1];         /* move 1st el. to end of Ei */
+        Ci[p1] = k;               /* add k as 1st element in of Ei */
+        len[i] = pn - p1 + 1;     /* new len of adj. list of node i */
+        h %= n;                    /* finalize hash of i */
+        next[i] = hhead[h];      /* place i in hash bucket */
+        hhead[h] = i;
+        last[i] = h;      /* save hash of i in last[i] */
+      }
+    }                                   /* scan2 is done */
+    degree[k] = dk;                   /* finalize |Lk| */
+    lemax = std::max<StorageIndex>(lemax, dk);
+    mark = internal::cs_wclear<StorageIndex>(mark+lemax, lemax, w, n);    /* clear w */
+    
+    /* --- Supernode detection ------------------------------------------ */
+    for(pk = pk1; pk < pk2; pk++)
+    {
+      i = Ci[pk];
+      if(nv[i] >= 0) continue;         /* skip if i is dead */
+      h = last[i];                      /* scan hash bucket of node i */
+      i = hhead[h];
+      hhead[h] = -1;                    /* hash bucket will be empty */
+      for(; i != -1 && next[i] != -1; i = next[i], mark++)
+      {
+        ln = len[i];
+        eln = elen[i];
+        for(p = Cp[i]+1; p <= Cp[i] + ln-1; p++) w[Ci[p]] = mark;
+        jlast = i;
+        for(j = next[i]; j != -1; ) /* compare i with all j */
+        {
+          ok = (len[j] == ln) && (elen[j] == eln);
+          for(p = Cp[j] + 1; ok && p <= Cp[j] + ln - 1; p++)
+          {
+            if(w[Ci[p]] != mark) ok = 0;    /* compare i and j*/
+          }
+          if(ok)                     /* i and j are identical */
+          {
+            Cp[j] = amd_flip (i);  /* absorb j into i */
+            nv[i] += nv[j];
+            nv[j] = 0;
+            elen[j] = -1;         /* node j is dead */
+            j = next[j];          /* delete j from hash bucket */
+            next[jlast] = j;
+          }
+          else
+          {
+            jlast = j;             /* j and i are different */
+            j = next[j];
+          }
+        }
+      }
+    }
+    
+    /* --- Finalize new element------------------------------------------ */
+    for(p = pk1, pk = pk1; pk < pk2; pk++)   /* finalize Lk */
+    {
+      i = Ci[pk];
+      if((nvi = -nv[i]) <= 0) continue;/* skip if i is dead */
+      nv[i] = nvi;                      /* restore nv[i] */
+      d = degree[i] + dk - nvi;         /* compute external degree(i) */
+      d = std::min<StorageIndex> (d, n - nel - nvi);
+      if(head[d] != -1) last[head[d]] = i;
+      next[i] = head[d];               /* put i back in degree list */
+      last[i] = -1;
+      head[d] = i;
+      mindeg = std::min<StorageIndex> (mindeg, d);       /* find new minimum degree */
+      degree[i] = d;
+      Ci[p++] = i;                      /* place i in Lk */
+    }
+    nv[k] = nvk;                      /* # nodes absorbed into k */
+    if((len[k] = p-pk1) == 0)         /* length of adj list of element k*/
+    {
+      Cp[k] = -1;                   /* k is a root of the tree */
+      w[k] = 0;                     /* k is now a dead element */
+    }
+    if(elenk != 0) cnz = p;           /* free unused space in Lk */
+  }
+  
+  /* --- Postordering ----------------------------------------------------- */
+  for(i = 0; i < n; i++) Cp[i] = amd_flip (Cp[i]);/* fix assembly tree */
+  for(j = 0; j <= n; j++) head[j] = -1;
+  for(j = n; j >= 0; j--)              /* place unordered nodes in lists */
+  {
+    if(nv[j] > 0) continue;          /* skip if j is an element */
+    next[j] = head[Cp[j]];          /* place j in list of its parent */
+    head[Cp[j]] = j;
+  }
+  for(e = n; e >= 0; e--)              /* place elements in lists */
+  {
+    if(nv[e] <= 0) continue;         /* skip unless e is an element */
+    if(Cp[e] != -1)
+    {
+      next[e] = head[Cp[e]];      /* place e in list of its parent */
+      head[Cp[e]] = e;
+    }
+  }
+  for(k = 0, i = 0; i <= n; i++)       /* postorder the assembly tree */
+  {
+    if(Cp[i] == -1) k = internal::cs_tdfs<StorageIndex>(i, k, head, next, perm.indices().data(), w);
+  }
+  
+  perm.indices().conservativeResize(n);
+}
+
+} // namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_AMD_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h
new file mode 100644
index 0000000..8e339a7
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Eigen_Colamd.h
@@ -0,0 +1,1863 @@
+// // This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Desire Nuentsa Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+// This file is modified from the colamd/symamd library. The copyright is below
+
+//   The authors of the code itself are Stefan I. Larimore and Timothy A.
+//   Davis (davis@cise.ufl.edu), University of Florida.  The algorithm was
+//   developed in collaboration with John Gilbert, Xerox PARC, and Esmond
+//   Ng, Oak Ridge National Laboratory.
+//
+//     Date:
+//
+//   September 8, 2003.  Version 2.3.
+//
+//     Acknowledgements:
+//
+//   This work was supported by the National Science Foundation, under
+//   grants DMS-9504974 and DMS-9803599.
+//
+//     Notice:
+//
+//   Copyright (c) 1998-2003 by the University of Florida.
+//   All Rights Reserved.
+//
+//   THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+//   EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
+//
+//   Permission is hereby granted to use, copy, modify, and/or distribute
+//   this program, provided that the Copyright, this License, and the
+//   Availability of the original version is retained on all copies and made
+//   accessible to the end-user of any code or package that includes COLAMD
+//   or any modified version of COLAMD.
+//
+//     Availability:
+//
+//   The colamd/symamd library is available at
+//
+//       http://www.suitesparse.com
+
+
+#ifndef EIGEN_COLAMD_H
+#define EIGEN_COLAMD_H
+
+namespace internal {
+
+namespace Colamd {
+
+/* Ensure that debugging is turned off: */
+#ifndef COLAMD_NDEBUG
+#define COLAMD_NDEBUG
+#endif /* NDEBUG */
+
+
+/* ========================================================================== */
+/* === Knob and statistics definitions ====================================== */
+/* ========================================================================== */
+
+/* size of the knobs [ ] array.  Only knobs [0..1] are currently used. */
+const int NKnobs = 20;
+
+/* number of output statistics.  Only stats [0..6] are currently used. */
+const int NStats = 20;
+
+/* Indices into knobs and stats array. */
+enum KnobsStatsIndex {
+  /* knobs [0] and stats [0]: dense row knob and output statistic. */
+  DenseRow = 0,
+
+  /* knobs [1] and stats [1]: dense column knob and output statistic. */
+  DenseCol = 1,
+
+  /* stats [2]: memory defragmentation count output statistic */
+  DefragCount = 2,
+
+  /* stats [3]: colamd status:  zero OK, > 0 warning or notice, < 0 error */
+  Status = 3,
+
+  /* stats [4..6]: error info, or info on jumbled columns */
+  Info1 = 4,
+  Info2 = 5,
+  Info3 = 6
+};
+
+/* error codes returned in stats [3]: */
+enum Status {
+  Ok = 0,
+  OkButJumbled = 1,
+  ErrorANotPresent = -1,
+  ErrorPNotPresent = -2,
+  ErrorNrowNegative = -3,
+  ErrorNcolNegative = -4,
+  ErrorNnzNegative = -5,
+  ErrorP0Nonzero = -6,
+  ErrorATooSmall = -7,
+  ErrorColLengthNegative = -8,
+  ErrorRowIndexOutOfBounds = -9,
+  ErrorOutOfMemory = -10,
+  ErrorInternalError = -999
+};
+/* ========================================================================== */
+/* === Definitions ========================================================== */
+/* ========================================================================== */
+
+template <typename IndexType>
+IndexType ones_complement(const IndexType r) {
+  return (-(r)-1);
+}
+
+/* -------------------------------------------------------------------------- */
+const int Empty = -1;
+
+/* Row and column status */
+enum RowColumnStatus {
+  Alive = 0,
+  Dead = -1
+};
+
+/* Column status */
+enum ColumnStatus {
+  DeadPrincipal = -1,
+  DeadNonPrincipal = -2
+};
+
+/* ========================================================================== */
+/* === Colamd reporting mechanism =========================================== */
+/* ========================================================================== */
+
+// == Row and Column structures ==
+template <typename IndexType>
+struct ColStructure
+{
+  IndexType start ;   /* index for A of first row in this column, or Dead */
+  /* if column is dead */
+  IndexType length ;  /* number of rows in this column */
+  union
+  {
+    IndexType thickness ; /* number of original columns represented by this */
+    /* col, if the column is alive */
+    IndexType parent ;  /* parent in parent tree super-column structure, if */
+    /* the column is dead */
+  } shared1 ;
+  union
+  {
+    IndexType score ; /* the score used to maintain heap, if col is alive */
+    IndexType order ; /* pivot ordering of this column, if col is dead */
+  } shared2 ;
+  union
+  {
+    IndexType headhash ;  /* head of a hash bucket, if col is at the head of */
+    /* a degree list */
+    IndexType hash ;  /* hash value, if col is not in a degree list */
+    IndexType prev ;  /* previous column in degree list, if col is in a */
+    /* degree list (but not at the head of a degree list) */
+  } shared3 ;
+  union
+  {
+    IndexType degree_next ; /* next column, if col is in a degree list */
+    IndexType hash_next ;   /* next column, if col is in a hash list */
+  } shared4 ;
+
+  inline bool is_dead() const { return start < Alive; }
+
+  inline bool is_alive() const { return start >= Alive; }
+
+  inline bool is_dead_principal() const { return start == DeadPrincipal; }
+
+  inline void kill_principal() { start = DeadPrincipal; }
+
+  inline void kill_non_principal() { start = DeadNonPrincipal; }
+
+};
+
+template <typename IndexType>
+struct RowStructure
+{
+  IndexType start ;   /* index for A of first col in this row */
+  IndexType length ;  /* number of principal columns in this row */
+  union
+  {
+    IndexType degree ;  /* number of principal & non-principal columns in row */
+    IndexType p ;   /* used as a row pointer in init_rows_cols () */
+  } shared1 ;
+  union
+  {
+    IndexType mark ;  /* for computing set differences and marking dead rows*/
+    IndexType first_column ;/* first column in row (used in garbage collection) */
+  } shared2 ;
+
+  inline bool is_dead() const { return shared2.mark < Alive; }
+
+  inline bool is_alive() const { return shared2.mark >= Alive; }
+
+  inline void kill() { shared2.mark = Dead; }
+
+};
+
+/* ========================================================================== */
+/* === Colamd recommended memory size ======================================= */
+/* ========================================================================== */
+
+/*
+  The recommended length Alen of the array A passed to colamd is given by
+  the COLAMD_RECOMMENDED (nnz, n_row, n_col) macro.  It returns -1 if any
+  argument is negative.  2*nnz space is required for the row and column
+  indices of the matrix. colamd_c (n_col) + colamd_r (n_row) space is
+  required for the Col and Row arrays, respectively, which are internal to
+  colamd.  An additional n_col space is the minimal amount of "elbow room",
+  and nnz/5 more space is recommended for run time efficiency.
+
+  This macro is not needed when using symamd.
+
+  Explicit typecast to IndexType added Sept. 23, 2002, COLAMD version 2.2, to avoid
+  gcc -pedantic warning messages.
+*/
+template <typename IndexType>
+inline IndexType colamd_c(IndexType n_col)
+{ return IndexType( ((n_col) + 1) * sizeof (ColStructure<IndexType>) / sizeof (IndexType) ) ; }
+
+template <typename IndexType>
+inline IndexType  colamd_r(IndexType n_row)
+{ return IndexType(((n_row) + 1) * sizeof (RowStructure<IndexType>) / sizeof (IndexType)); }
+
+// Prototypes of non-user callable routines
+template <typename IndexType>
+static IndexType init_rows_cols (IndexType n_row, IndexType n_col, RowStructure<IndexType> Row [], ColStructure<IndexType> col [], IndexType A [], IndexType p [], IndexType stats[NStats] );
+
+template <typename IndexType>
+static void init_scoring (IndexType n_row, IndexType n_col, RowStructure<IndexType> Row [], ColStructure<IndexType> Col [], IndexType A [], IndexType head [], double knobs[NKnobs], IndexType *p_n_row2, IndexType *p_n_col2, IndexType *p_max_deg);
+
+template <typename IndexType>
+static IndexType find_ordering (IndexType n_row, IndexType n_col, IndexType Alen, RowStructure<IndexType> Row [], ColStructure<IndexType> Col [], IndexType A [], IndexType head [], IndexType n_col2, IndexType max_deg, IndexType pfree);
+
+template <typename IndexType>
+static void order_children (IndexType n_col, ColStructure<IndexType> Col [], IndexType p []);
+
+template <typename IndexType>
+static void detect_super_cols (ColStructure<IndexType> Col [], IndexType A [], IndexType head [], IndexType row_start, IndexType row_length ) ;
+
+template <typename IndexType>
+static IndexType garbage_collection (IndexType n_row, IndexType n_col, RowStructure<IndexType> Row [], ColStructure<IndexType> Col [], IndexType A [], IndexType *pfree) ;
+
+template <typename IndexType>
+static inline  IndexType clear_mark (IndexType n_row, RowStructure<IndexType> Row [] ) ;
+
+/* === No debugging ========================================================= */
+
+#define COLAMD_DEBUG0(params) ;
+#define COLAMD_DEBUG1(params) ;
+#define COLAMD_DEBUG2(params) ;
+#define COLAMD_DEBUG3(params) ;
+#define COLAMD_DEBUG4(params) ;
+
+#define COLAMD_ASSERT(expression) ((void) 0)
+
+
+/**
+ * \brief Returns the recommended value of Alen
+ *
+ * Returns recommended value of Alen for use by colamd.
+ * Returns -1 if any input argument is negative.
+ * The use of this routine or macro is optional.
+ * Note that the macro uses its arguments   more than once,
+ * so be careful for side effects, if you pass expressions as arguments to COLAMD_RECOMMENDED.
+ *
+ * \param nnz nonzeros in A
+ * \param n_row number of rows in A
+ * \param n_col number of columns in A
+ * \return recommended value of Alen for use by colamd
+ */
+template <typename IndexType>
+inline IndexType recommended ( IndexType nnz, IndexType n_row, IndexType n_col)
+{
+  if ((nnz) < 0 || (n_row) < 0 || (n_col) < 0)
+    return (-1);
+  else
+    return (2 * (nnz) + colamd_c (n_col) + colamd_r (n_row) + (n_col) + ((nnz) / 5));
+}
+
+/**
+ * \brief set default parameters  The use of this routine is optional.
+ *
+ * Colamd: rows with more than (knobs [DenseRow] * n_col)
+ * entries are removed prior to ordering.  Columns with more than
+ * (knobs [DenseCol] * n_row) entries are removed prior to
+ * ordering, and placed last in the output column ordering.
+ *
+ * DenseRow and DenseCol are defined as 0 and 1,
+ * respectively, in colamd.h.  Default values of these two knobs
+ * are both 0.5.  Currently, only knobs [0] and knobs [1] are
+ * used, but future versions may use more knobs.  If so, they will
+ * be properly set to their defaults by the future version of
+ * colamd_set_defaults, so that the code that calls colamd will
+ * not need to change, assuming that you either use
+ * colamd_set_defaults, or pass a (double *) NULL pointer as the
+ * knobs array to colamd or symamd.
+ *
+ * \param knobs parameter settings for colamd
+ */
+
+static inline void set_defaults(double knobs[NKnobs])
+{
+  /* === Local variables ================================================== */
+
+  int i ;
+
+  if (!knobs)
+  {
+    return ;      /* no knobs to initialize */
+  }
+  for (i = 0 ; i < NKnobs ; i++)
+  {
+    knobs [i] = 0 ;
+  }
+  knobs [Colamd::DenseRow] = 0.5 ;  /* ignore rows over 50% dense */
+  knobs [Colamd::DenseCol] = 0.5 ;  /* ignore columns over 50% dense */
+}
+
+/**
+ * \brief  Computes a column ordering using the column approximate minimum degree ordering
+ *
+ * Computes a column ordering (Q) of A such that P(AQ)=LU or
+ * (AQ)'AQ=LL' have less fill-in and require fewer floating point
+ * operations than factorizing the unpermuted matrix A or A'A,
+ * respectively.
+ *
+ *
+ * \param n_row number of rows in A
+ * \param n_col number of columns in A
+ * \param Alen, size of the array A
+ * \param A row indices of the matrix, of size ALen
+ * \param p column pointers of A, of size n_col+1
+ * \param knobs parameter settings for colamd
+ * \param stats colamd output statistics and error codes
+ */
+template <typename IndexType>
+static bool compute_ordering(IndexType n_row, IndexType n_col, IndexType Alen, IndexType *A, IndexType *p, double knobs[NKnobs], IndexType stats[NStats])
+{
+  /* === Local variables ================================================== */
+
+  IndexType i ;     /* loop index */
+  IndexType nnz ;     /* nonzeros in A */
+  IndexType Row_size ;    /* size of Row [], in integers */
+  IndexType Col_size ;    /* size of Col [], in integers */
+  IndexType need ;      /* minimum required length of A */
+  Colamd::RowStructure<IndexType> *Row ;   /* pointer into A of Row [0..n_row] array */
+  Colamd::ColStructure<IndexType> *Col ;   /* pointer into A of Col [0..n_col] array */
+  IndexType n_col2 ;    /* number of non-dense, non-empty columns */
+  IndexType n_row2 ;    /* number of non-dense, non-empty rows */
+  IndexType ngarbage ;    /* number of garbage collections performed */
+  IndexType max_deg ;   /* maximum row degree */
+  double default_knobs [NKnobs] ; /* default knobs array */
+
+
+  /* === Check the input arguments ======================================== */
+
+  if (!stats)
+  {
+    COLAMD_DEBUG0 (("colamd: stats not present\n")) ;
+    return (false) ;
+  }
+  for (i = 0 ; i < NStats ; i++)
+  {
+    stats [i] = 0 ;
+  }
+  stats [Colamd::Status] = Colamd::Ok ;
+  stats [Colamd::Info1] = -1 ;
+  stats [Colamd::Info2] = -1 ;
+
+  if (!A)   /* A is not present */
+  {
+    stats [Colamd::Status] = Colamd::ErrorANotPresent ;
+    COLAMD_DEBUG0 (("colamd: A not present\n")) ;
+    return (false) ;
+  }
+
+  if (!p)   /* p is not present */
+  {
+    stats [Colamd::Status] = Colamd::ErrorPNotPresent ;
+    COLAMD_DEBUG0 (("colamd: p not present\n")) ;
+    return (false) ;
+  }
+
+  if (n_row < 0)  /* n_row must be >= 0 */
+  {
+    stats [Colamd::Status] = Colamd::ErrorNrowNegative ;
+    stats [Colamd::Info1] = n_row ;
+    COLAMD_DEBUG0 (("colamd: nrow negative %d\n", n_row)) ;
+    return (false) ;
+  }
+
+  if (n_col < 0)  /* n_col must be >= 0 */
+  {
+    stats [Colamd::Status] = Colamd::ErrorNcolNegative ;
+    stats [Colamd::Info1] = n_col ;
+    COLAMD_DEBUG0 (("colamd: ncol negative %d\n", n_col)) ;
+    return (false) ;
+  }
+
+  nnz = p [n_col] ;
+  if (nnz < 0)  /* nnz must be >= 0 */
+  {
+    stats [Colamd::Status] = Colamd::ErrorNnzNegative ;
+    stats [Colamd::Info1] = nnz ;
+    COLAMD_DEBUG0 (("colamd: number of entries negative %d\n", nnz)) ;
+    return (false) ;
+  }
+
+  if (p [0] != 0)
+  {
+    stats [Colamd::Status] = Colamd::ErrorP0Nonzero ;
+    stats [Colamd::Info1] = p [0] ;
+    COLAMD_DEBUG0 (("colamd: p[0] not zero %d\n", p [0])) ;
+    return (false) ;
+  }
+
+  /* === If no knobs, set default knobs =================================== */
+
+  if (!knobs)
+  {
+    set_defaults (default_knobs) ;
+    knobs = default_knobs ;
+  }
+
+  /* === Allocate the Row and Col arrays from array A ===================== */
+
+  Col_size = colamd_c (n_col) ;
+  Row_size = colamd_r (n_row) ;
+  need = 2*nnz + n_col + Col_size + Row_size ;
+
+  if (need > Alen)
+  {
+    /* not enough space in array A to perform the ordering */
+    stats [Colamd::Status] = Colamd::ErrorATooSmall ;
+    stats [Colamd::Info1] = need ;
+    stats [Colamd::Info2] = Alen ;
+    COLAMD_DEBUG0 (("colamd: Need Alen >= %d, given only Alen = %d\n", need,Alen));
+    return (false) ;
+  }
+
+  Alen -= Col_size + Row_size ;
+  Col = (ColStructure<IndexType> *) &A [Alen] ;
+  Row = (RowStructure<IndexType> *) &A [Alen + Col_size] ;
+
+  /* === Construct the row and column data structures ===================== */
+
+  if (!Colamd::init_rows_cols (n_row, n_col, Row, Col, A, p, stats))
+  {
+    /* input matrix is invalid */
+    COLAMD_DEBUG0 (("colamd: Matrix invalid\n")) ;
+    return (false) ;
+  }
+
+  /* === Initialize scores, kill dense rows/columns ======================= */
+
+  Colamd::init_scoring (n_row, n_col, Row, Col, A, p, knobs,
+		&n_row2, &n_col2, &max_deg) ;
+
+  /* === Order the supercolumns =========================================== */
+
+  ngarbage = Colamd::find_ordering (n_row, n_col, Alen, Row, Col, A, p,
+			    n_col2, max_deg, 2*nnz) ;
+
+  /* === Order the non-principal columns ================================== */
+
+  Colamd::order_children (n_col, Col, p) ;
+
+  /* === Return statistics in stats ======================================= */
+
+  stats [Colamd::DenseRow] = n_row - n_row2 ;
+  stats [Colamd::DenseCol] = n_col - n_col2 ;
+  stats [Colamd::DefragCount] = ngarbage ;
+  COLAMD_DEBUG0 (("colamd: done.\n")) ;
+  return (true) ;
+}
+
+/* ========================================================================== */
+/* === NON-USER-CALLABLE ROUTINES: ========================================== */
+/* ========================================================================== */
+
+/* There are no user-callable routines beyond this point in the file */
+
+/* ========================================================================== */
+/* === init_rows_cols ======================================================= */
+/* ========================================================================== */
+
+/*
+  Takes the column form of the matrix in A and creates the row form of the
+  matrix.  Also, row and column attributes are stored in the Col and Row
+  structs.  If the columns are un-sorted or contain duplicate row indices,
+  this routine will also sort and remove duplicate row indices from the
+  column form of the matrix.  Returns false if the matrix is invalid,
+  true otherwise.  Not user-callable.
+*/
+template <typename IndexType>
+static IndexType init_rows_cols  /* returns true if OK, or false otherwise */
+  (
+    /* === Parameters ======================================================= */
+
+    IndexType n_row,      /* number of rows of A */
+    IndexType n_col,      /* number of columns of A */
+    RowStructure<IndexType> Row [],    /* of size n_row+1 */
+    ColStructure<IndexType> Col [],    /* of size n_col+1 */
+    IndexType A [],     /* row indices of A, of size Alen */
+    IndexType p [],     /* pointers to columns in A, of size n_col+1 */
+    IndexType stats [NStats]  /* colamd statistics */
+    )
+{
+  /* === Local variables ================================================== */
+
+  IndexType col ;     /* a column index */
+  IndexType row ;     /* a row index */
+  IndexType *cp ;     /* a column pointer */
+  IndexType *cp_end ;   /* a pointer to the end of a column */
+  IndexType *rp ;     /* a row pointer */
+  IndexType *rp_end ;   /* a pointer to the end of a row */
+  IndexType last_row ;    /* previous row */
+
+  /* === Initialize columns, and check column pointers ==================== */
+
+  for (col = 0 ; col < n_col ; col++)
+  {
+    Col [col].start = p [col] ;
+    Col [col].length = p [col+1] - p [col] ;
+
+    if ((Col [col].length) < 0) // extra parentheses to work-around gcc bug 10200
+    {
+      /* column pointers must be non-decreasing */
+      stats [Colamd::Status] = Colamd::ErrorColLengthNegative ;
+      stats [Colamd::Info1] = col ;
+      stats [Colamd::Info2] = Col [col].length ;
+      COLAMD_DEBUG0 (("colamd: col %d length %d < 0\n", col, Col [col].length)) ;
+      return (false) ;
+    }
+
+    Col [col].shared1.thickness = 1 ;
+    Col [col].shared2.score = 0 ;
+    Col [col].shared3.prev = Empty ;
+    Col [col].shared4.degree_next = Empty ;
+  }
+
+  /* p [0..n_col] no longer needed, used as "head" in subsequent routines */
+
+  /* === Scan columns, compute row degrees, and check row indices ========= */
+
+  stats [Info3] = 0 ;  /* number of duplicate or unsorted row indices*/
+
+  for (row = 0 ; row < n_row ; row++)
+  {
+    Row [row].length = 0 ;
+    Row [row].shared2.mark = -1 ;
+  }
+
+  for (col = 0 ; col < n_col ; col++)
+  {
+    last_row = -1 ;
+
+    cp = &A [p [col]] ;
+    cp_end = &A [p [col+1]] ;
+
+    while (cp < cp_end)
+    {
+      row = *cp++ ;
+
+      /* make sure row indices within range */
+      if (row < 0 || row >= n_row)
+      {
+	stats [Colamd::Status] = Colamd::ErrorRowIndexOutOfBounds ;
+	stats [Colamd::Info1] = col ;
+	stats [Colamd::Info2] = row ;
+	stats [Colamd::Info3] = n_row ;
+	COLAMD_DEBUG0 (("colamd: row %d col %d out of bounds\n", row, col)) ;
+	return (false) ;
+      }
+
+      if (row <= last_row || Row [row].shared2.mark == col)
+      {
+	/* row index are unsorted or repeated (or both), thus col */
+	/* is jumbled.  This is a notice, not an error condition. */
+	stats [Colamd::Status] = Colamd::OkButJumbled ;
+	stats [Colamd::Info1] = col ;
+	stats [Colamd::Info2] = row ;
+	(stats [Colamd::Info3]) ++ ;
+	COLAMD_DEBUG1 (("colamd: row %d col %d unsorted/duplicate\n",row,col));
+      }
+
+      if (Row [row].shared2.mark != col)
+      {
+	Row [row].length++ ;
+      }
+      else
+      {
+	/* this is a repeated entry in the column, */
+	/* it will be removed */
+	Col [col].length-- ;
+      }
+
+      /* mark the row as having been seen in this column */
+      Row [row].shared2.mark = col ;
+
+      last_row = row ;
+    }
+  }
+
+  /* === Compute row pointers ============================================= */
+
+  /* row form of the matrix starts directly after the column */
+  /* form of matrix in A */
+  Row [0].start = p [n_col] ;
+  Row [0].shared1.p = Row [0].start ;
+  Row [0].shared2.mark = -1 ;
+  for (row = 1 ; row < n_row ; row++)
+  {
+    Row [row].start = Row [row-1].start + Row [row-1].length ;
+    Row [row].shared1.p = Row [row].start ;
+    Row [row].shared2.mark = -1 ;
+  }
+
+  /* === Create row form ================================================== */
+
+  if (stats [Status] == OkButJumbled)
+  {
+    /* if cols jumbled, watch for repeated row indices */
+    for (col = 0 ; col < n_col ; col++)
+    {
+      cp = &A [p [col]] ;
+      cp_end = &A [p [col+1]] ;
+      while (cp < cp_end)
+      {
+	row = *cp++ ;
+	if (Row [row].shared2.mark != col)
+	{
+	  A [(Row [row].shared1.p)++] = col ;
+	  Row [row].shared2.mark = col ;
+	}
+      }
+    }
+  }
+  else
+  {
+    /* if cols not jumbled, we don't need the mark (this is faster) */
+    for (col = 0 ; col < n_col ; col++)
+    {
+      cp = &A [p [col]] ;
+      cp_end = &A [p [col+1]] ;
+      while (cp < cp_end)
+      {
+	A [(Row [*cp++].shared1.p)++] = col ;
+      }
+    }
+  }
+
+  /* === Clear the row marks and set row degrees ========================== */
+
+  for (row = 0 ; row < n_row ; row++)
+  {
+    Row [row].shared2.mark = 0 ;
+    Row [row].shared1.degree = Row [row].length ;
+  }
+
+  /* === See if we need to re-create columns ============================== */
+
+  if (stats [Status] == OkButJumbled)
+  {
+    COLAMD_DEBUG0 (("colamd: reconstructing column form, matrix jumbled\n")) ;
+
+
+    /* === Compute col pointers ========================================= */
+
+    /* col form of the matrix starts at A [0]. */
+    /* Note, we may have a gap between the col form and the row */
+    /* form if there were duplicate entries, if so, it will be */
+    /* removed upon the first garbage collection */
+    Col [0].start = 0 ;
+    p [0] = Col [0].start ;
+    for (col = 1 ; col < n_col ; col++)
+    {
+      /* note that the lengths here are for pruned columns, i.e. */
+      /* no duplicate row indices will exist for these columns */
+      Col [col].start = Col [col-1].start + Col [col-1].length ;
+      p [col] = Col [col].start ;
+    }
+
+    /* === Re-create col form =========================================== */
+
+    for (row = 0 ; row < n_row ; row++)
+    {
+      rp = &A [Row [row].start] ;
+      rp_end = rp + Row [row].length ;
+      while (rp < rp_end)
+      {
+	A [(p [*rp++])++] = row ;
+      }
+    }
+  }
+
+  /* === Done.  Matrix is not (or no longer) jumbled ====================== */
+
+  return (true) ;
+}
+
+
+/* ========================================================================== */
+/* === init_scoring ========================================================= */
+/* ========================================================================== */
+
+/*
+  Kills dense or empty columns and rows, calculates an initial score for
+  each column, and places all columns in the degree lists.  Not user-callable.
+*/
+template <typename IndexType>
+static void init_scoring
+  (
+    /* === Parameters ======================================================= */
+
+    IndexType n_row,      /* number of rows of A */
+    IndexType n_col,      /* number of columns of A */
+    RowStructure<IndexType> Row [],    /* of size n_row+1 */
+    ColStructure<IndexType> Col [],    /* of size n_col+1 */
+    IndexType A [],     /* column form and row form of A */
+    IndexType head [],    /* of size n_col+1 */
+    double knobs [NKnobs],/* parameters */
+    IndexType *p_n_row2,    /* number of non-dense, non-empty rows */
+    IndexType *p_n_col2,    /* number of non-dense, non-empty columns */
+    IndexType *p_max_deg    /* maximum row degree */
+    )
+{
+  /* === Local variables ================================================== */
+
+  IndexType c ;     /* a column index */
+  IndexType r, row ;    /* a row index */
+  IndexType *cp ;     /* a column pointer */
+  IndexType deg ;     /* degree of a row or column */
+  IndexType *cp_end ;   /* a pointer to the end of a column */
+  IndexType *new_cp ;   /* new column pointer */
+  IndexType col_length ;    /* length of pruned column */
+  IndexType score ;     /* current column score */
+  IndexType n_col2 ;    /* number of non-dense, non-empty columns */
+  IndexType n_row2 ;    /* number of non-dense, non-empty rows */
+  IndexType dense_row_count ; /* remove rows with more entries than this */
+  IndexType dense_col_count ; /* remove cols with more entries than this */
+  IndexType min_score ;   /* smallest column score */
+  IndexType max_deg ;   /* maximum row degree */
+  IndexType next_col ;    /* Used to add to degree list.*/
+
+
+  /* === Extract knobs ==================================================== */
+
+  dense_row_count = numext::maxi(IndexType(0), numext::mini(IndexType(knobs [Colamd::DenseRow] * n_col), n_col)) ;
+  dense_col_count = numext::maxi(IndexType(0), numext::mini(IndexType(knobs [Colamd::DenseCol] * n_row), n_row)) ;
+  COLAMD_DEBUG1 (("colamd: densecount: %d %d\n", dense_row_count, dense_col_count)) ;
+  max_deg = 0 ;
+  n_col2 = n_col ;
+  n_row2 = n_row ;
+
+  /* === Kill empty columns =============================================== */
+
+  /* Put the empty columns at the end in their natural order, so that LU */
+  /* factorization can proceed as far as possible. */
+  for (c = n_col-1 ; c >= 0 ; c--)
+  {
+    deg = Col [c].length ;
+    if (deg == 0)
+    {
+      /* this is a empty column, kill and order it last */
+      Col [c].shared2.order = --n_col2 ;
+      Col[c].kill_principal() ;
+    }
+  }
+  COLAMD_DEBUG1 (("colamd: null columns killed: %d\n", n_col - n_col2)) ;
+
+  /* === Kill dense columns =============================================== */
+
+  /* Put the dense columns at the end, in their natural order */
+  for (c = n_col-1 ; c >= 0 ; c--)
+  {
+    /* skip any dead columns */
+    if (Col[c].is_dead())
+    {
+      continue ;
+    }
+    deg = Col [c].length ;
+    if (deg > dense_col_count)
+    {
+      /* this is a dense column, kill and order it last */
+      Col [c].shared2.order = --n_col2 ;
+      /* decrement the row degrees */
+      cp = &A [Col [c].start] ;
+      cp_end = cp + Col [c].length ;
+      while (cp < cp_end)
+      {
+	Row [*cp++].shared1.degree-- ;
+      }
+      Col[c].kill_principal() ;
+    }
+  }
+  COLAMD_DEBUG1 (("colamd: Dense and null columns killed: %d\n", n_col - n_col2)) ;
+
+  /* === Kill dense and empty rows ======================================== */
+
+  for (r = 0 ; r < n_row ; r++)
+  {
+    deg = Row [r].shared1.degree ;
+    COLAMD_ASSERT (deg >= 0 && deg <= n_col) ;
+    if (deg > dense_row_count || deg == 0)
+    {
+      /* kill a dense or empty row */
+      Row[r].kill() ;
+      --n_row2 ;
+    }
+    else
+    {
+      /* keep track of max degree of remaining rows */
+      max_deg = numext::maxi(max_deg, deg) ;
+    }
+  }
+  COLAMD_DEBUG1 (("colamd: Dense and null rows killed: %d\n", n_row - n_row2)) ;
+
+  /* === Compute initial column scores ==================================== */
+
+  /* At this point the row degrees are accurate.  They reflect the number */
+  /* of "live" (non-dense) columns in each row.  No empty rows exist. */
+  /* Some "live" columns may contain only dead rows, however.  These are */
+  /* pruned in the code below. */
+
+  /* now find the initial matlab score for each column */
+  for (c = n_col-1 ; c >= 0 ; c--)
+  {
+    /* skip dead column */
+    if (Col[c].is_dead())
+    {
+      continue ;
+    }
+    score = 0 ;
+    cp = &A [Col [c].start] ;
+    new_cp = cp ;
+    cp_end = cp + Col [c].length ;
+    while (cp < cp_end)
+    {
+      /* get a row */
+      row = *cp++ ;
+      /* skip if dead */
+      if (Row[row].is_dead())
+      {
+	continue ;
+      }
+      /* compact the column */
+      *new_cp++ = row ;
+      /* add row's external degree */
+      score += Row [row].shared1.degree - 1 ;
+      /* guard against integer overflow */
+      score = numext::mini(score, n_col) ;
+    }
+    /* determine pruned column length */
+    col_length = (IndexType) (new_cp - &A [Col [c].start]) ;
+    if (col_length == 0)
+    {
+      /* a newly-made null column (all rows in this col are "dense" */
+      /* and have already been killed) */
+      COLAMD_DEBUG2 (("Newly null killed: %d\n", c)) ;
+      Col [c].shared2.order = --n_col2 ;
+      Col[c].kill_principal() ;
+    }
+    else
+    {
+      /* set column length and set score */
+      COLAMD_ASSERT (score >= 0) ;
+      COLAMD_ASSERT (score <= n_col) ;
+      Col [c].length = col_length ;
+      Col [c].shared2.score = score ;
+    }
+  }
+  COLAMD_DEBUG1 (("colamd: Dense, null, and newly-null columns killed: %d\n",
+		  n_col-n_col2)) ;
+
+  /* At this point, all empty rows and columns are dead.  All live columns */
+  /* are "clean" (containing no dead rows) and simplicial (no supercolumns */
+  /* yet).  Rows may contain dead columns, but all live rows contain at */
+  /* least one live column. */
+
+  /* === Initialize degree lists ========================================== */
+
+
+  /* clear the hash buckets */
+  for (c = 0 ; c <= n_col ; c++)
+  {
+    head [c] = Empty ;
+  }
+  min_score = n_col ;
+  /* place in reverse order, so low column indices are at the front */
+  /* of the lists.  This is to encourage natural tie-breaking */
+  for (c = n_col-1 ; c >= 0 ; c--)
+  {
+    /* only add principal columns to degree lists */
+    if (Col[c].is_alive())
+    {
+      COLAMD_DEBUG4 (("place %d score %d minscore %d ncol %d\n",
+		      c, Col [c].shared2.score, min_score, n_col)) ;
+
+      /* === Add columns score to DList =============================== */
+
+      score = Col [c].shared2.score ;
+
+      COLAMD_ASSERT (min_score >= 0) ;
+      COLAMD_ASSERT (min_score <= n_col) ;
+      COLAMD_ASSERT (score >= 0) ;
+      COLAMD_ASSERT (score <= n_col) ;
+      COLAMD_ASSERT (head [score] >= Empty) ;
+
+      /* now add this column to dList at proper score location */
+      next_col = head [score] ;
+      Col [c].shared3.prev = Empty ;
+      Col [c].shared4.degree_next = next_col ;
+
+      /* if there already was a column with the same score, set its */
+      /* previous pointer to this new column */
+      if (next_col != Empty)
+      {
+	Col [next_col].shared3.prev = c ;
+      }
+      head [score] = c ;
+
+      /* see if this score is less than current min */
+      min_score = numext::mini(min_score, score) ;
+
+
+    }
+  }
+
+
+  /* === Return number of remaining columns, and max row degree =========== */
+
+  *p_n_col2 = n_col2 ;
+  *p_n_row2 = n_row2 ;
+  *p_max_deg = max_deg ;
+}
+
+
+/* ========================================================================== */
+/* === find_ordering ======================================================== */
+/* ========================================================================== */
+
+/*
+  Order the principal columns of the supercolumn form of the matrix
+  (no supercolumns on input).  Uses a minimum approximate column minimum
+  degree ordering method.  Not user-callable.
+*/
+template <typename IndexType>
+static IndexType find_ordering /* return the number of garbage collections */
+  (
+    /* === Parameters ======================================================= */
+
+    IndexType n_row,      /* number of rows of A */
+    IndexType n_col,      /* number of columns of A */
+    IndexType Alen,     /* size of A, 2*nnz + n_col or larger */
+    RowStructure<IndexType> Row [],    /* of size n_row+1 */
+    ColStructure<IndexType> Col [],    /* of size n_col+1 */
+    IndexType A [],     /* column form and row form of A */
+    IndexType head [],    /* of size n_col+1 */
+    IndexType n_col2,     /* Remaining columns to order */
+    IndexType max_deg,    /* Maximum row degree */
+    IndexType pfree     /* index of first free slot (2*nnz on entry) */
+    )
+{
+  /* === Local variables ================================================== */
+
+  IndexType k ;     /* current pivot ordering step */
+  IndexType pivot_col ;   /* current pivot column */
+  IndexType *cp ;     /* a column pointer */
+  IndexType *rp ;     /* a row pointer */
+  IndexType pivot_row ;   /* current pivot row */
+  IndexType *new_cp ;   /* modified column pointer */
+  IndexType *new_rp ;   /* modified row pointer */
+  IndexType pivot_row_start ; /* pointer to start of pivot row */
+  IndexType pivot_row_degree ;  /* number of columns in pivot row */
+  IndexType pivot_row_length ;  /* number of supercolumns in pivot row */
+  IndexType pivot_col_score ; /* score of pivot column */
+  IndexType needed_memory ;   /* free space needed for pivot row */
+  IndexType *cp_end ;   /* pointer to the end of a column */
+  IndexType *rp_end ;   /* pointer to the end of a row */
+  IndexType row ;     /* a row index */
+  IndexType col ;     /* a column index */
+  IndexType max_score ;   /* maximum possible score */
+  IndexType cur_score ;   /* score of current column */
+  unsigned int hash ;   /* hash value for supernode detection */
+  IndexType head_column ;   /* head of hash bucket */
+  IndexType first_col ;   /* first column in hash bucket */
+  IndexType tag_mark ;    /* marker value for mark array */
+  IndexType row_mark ;    /* Row [row].shared2.mark */
+  IndexType set_difference ;  /* set difference size of row with pivot row */
+  IndexType min_score ;   /* smallest column score */
+  IndexType col_thickness ;   /* "thickness" (no. of columns in a supercol) */
+  IndexType max_mark ;    /* maximum value of tag_mark */
+  IndexType pivot_col_thickness ; /* number of columns represented by pivot col */
+  IndexType prev_col ;    /* Used by Dlist operations. */
+  IndexType next_col ;    /* Used by Dlist operations. */
+  IndexType ngarbage ;    /* number of garbage collections performed */
+
+
+  /* === Initialization and clear mark ==================================== */
+
+  max_mark = INT_MAX - n_col ;  /* INT_MAX defined in <limits.h> */
+  tag_mark = Colamd::clear_mark (n_row, Row) ;
+  min_score = 0 ;
+  ngarbage = 0 ;
+  COLAMD_DEBUG1 (("colamd: Ordering, n_col2=%d\n", n_col2)) ;
+
+  /* === Order the columns ================================================ */
+
+  for (k = 0 ; k < n_col2 ; /* 'k' is incremented below */)
+  {
+
+    /* === Select pivot column, and order it ============================ */
+
+    /* make sure degree list isn't empty */
+    COLAMD_ASSERT (min_score >= 0) ;
+    COLAMD_ASSERT (min_score <= n_col) ;
+    COLAMD_ASSERT (head [min_score] >= Empty) ;
+
+    /* get pivot column from head of minimum degree list */
+    while (min_score < n_col && head [min_score] == Empty)
+    {
+      min_score++ ;
+    }
+    pivot_col = head [min_score] ;
+    COLAMD_ASSERT (pivot_col >= 0 && pivot_col <= n_col) ;
+    next_col = Col [pivot_col].shared4.degree_next ;
+    head [min_score] = next_col ;
+    if (next_col != Empty)
+    {
+      Col [next_col].shared3.prev = Empty ;
+    }
+
+    COLAMD_ASSERT (Col[pivot_col].is_alive()) ;
+    COLAMD_DEBUG3 (("Pivot col: %d\n", pivot_col)) ;
+
+    /* remember score for defrag check */
+    pivot_col_score = Col [pivot_col].shared2.score ;
+
+    /* the pivot column is the kth column in the pivot order */
+    Col [pivot_col].shared2.order = k ;
+
+    /* increment order count by column thickness */
+    pivot_col_thickness = Col [pivot_col].shared1.thickness ;
+    k += pivot_col_thickness ;
+    COLAMD_ASSERT (pivot_col_thickness > 0) ;
+
+    /* === Garbage_collection, if necessary ============================= */
+
+    needed_memory = numext::mini(pivot_col_score, n_col - k) ;
+    if (pfree + needed_memory >= Alen)
+    {
+      pfree = Colamd::garbage_collection (n_row, n_col, Row, Col, A, &A [pfree]) ;
+      ngarbage++ ;
+      /* after garbage collection we will have enough */
+      COLAMD_ASSERT (pfree + needed_memory < Alen) ;
+      /* garbage collection has wiped out the Row[].shared2.mark array */
+      tag_mark = Colamd::clear_mark (n_row, Row) ;
+
+    }
+
+    /* === Compute pivot row pattern ==================================== */
+
+    /* get starting location for this new merged row */
+    pivot_row_start = pfree ;
+
+    /* initialize new row counts to zero */
+    pivot_row_degree = 0 ;
+
+    /* tag pivot column as having been visited so it isn't included */
+    /* in merged pivot row */
+    Col [pivot_col].shared1.thickness = -pivot_col_thickness ;
+
+    /* pivot row is the union of all rows in the pivot column pattern */
+    cp = &A [Col [pivot_col].start] ;
+    cp_end = cp + Col [pivot_col].length ;
+    while (cp < cp_end)
+    {
+      /* get a row */
+      row = *cp++ ;
+      COLAMD_DEBUG4 (("Pivot col pattern %d %d\n", Row[row].is_alive(), row)) ;
+      /* skip if row is dead */
+      if (Row[row].is_dead())
+      {
+	continue ;
+      }
+      rp = &A [Row [row].start] ;
+      rp_end = rp + Row [row].length ;
+      while (rp < rp_end)
+      {
+	/* get a column */
+	col = *rp++ ;
+	/* add the column, if alive and untagged */
+	col_thickness = Col [col].shared1.thickness ;
+	if (col_thickness > 0 && Col[col].is_alive())
+	{
+	  /* tag column in pivot row */
+	  Col [col].shared1.thickness = -col_thickness ;
+	  COLAMD_ASSERT (pfree < Alen) ;
+	  /* place column in pivot row */
+	  A [pfree++] = col ;
+	  pivot_row_degree += col_thickness ;
+	}
+      }
+    }
+
+    /* clear tag on pivot column */
+    Col [pivot_col].shared1.thickness = pivot_col_thickness ;
+    max_deg = numext::maxi(max_deg, pivot_row_degree) ;
+
+
+    /* === Kill all rows used to construct pivot row ==================== */
+
+    /* also kill pivot row, temporarily */
+    cp = &A [Col [pivot_col].start] ;
+    cp_end = cp + Col [pivot_col].length ;
+    while (cp < cp_end)
+    {
+      /* may be killing an already dead row */
+      row = *cp++ ;
+      COLAMD_DEBUG3 (("Kill row in pivot col: %d\n", row)) ;
+      Row[row].kill() ;
+    }
+
+    /* === Select a row index to use as the new pivot row =============== */
+
+    pivot_row_length = pfree - pivot_row_start ;
+    if (pivot_row_length > 0)
+    {
+      /* pick the "pivot" row arbitrarily (first row in col) */
+      pivot_row = A [Col [pivot_col].start] ;
+      COLAMD_DEBUG3 (("Pivotal row is %d\n", pivot_row)) ;
+    }
+    else
+    {
+      /* there is no pivot row, since it is of zero length */
+      pivot_row = Empty ;
+      COLAMD_ASSERT (pivot_row_length == 0) ;
+    }
+    COLAMD_ASSERT (Col [pivot_col].length > 0 || pivot_row_length == 0) ;
+
+    /* === Approximate degree computation =============================== */
+
+    /* Here begins the computation of the approximate degree.  The column */
+    /* score is the sum of the pivot row "length", plus the size of the */
+    /* set differences of each row in the column minus the pattern of the */
+    /* pivot row itself.  The column ("thickness") itself is also */
+    /* excluded from the column score (we thus use an approximate */
+    /* external degree). */
+
+    /* The time taken by the following code (compute set differences, and */
+    /* add them up) is proportional to the size of the data structure */
+    /* being scanned - that is, the sum of the sizes of each column in */
+    /* the pivot row.  Thus, the amortized time to compute a column score */
+    /* is proportional to the size of that column (where size, in this */
+    /* context, is the column "length", or the number of row indices */
+    /* in that column).  The number of row indices in a column is */
+    /* monotonically non-decreasing, from the length of the original */
+    /* column on input to colamd. */
+
+    /* === Compute set differences ====================================== */
+
+    COLAMD_DEBUG3 (("** Computing set differences phase. **\n")) ;
+
+    /* pivot row is currently dead - it will be revived later. */
+
+    COLAMD_DEBUG3 (("Pivot row: ")) ;
+    /* for each column in pivot row */
+    rp = &A [pivot_row_start] ;
+    rp_end = rp + pivot_row_length ;
+    while (rp < rp_end)
+    {
+      col = *rp++ ;
+      COLAMD_ASSERT (Col[col].is_alive() && col != pivot_col) ;
+      COLAMD_DEBUG3 (("Col: %d\n", col)) ;
+
+      /* clear tags used to construct pivot row pattern */
+      col_thickness = -Col [col].shared1.thickness ;
+      COLAMD_ASSERT (col_thickness > 0) ;
+      Col [col].shared1.thickness = col_thickness ;
+
+      /* === Remove column from degree list =========================== */
+
+      cur_score = Col [col].shared2.score ;
+      prev_col = Col [col].shared3.prev ;
+      next_col = Col [col].shared4.degree_next ;
+      COLAMD_ASSERT (cur_score >= 0) ;
+      COLAMD_ASSERT (cur_score <= n_col) ;
+      COLAMD_ASSERT (cur_score >= Empty) ;
+      if (prev_col == Empty)
+      {
+	head [cur_score] = next_col ;
+      }
+      else
+      {
+	Col [prev_col].shared4.degree_next = next_col ;
+      }
+      if (next_col != Empty)
+      {
+	Col [next_col].shared3.prev = prev_col ;
+      }
+
+      /* === Scan the column ========================================== */
+
+      cp = &A [Col [col].start] ;
+      cp_end = cp + Col [col].length ;
+      while (cp < cp_end)
+      {
+	/* get a row */
+	row = *cp++ ;
+	/* skip if dead */
+	if (Row[row].is_dead())
+	{
+	  continue ;
+	}
+  row_mark = Row [row].shared2.mark ;
+	COLAMD_ASSERT (row != pivot_row) ;
+	set_difference = row_mark - tag_mark ;
+	/* check if the row has been seen yet */
+	if (set_difference < 0)
+	{
+	  COLAMD_ASSERT (Row [row].shared1.degree <= max_deg) ;
+	  set_difference = Row [row].shared1.degree ;
+	}
+	/* subtract column thickness from this row's set difference */
+	set_difference -= col_thickness ;
+	COLAMD_ASSERT (set_difference >= 0) ;
+	/* absorb this row if the set difference becomes zero */
+	if (set_difference == 0)
+	{
+	  COLAMD_DEBUG3 (("aggressive absorption. Row: %d\n", row)) ;
+	  Row[row].kill() ;
+	}
+	else
+	{
+	  /* save the new mark */
+	  Row [row].shared2.mark = set_difference + tag_mark ;
+	}
+      }
+    }
+
+
+    /* === Add up set differences for each column ======================= */
+
+    COLAMD_DEBUG3 (("** Adding set differences phase. **\n")) ;
+
+    /* for each column in pivot row */
+    rp = &A [pivot_row_start] ;
+    rp_end = rp + pivot_row_length ;
+    while (rp < rp_end)
+    {
+      /* get a column */
+      col = *rp++ ;
+      COLAMD_ASSERT (Col[col].is_alive() && col != pivot_col) ;
+      hash = 0 ;
+      cur_score = 0 ;
+      cp = &A [Col [col].start] ;
+      /* compact the column */
+      new_cp = cp ;
+      cp_end = cp + Col [col].length ;
+
+      COLAMD_DEBUG4 (("Adding set diffs for Col: %d.\n", col)) ;
+
+      while (cp < cp_end)
+      {
+	/* get a row */
+	row = *cp++ ;
+	COLAMD_ASSERT(row >= 0 && row < n_row) ;
+	/* skip if dead */
+	if (Row [row].is_dead())
+	{
+	  continue ;
+	}
+  row_mark = Row [row].shared2.mark ;
+	COLAMD_ASSERT (row_mark > tag_mark) ;
+	/* compact the column */
+	*new_cp++ = row ;
+	/* compute hash function */
+	hash += row ;
+	/* add set difference */
+	cur_score += row_mark - tag_mark ;
+	/* integer overflow... */
+	cur_score = numext::mini(cur_score, n_col) ;
+      }
+
+      /* recompute the column's length */
+      Col [col].length = (IndexType) (new_cp - &A [Col [col].start]) ;
+
+      /* === Further mass elimination ================================= */
+
+      if (Col [col].length == 0)
+      {
+	COLAMD_DEBUG4 (("further mass elimination. Col: %d\n", col)) ;
+	/* nothing left but the pivot row in this column */
+	Col[col].kill_principal() ;
+	pivot_row_degree -= Col [col].shared1.thickness ;
+	COLAMD_ASSERT (pivot_row_degree >= 0) ;
+	/* order it */
+	Col [col].shared2.order = k ;
+	/* increment order count by column thickness */
+	k += Col [col].shared1.thickness ;
+      }
+      else
+      {
+	/* === Prepare for supercolumn detection ==================== */
+
+	COLAMD_DEBUG4 (("Preparing supercol detection for Col: %d.\n", col)) ;
+
+	/* save score so far */
+	Col [col].shared2.score = cur_score ;
+
+	/* add column to hash table, for supercolumn detection */
+	hash %= n_col + 1 ;
+
+	COLAMD_DEBUG4 ((" Hash = %d, n_col = %d.\n", hash, n_col)) ;
+	COLAMD_ASSERT (hash <= n_col) ;
+
+	head_column = head [hash] ;
+	if (head_column > Empty)
+	{
+	  /* degree list "hash" is non-empty, use prev (shared3) of */
+	  /* first column in degree list as head of hash bucket */
+	  first_col = Col [head_column].shared3.headhash ;
+	  Col [head_column].shared3.headhash = col ;
+	}
+	else
+	{
+	  /* degree list "hash" is empty, use head as hash bucket */
+	  first_col = - (head_column + 2) ;
+	  head [hash] = - (col + 2) ;
+	}
+	Col [col].shared4.hash_next = first_col ;
+
+	/* save hash function in Col [col].shared3.hash */
+	Col [col].shared3.hash = (IndexType) hash ;
+	COLAMD_ASSERT (Col[col].is_alive()) ;
+      }
+    }
+
+    /* The approximate external column degree is now computed.  */
+
+    /* === Supercolumn detection ======================================== */
+
+    COLAMD_DEBUG3 (("** Supercolumn detection phase. **\n")) ;
+
+    Colamd::detect_super_cols (Col, A, head, pivot_row_start, pivot_row_length) ;
+
+    /* === Kill the pivotal column ====================================== */
+
+    Col[pivot_col].kill_principal() ;
+
+    /* === Clear mark =================================================== */
+
+    tag_mark += (max_deg + 1) ;
+    if (tag_mark >= max_mark)
+    {
+      COLAMD_DEBUG2 (("clearing tag_mark\n")) ;
+      tag_mark = Colamd::clear_mark (n_row, Row) ;
+    }
+
+    /* === Finalize the new pivot row, and column scores ================ */
+
+    COLAMD_DEBUG3 (("** Finalize scores phase. **\n")) ;
+
+    /* for each column in pivot row */
+    rp = &A [pivot_row_start] ;
+    /* compact the pivot row */
+    new_rp = rp ;
+    rp_end = rp + pivot_row_length ;
+    while (rp < rp_end)
+    {
+      col = *rp++ ;
+      /* skip dead columns */
+      if (Col[col].is_dead())
+      {
+	continue ;
+      }
+      *new_rp++ = col ;
+      /* add new pivot row to column */
+      A [Col [col].start + (Col [col].length++)] = pivot_row ;
+
+      /* retrieve score so far and add on pivot row's degree. */
+      /* (we wait until here for this in case the pivot */
+      /* row's degree was reduced due to mass elimination). */
+      cur_score = Col [col].shared2.score + pivot_row_degree ;
+
+      /* calculate the max possible score as the number of */
+      /* external columns minus the 'k' value minus the */
+      /* columns thickness */
+      max_score = n_col - k - Col [col].shared1.thickness ;
+
+      /* make the score the external degree of the union-of-rows */
+      cur_score -= Col [col].shared1.thickness ;
+
+      /* make sure score is less or equal than the max score */
+      cur_score = numext::mini(cur_score, max_score) ;
+      COLAMD_ASSERT (cur_score >= 0) ;
+
+      /* store updated score */
+      Col [col].shared2.score = cur_score ;
+
+      /* === Place column back in degree list ========================= */
+
+      COLAMD_ASSERT (min_score >= 0) ;
+      COLAMD_ASSERT (min_score <= n_col) ;
+      COLAMD_ASSERT (cur_score >= 0) ;
+      COLAMD_ASSERT (cur_score <= n_col) ;
+      COLAMD_ASSERT (head [cur_score] >= Empty) ;
+      next_col = head [cur_score] ;
+      Col [col].shared4.degree_next = next_col ;
+      Col [col].shared3.prev = Empty ;
+      if (next_col != Empty)
+      {
+	Col [next_col].shared3.prev = col ;
+      }
+      head [cur_score] = col ;
+
+      /* see if this score is less than current min */
+      min_score = numext::mini(min_score, cur_score) ;
+
+    }
+
+    /* === Resurrect the new pivot row ================================== */
+
+    if (pivot_row_degree > 0)
+    {
+      /* update pivot row length to reflect any cols that were killed */
+      /* during super-col detection and mass elimination */
+      Row [pivot_row].start  = pivot_row_start ;
+      Row [pivot_row].length = (IndexType) (new_rp - &A[pivot_row_start]) ;
+      Row [pivot_row].shared1.degree = pivot_row_degree ;
+      Row [pivot_row].shared2.mark = 0 ;
+      /* pivot row is no longer dead */
+    }
+  }
+
+  /* === All principal columns have now been ordered ====================== */
+
+  return (ngarbage) ;
+}
+
+
+/* ========================================================================== */
+/* === order_children ======================================================= */
+/* ========================================================================== */
+
+/*
+  The find_ordering routine has ordered all of the principal columns (the
+  representatives of the supercolumns).  The non-principal columns have not
+  yet been ordered.  This routine orders those columns by walking up the
+  parent tree (a column is a child of the column which absorbed it).  The
+  final permutation vector is then placed in p [0 ... n_col-1], with p [0]
+  being the first column, and p [n_col-1] being the last.  It doesn't look
+  like it at first glance, but be assured that this routine takes time linear
+  in the number of columns.  Although not immediately obvious, the time
+  taken by this routine is O (n_col), that is, linear in the number of
+  columns.  Not user-callable.
+*/
+template <typename IndexType>
+static inline  void order_children
+(
+  /* === Parameters ======================================================= */
+
+  IndexType n_col,      /* number of columns of A */
+  ColStructure<IndexType> Col [],    /* of size n_col+1 */
+  IndexType p []      /* p [0 ... n_col-1] is the column permutation*/
+  )
+{
+  /* === Local variables ================================================== */
+
+  IndexType i ;     /* loop counter for all columns */
+  IndexType c ;     /* column index */
+  IndexType parent ;    /* index of column's parent */
+  IndexType order ;     /* column's order */
+
+  /* === Order each non-principal column ================================== */
+
+  for (i = 0 ; i < n_col ; i++)
+  {
+    /* find an un-ordered non-principal column */
+    COLAMD_ASSERT (col_is_dead(Col, i)) ;
+    if (!Col[i].is_dead_principal() && Col [i].shared2.order == Empty)
+    {
+      parent = i ;
+      /* once found, find its principal parent */
+      do
+      {
+	parent = Col [parent].shared1.parent ;
+      } while (!Col[parent].is_dead_principal()) ;
+
+      /* now, order all un-ordered non-principal columns along path */
+      /* to this parent.  collapse tree at the same time */
+      c = i ;
+      /* get order of parent */
+      order = Col [parent].shared2.order ;
+
+      do
+      {
+	COLAMD_ASSERT (Col [c].shared2.order == Empty) ;
+
+	/* order this column */
+	Col [c].shared2.order = order++ ;
+	/* collaps tree */
+	Col [c].shared1.parent = parent ;
+
+	/* get immediate parent of this column */
+	c = Col [c].shared1.parent ;
+
+	/* continue until we hit an ordered column.  There are */
+	/* guaranteed not to be anymore unordered columns */
+	/* above an ordered column */
+      } while (Col [c].shared2.order == Empty) ;
+
+      /* re-order the super_col parent to largest order for this group */
+      Col [parent].shared2.order = order ;
+    }
+  }
+
+  /* === Generate the permutation ========================================= */
+
+  for (c = 0 ; c < n_col ; c++)
+  {
+    p [Col [c].shared2.order] = c ;
+  }
+}
+
+
+/* ========================================================================== */
+/* === detect_super_cols ==================================================== */
+/* ========================================================================== */
+
+/*
+  Detects supercolumns by finding matches between columns in the hash buckets.
+  Check amongst columns in the set A [row_start ... row_start + row_length-1].
+  The columns under consideration are currently *not* in the degree lists,
+  and have already been placed in the hash buckets.
+
+  The hash bucket for columns whose hash function is equal to h is stored
+  as follows:
+
+  if head [h] is >= 0, then head [h] contains a degree list, so:
+
+  head [h] is the first column in degree bucket h.
+  Col [head [h]].headhash gives the first column in hash bucket h.
+
+  otherwise, the degree list is empty, and:
+
+  -(head [h] + 2) is the first column in hash bucket h.
+
+  For a column c in a hash bucket, Col [c].shared3.prev is NOT a "previous
+  column" pointer.  Col [c].shared3.hash is used instead as the hash number
+  for that column.  The value of Col [c].shared4.hash_next is the next column
+  in the same hash bucket.
+
+  Assuming no, or "few" hash collisions, the time taken by this routine is
+  linear in the sum of the sizes (lengths) of each column whose score has
+  just been computed in the approximate degree computation.
+  Not user-callable.
+*/
+template <typename IndexType>
+static void detect_super_cols
+(
+  /* === Parameters ======================================================= */
+
+  ColStructure<IndexType> Col [],    /* of size n_col+1 */
+  IndexType A [],     /* row indices of A */
+  IndexType head [],    /* head of degree lists and hash buckets */
+  IndexType row_start,    /* pointer to set of columns to check */
+  IndexType row_length    /* number of columns to check */
+)
+{
+  /* === Local variables ================================================== */
+
+  IndexType hash ;      /* hash value for a column */
+  IndexType *rp ;     /* pointer to a row */
+  IndexType c ;     /* a column index */
+  IndexType super_c ;   /* column index of the column to absorb into */
+  IndexType *cp1 ;      /* column pointer for column super_c */
+  IndexType *cp2 ;      /* column pointer for column c */
+  IndexType length ;    /* length of column super_c */
+  IndexType prev_c ;    /* column preceding c in hash bucket */
+  IndexType i ;     /* loop counter */
+  IndexType *rp_end ;   /* pointer to the end of the row */
+  IndexType col ;     /* a column index in the row to check */
+  IndexType head_column ;   /* first column in hash bucket or degree list */
+  IndexType first_col ;   /* first column in hash bucket */
+
+  /* === Consider each column in the row ================================== */
+
+  rp = &A [row_start] ;
+  rp_end = rp + row_length ;
+  while (rp < rp_end)
+  {
+    col = *rp++ ;
+    if (Col[col].is_dead())
+    {
+      continue ;
+    }
+
+    /* get hash number for this column */
+    hash = Col [col].shared3.hash ;
+    COLAMD_ASSERT (hash <= n_col) ;
+
+    /* === Get the first column in this hash bucket ===================== */
+
+    head_column = head [hash] ;
+    if (head_column > Empty)
+    {
+      first_col = Col [head_column].shared3.headhash ;
+    }
+    else
+    {
+      first_col = - (head_column + 2) ;
+    }
+
+    /* === Consider each column in the hash bucket ====================== */
+
+    for (super_c = first_col ; super_c != Empty ;
+	 super_c = Col [super_c].shared4.hash_next)
+    {
+      COLAMD_ASSERT (Col [super_c].is_alive()) ;
+      COLAMD_ASSERT (Col [super_c].shared3.hash == hash) ;
+      length = Col [super_c].length ;
+
+      /* prev_c is the column preceding column c in the hash bucket */
+      prev_c = super_c ;
+
+      /* === Compare super_c with all columns after it ================ */
+
+      for (c = Col [super_c].shared4.hash_next ;
+	   c != Empty ; c = Col [c].shared4.hash_next)
+      {
+	COLAMD_ASSERT (c != super_c) ;
+	COLAMD_ASSERT (Col[c].is_alive()) ;
+	COLAMD_ASSERT (Col [c].shared3.hash == hash) ;
+
+	/* not identical if lengths or scores are different */
+	if (Col [c].length != length ||
+	    Col [c].shared2.score != Col [super_c].shared2.score)
+	{
+	  prev_c = c ;
+	  continue ;
+	}
+
+	/* compare the two columns */
+	cp1 = &A [Col [super_c].start] ;
+	cp2 = &A [Col [c].start] ;
+
+	for (i = 0 ; i < length ; i++)
+	{
+	  /* the columns are "clean" (no dead rows) */
+	  COLAMD_ASSERT ( cp1->is_alive() );
+	  COLAMD_ASSERT ( cp2->is_alive() );
+	  /* row indices will same order for both supercols, */
+	  /* no gather scatter necessary */
+	  if (*cp1++ != *cp2++)
+	  {
+	    break ;
+	  }
+	}
+
+	/* the two columns are different if the for-loop "broke" */
+	if (i != length)
+	{
+	  prev_c = c ;
+	  continue ;
+	}
+
+	/* === Got it!  two columns are identical =================== */
+
+	COLAMD_ASSERT (Col [c].shared2.score == Col [super_c].shared2.score) ;
+
+	Col [super_c].shared1.thickness += Col [c].shared1.thickness ;
+	Col [c].shared1.parent = super_c ;
+	Col[c].kill_non_principal() ;
+	/* order c later, in order_children() */
+	Col [c].shared2.order = Empty ;
+	/* remove c from hash bucket */
+	Col [prev_c].shared4.hash_next = Col [c].shared4.hash_next ;
+      }
+    }
+
+    /* === Empty this hash bucket ======================================= */
+
+    if (head_column > Empty)
+    {
+      /* corresponding degree list "hash" is not empty */
+      Col [head_column].shared3.headhash = Empty ;
+    }
+    else
+    {
+      /* corresponding degree list "hash" is empty */
+      head [hash] = Empty ;
+    }
+  }
+}
+
+
+/* ========================================================================== */
+/* === garbage_collection =================================================== */
+/* ========================================================================== */
+
+/*
+  Defragments and compacts columns and rows in the workspace A.  Used when
+  all available memory has been used while performing row merging.  Returns
+  the index of the first free position in A, after garbage collection.  The
+  time taken by this routine is linear is the size of the array A, which is
+  itself linear in the number of nonzeros in the input matrix.
+  Not user-callable.
+*/
+template <typename IndexType>
+static IndexType garbage_collection  /* returns the new value of pfree */
+  (
+    /* === Parameters ======================================================= */
+
+    IndexType n_row,      /* number of rows */
+    IndexType n_col,      /* number of columns */
+    RowStructure<IndexType> Row [],    /* row info */
+    ColStructure<IndexType> Col [],    /* column info */
+    IndexType A [],     /* A [0 ... Alen-1] holds the matrix */
+    IndexType *pfree      /* &A [0] ... pfree is in use */
+    )
+{
+  /* === Local variables ================================================== */
+
+  IndexType *psrc ;     /* source pointer */
+  IndexType *pdest ;    /* destination pointer */
+  IndexType j ;     /* counter */
+  IndexType r ;     /* a row index */
+  IndexType c ;     /* a column index */
+  IndexType length ;    /* length of a row or column */
+
+  /* === Defragment the columns =========================================== */
+
+  pdest = &A[0] ;
+  for (c = 0 ; c < n_col ; c++)
+  {
+    if (Col[c].is_alive())
+    {
+      psrc = &A [Col [c].start] ;
+
+      /* move and compact the column */
+      COLAMD_ASSERT (pdest <= psrc) ;
+      Col [c].start = (IndexType) (pdest - &A [0]) ;
+      length = Col [c].length ;
+      for (j = 0 ; j < length ; j++)
+      {
+	r = *psrc++ ;
+	if (Row[r].is_alive())
+	{
+	  *pdest++ = r ;
+	}
+      }
+      Col [c].length = (IndexType) (pdest - &A [Col [c].start]) ;
+    }
+  }
+
+  /* === Prepare to defragment the rows =================================== */
+
+  for (r = 0 ; r < n_row ; r++)
+  {
+    if (Row[r].is_alive())
+    {
+      if (Row [r].length == 0)
+      {
+        /* this row is of zero length.  cannot compact it, so kill it */
+        COLAMD_DEBUG3 (("Defrag row kill\n")) ;
+        Row[r].kill() ;
+      }
+      else
+      {
+        /* save first column index in Row [r].shared2.first_column */
+        psrc = &A [Row [r].start] ;
+        Row [r].shared2.first_column = *psrc ;
+        COLAMD_ASSERT (Row[r].is_alive()) ;
+        /* flag the start of the row with the one's complement of row */
+        *psrc = ones_complement(r) ;
+
+      }
+    }
+  }
+
+  /* === Defragment the rows ============================================== */
+
+  psrc = pdest ;
+  while (psrc < pfree)
+  {
+    /* find a negative number ... the start of a row */
+    if (*psrc++ < 0)
+    {
+      psrc-- ;
+      /* get the row index */
+      r = ones_complement(*psrc) ;
+      COLAMD_ASSERT (r >= 0 && r < n_row) ;
+      /* restore first column index */
+      *psrc = Row [r].shared2.first_column ;
+      COLAMD_ASSERT (Row[r].is_alive()) ;
+
+      /* move and compact the row */
+      COLAMD_ASSERT (pdest <= psrc) ;
+      Row [r].start = (IndexType) (pdest - &A [0]) ;
+      length = Row [r].length ;
+      for (j = 0 ; j < length ; j++)
+      {
+	c = *psrc++ ;
+	if (Col[c].is_alive())
+	{
+	  *pdest++ = c ;
+	}
+      }
+      Row [r].length = (IndexType) (pdest - &A [Row [r].start]) ;
+
+    }
+  }
+  /* ensure we found all the rows */
+  COLAMD_ASSERT (debug_rows == 0) ;
+
+  /* === Return the new value of pfree ==================================== */
+
+  return ((IndexType) (pdest - &A [0])) ;
+}
+
+
+/* ========================================================================== */
+/* === clear_mark =========================================================== */
+/* ========================================================================== */
+
+/*
+  Clears the Row [].shared2.mark array, and returns the new tag_mark.
+  Return value is the new tag_mark.  Not user-callable.
+*/
+template <typename IndexType>
+static inline  IndexType clear_mark  /* return the new value for tag_mark */
+  (
+      /* === Parameters ======================================================= */
+
+    IndexType n_row,    /* number of rows in A */
+    RowStructure<IndexType> Row [] /* Row [0 ... n_row-1].shared2.mark is set to zero */
+    )
+{
+  /* === Local variables ================================================== */
+
+  IndexType r ;
+
+  for (r = 0 ; r < n_row ; r++)
+  {
+    if (Row[r].is_alive())
+    {
+      Row [r].shared2.mark = 0 ;
+    }
+  }
+  return (1) ;
+}
+
+} // namespace Colamd
+
+} // namespace internal
+#endif
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h
new file mode 100644
index 0000000..c578970
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/OrderingMethods/Ordering.h
@@ -0,0 +1,153 @@
+ 
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012  Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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_ORDERING_H
+#define EIGEN_ORDERING_H
+
+namespace Eigen {
+  
+#include "Eigen_Colamd.h"
+
+namespace internal {
+    
+/** \internal
+  * \ingroup OrderingMethods_Module
+  * \param[in] A the input non-symmetric matrix
+  * \param[out] symmat the symmetric pattern A^T+A from the input matrix \a A.
+  * FIXME: The values should not be considered here
+  */
+template<typename MatrixType> 
+void ordering_helper_at_plus_a(const MatrixType& A, MatrixType& symmat)
+{
+  MatrixType C;
+  C = A.transpose(); // NOTE: Could be  costly
+  for (int i = 0; i < C.rows(); i++) 
+  {
+      for (typename MatrixType::InnerIterator it(C, i); it; ++it)
+        it.valueRef() = typename MatrixType::Scalar(0);
+  }
+  symmat = C + A;
+}
+    
+}
+
+/** \ingroup OrderingMethods_Module
+  * \class AMDOrdering
+  *
+  * Functor computing the \em approximate \em minimum \em degree ordering
+  * If the matrix is not structurally symmetric, an ordering of A^T+A is computed
+  * \tparam  StorageIndex The type of indices of the matrix 
+  * \sa COLAMDOrdering
+  */
+template <typename StorageIndex>
+class AMDOrdering
+{
+  public:
+    typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
+    
+    /** Compute the permutation vector from a sparse matrix
+     * This routine is much faster if the input matrix is column-major     
+     */
+    template <typename MatrixType>
+    void operator()(const MatrixType& mat, PermutationType& perm)
+    {
+      // Compute the symmetric pattern
+      SparseMatrix<typename MatrixType::Scalar, ColMajor, StorageIndex> symm;
+      internal::ordering_helper_at_plus_a(mat,symm); 
+    
+      // Call the AMD routine 
+      //m_mat.prune(keep_diag());
+      internal::minimum_degree_ordering(symm, perm);
+    }
+    
+    /** Compute the permutation with a selfadjoint matrix */
+    template <typename SrcType, unsigned int SrcUpLo> 
+    void operator()(const SparseSelfAdjointView<SrcType, SrcUpLo>& mat, PermutationType& perm)
+    { 
+      SparseMatrix<typename SrcType::Scalar, ColMajor, StorageIndex> C; C = mat;
+      
+      // Call the AMD routine 
+      // m_mat.prune(keep_diag()); //Remove the diagonal elements 
+      internal::minimum_degree_ordering(C, perm);
+    }
+};
+
+/** \ingroup OrderingMethods_Module
+  * \class NaturalOrdering
+  *
+  * Functor computing the natural ordering (identity)
+  * 
+  * \note Returns an empty permutation matrix
+  * \tparam  StorageIndex The type of indices of the matrix 
+  */
+template <typename StorageIndex>
+class NaturalOrdering
+{
+  public:
+    typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
+    
+    /** Compute the permutation vector from a column-major sparse matrix */
+    template <typename MatrixType>
+    void operator()(const MatrixType& /*mat*/, PermutationType& perm)
+    {
+      perm.resize(0); 
+    }
+    
+};
+
+/** \ingroup OrderingMethods_Module
+  * \class COLAMDOrdering
+  *
+  * \tparam  StorageIndex The type of indices of the matrix 
+  * 
+  * Functor computing the \em column \em approximate \em minimum \em degree ordering 
+  * The matrix should be in column-major and \b compressed format (see SparseMatrix::makeCompressed()).
+  */
+template<typename StorageIndex>
+class COLAMDOrdering
+{
+  public:
+    typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType; 
+    typedef Matrix<StorageIndex, Dynamic, 1> IndexVector;
+    
+    /** Compute the permutation vector \a perm form the sparse matrix \a mat
+      * \warning The input sparse matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
+      */
+    template <typename MatrixType>
+    void operator() (const MatrixType& mat, PermutationType& perm)
+    {
+      eigen_assert(mat.isCompressed() && "COLAMDOrdering requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to COLAMDOrdering");
+      
+      StorageIndex m = StorageIndex(mat.rows());
+      StorageIndex n = StorageIndex(mat.cols());
+      StorageIndex nnz = StorageIndex(mat.nonZeros());
+      // Get the recommended value of Alen to be used by colamd
+      StorageIndex Alen = internal::Colamd::recommended(nnz, m, n); 
+      // Set the default parameters
+      double knobs [internal::Colamd::NKnobs]; 
+      StorageIndex stats [internal::Colamd::NStats];
+      internal::Colamd::set_defaults(knobs);
+      
+      IndexVector p(n+1), A(Alen); 
+      for(StorageIndex i=0; i <= n; i++)   p(i) = mat.outerIndexPtr()[i];
+      for(StorageIndex i=0; i < nnz; i++)  A(i) = mat.innerIndexPtr()[i];
+      // Call Colamd routine to compute the ordering 
+      StorageIndex info = internal::Colamd::compute_ordering(m, n, Alen, A.data(), p.data(), knobs, stats); 
+      EIGEN_UNUSED_VARIABLE(info);
+      eigen_assert( info && "COLAMD failed " );
+      
+      perm.resize(n);
+      for (StorageIndex i = 0; i < n; i++) perm.indices()(p(i)) = i;
+    }
+};
+
+} // end namespace Eigen
+
+#endif
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/QR/ColPivHouseholderQR.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/ColPivHouseholderQR.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/QR/ColPivHouseholderQR.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/ColPivHouseholderQR.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/QR/CompleteOrthogonalDecomposition.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/CompleteOrthogonalDecomposition.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/QR/CompleteOrthogonalDecomposition.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/CompleteOrthogonalDecomposition.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/QR/FullPivHouseholderQR.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/FullPivHouseholderQR.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/QR/FullPivHouseholderQR.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/FullPivHouseholderQR.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/QR/HouseholderQR.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/HouseholderQR.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/QR/HouseholderQR.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/QR/HouseholderQR.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/BDCSVD.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/SVD/BDCSVD.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/JacobiSVD.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/SVD/JacobiSVD.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/SVDBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/SVDBase.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/SVD/SVDBase.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/SVDBase.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/UpperBidiagonalization.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/UpperBidiagonalization.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/SVD/UpperBidiagonalization.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/UpperBidiagonalization.h
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h
new file mode 100644
index 0000000..9f93e32
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky.h
@@ -0,0 +1,697 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SIMPLICIAL_CHOLESKY_H
+#define EIGEN_SIMPLICIAL_CHOLESKY_H
+
+namespace Eigen { 
+
+enum SimplicialCholeskyMode {
+  SimplicialCholeskyLLT,
+  SimplicialCholeskyLDLT
+};
+
+namespace internal {
+  template<typename CholMatrixType, typename InputMatrixType>
+  struct simplicial_cholesky_grab_input {
+    typedef CholMatrixType const * ConstCholMatrixPtr;
+    static void run(const InputMatrixType& input, ConstCholMatrixPtr &pmat, CholMatrixType &tmp)
+    {
+      tmp = input;
+      pmat = &tmp;
+    }
+  };
+  
+  template<typename MatrixType>
+  struct simplicial_cholesky_grab_input<MatrixType,MatrixType> {
+    typedef MatrixType const * ConstMatrixPtr;
+    static void run(const MatrixType& input, ConstMatrixPtr &pmat, MatrixType &/*tmp*/)
+    {
+      pmat = &input;
+    }
+  };
+} // end namespace internal
+
+/** \ingroup SparseCholesky_Module
+  * \brief A base class for direct sparse Cholesky factorizations
+  *
+  * This is a base class for LL^T and LDL^T Cholesky factorizations of sparse matrices that are
+  * selfadjoint and positive definite. These factorizations allow for solving A.X = B where
+  * X and B can be either dense or sparse.
+  * 
+  * In order to reduce the fill-in, a symmetric permutation P is applied prior to the factorization
+  * such that the factorized matrix is P A P^-1.
+  *
+  * \tparam Derived the type of the derived class, that is the actual factorization type.
+  *
+  */
+template<typename Derived>
+class SimplicialCholeskyBase : public SparseSolverBase<Derived>
+{
+    typedef SparseSolverBase<Derived> Base;
+    using Base::m_isInitialized;
+    
+  public:
+    typedef typename internal::traits<Derived>::MatrixType MatrixType;
+    typedef typename internal::traits<Derived>::OrderingType OrderingType;
+    enum { UpLo = internal::traits<Derived>::UpLo };
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename MatrixType::RealScalar RealScalar;
+    typedef typename MatrixType::StorageIndex StorageIndex;
+    typedef SparseMatrix<Scalar,ColMajor,StorageIndex> CholMatrixType;
+    typedef CholMatrixType const * ConstCholMatrixPtr;
+    typedef Matrix<Scalar,Dynamic,1> VectorType;
+    typedef Matrix<StorageIndex,Dynamic,1> VectorI;
+
+    enum {
+      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+    };
+
+  public:
+    
+    using Base::derived;
+
+    /** Default constructor */
+    SimplicialCholeskyBase()
+      : m_info(Success),
+        m_factorizationIsOk(false),
+        m_analysisIsOk(false),
+        m_shiftOffset(0),
+        m_shiftScale(1)
+    {}
+
+    explicit SimplicialCholeskyBase(const MatrixType& matrix)
+      : m_info(Success),
+        m_factorizationIsOk(false),
+        m_analysisIsOk(false),
+        m_shiftOffset(0),
+        m_shiftScale(1)
+    {
+      derived().compute(matrix);
+    }
+
+    ~SimplicialCholeskyBase()
+    {
+    }
+
+    Derived& derived() { return *static_cast<Derived*>(this); }
+    const Derived& derived() const { return *static_cast<const Derived*>(this); }
+    
+    inline Index cols() const { return m_matrix.cols(); }
+    inline Index rows() const { return m_matrix.rows(); }
+    
+    /** \brief Reports whether previous computation was successful.
+      *
+      * \returns \c Success if computation was successful,
+      *          \c NumericalIssue if the matrix.appears to be negative.
+      */
+    ComputationInfo info() const
+    {
+      eigen_assert(m_isInitialized && "Decomposition is not initialized.");
+      return m_info;
+    }
+    
+    /** \returns the permutation P
+      * \sa permutationPinv() */
+    const PermutationMatrix<Dynamic,Dynamic,StorageIndex>& permutationP() const
+    { return m_P; }
+    
+    /** \returns the inverse P^-1 of the permutation P
+      * \sa permutationP() */
+    const PermutationMatrix<Dynamic,Dynamic,StorageIndex>& permutationPinv() const
+    { return m_Pinv; }
+
+    /** Sets the shift parameters that will be used to adjust the diagonal coefficients during the numerical factorization.
+      *
+      * During the numerical factorization, the diagonal coefficients are transformed by the following linear model:\n
+      * \c d_ii = \a offset + \a scale * \c d_ii
+      *
+      * The default is the identity transformation with \a offset=0, and \a scale=1.
+      *
+      * \returns a reference to \c *this.
+      */
+    Derived& setShift(const RealScalar& offset, const RealScalar& scale = 1)
+    {
+      m_shiftOffset = offset;
+      m_shiftScale = scale;
+      return derived();
+    }
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** \internal */
+    template<typename Stream>
+    void dumpMemory(Stream& s)
+    {
+      int total = 0;
+      s << "  L:        " << ((total+=(m_matrix.cols()+1) * sizeof(int) + m_matrix.nonZeros()*(sizeof(int)+sizeof(Scalar))) >> 20) << "Mb" << "\n";
+      s << "  diag:     " << ((total+=m_diag.size() * sizeof(Scalar)) >> 20) << "Mb" << "\n";
+      s << "  tree:     " << ((total+=m_parent.size() * sizeof(int)) >> 20) << "Mb" << "\n";
+      s << "  nonzeros: " << ((total+=m_nonZerosPerCol.size() * sizeof(int)) >> 20) << "Mb" << "\n";
+      s << "  perm:     " << ((total+=m_P.size() * sizeof(int)) >> 20) << "Mb" << "\n";
+      s << "  perm^-1:  " << ((total+=m_Pinv.size() * sizeof(int)) >> 20) << "Mb" << "\n";
+      s << "  TOTAL:    " << (total>> 20) << "Mb" << "\n";
+    }
+
+    /** \internal */
+    template<typename Rhs,typename Dest>
+    void _solve_impl(const MatrixBase<Rhs> &b, MatrixBase<Dest> &dest) const
+    {
+      eigen_assert(m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()");
+      eigen_assert(m_matrix.rows()==b.rows());
+
+      if(m_info!=Success)
+        return;
+
+      if(m_P.size()>0)
+        dest = m_P * b;
+      else
+        dest = b;
+
+      if(m_matrix.nonZeros()>0) // otherwise L==I
+        derived().matrixL().solveInPlace(dest);
+
+      if(m_diag.size()>0)
+        dest = m_diag.asDiagonal().inverse() * dest;
+
+      if (m_matrix.nonZeros()>0) // otherwise U==I
+        derived().matrixU().solveInPlace(dest);
+
+      if(m_P.size()>0)
+        dest = m_Pinv * dest;
+    }
+    
+    template<typename Rhs,typename Dest>
+    void _solve_impl(const SparseMatrixBase<Rhs> &b, SparseMatrixBase<Dest> &dest) const
+    {
+      internal::solve_sparse_through_dense_panels(derived(), b, dest);
+    }
+
+#endif // EIGEN_PARSED_BY_DOXYGEN
+
+  protected:
+    
+    /** Computes the sparse Cholesky decomposition of \a matrix */
+    template<bool DoLDLT>
+    void compute(const MatrixType& matrix)
+    {
+      eigen_assert(matrix.rows()==matrix.cols());
+      Index size = matrix.cols();
+      CholMatrixType tmp(size,size);
+      ConstCholMatrixPtr pmat;
+      ordering(matrix, pmat, tmp);
+      analyzePattern_preordered(*pmat, DoLDLT);
+      factorize_preordered<DoLDLT>(*pmat);
+    }
+    
+    template<bool DoLDLT>
+    void factorize(const MatrixType& a)
+    {
+      eigen_assert(a.rows()==a.cols());
+      Index size = a.cols();
+      CholMatrixType tmp(size,size);
+      ConstCholMatrixPtr pmat;
+      
+      if(m_P.size() == 0 && (int(UpLo) & int(Upper)) == Upper)
+      {
+        // If there is no ordering, try to directly use the input matrix without any copy
+        internal::simplicial_cholesky_grab_input<CholMatrixType,MatrixType>::run(a, pmat, tmp);
+      }
+      else
+      {
+        tmp.template selfadjointView<Upper>() = a.template selfadjointView<UpLo>().twistedBy(m_P);
+        pmat = &tmp;
+      }
+      
+      factorize_preordered<DoLDLT>(*pmat);
+    }
+
+    template<bool DoLDLT>
+    void factorize_preordered(const CholMatrixType& a);
+
+    void analyzePattern(const MatrixType& a, bool doLDLT)
+    {
+      eigen_assert(a.rows()==a.cols());
+      Index size = a.cols();
+      CholMatrixType tmp(size,size);
+      ConstCholMatrixPtr pmat;
+      ordering(a, pmat, tmp);
+      analyzePattern_preordered(*pmat,doLDLT);
+    }
+    void analyzePattern_preordered(const CholMatrixType& a, bool doLDLT);
+    
+    void ordering(const MatrixType& a, ConstCholMatrixPtr &pmat, CholMatrixType& ap);
+
+    /** keeps off-diagonal entries; drops diagonal entries */
+    struct keep_diag {
+      inline bool operator() (const Index& row, const Index& col, const Scalar&) const
+      {
+        return row!=col;
+      }
+    };
+
+    mutable ComputationInfo m_info;
+    bool m_factorizationIsOk;
+    bool m_analysisIsOk;
+    
+    CholMatrixType m_matrix;
+    VectorType m_diag;                                // the diagonal coefficients (LDLT mode)
+    VectorI m_parent;                                 // elimination tree
+    VectorI m_nonZerosPerCol;
+    PermutationMatrix<Dynamic,Dynamic,StorageIndex> m_P;     // the permutation
+    PermutationMatrix<Dynamic,Dynamic,StorageIndex> m_Pinv;  // the inverse permutation
+
+    RealScalar m_shiftOffset;
+    RealScalar m_shiftScale;
+};
+
+template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::StorageIndex> > class SimplicialLLT;
+template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::StorageIndex> > class SimplicialLDLT;
+template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::StorageIndex> > class SimplicialCholesky;
+
+namespace internal {
+
+template<typename _MatrixType, int _UpLo, typename _Ordering> struct traits<SimplicialLLT<_MatrixType,_UpLo,_Ordering> >
+{
+  typedef _MatrixType MatrixType;
+  typedef _Ordering OrderingType;
+  enum { UpLo = _UpLo };
+  typedef typename MatrixType::Scalar                         Scalar;
+  typedef typename MatrixType::StorageIndex                   StorageIndex;
+  typedef SparseMatrix<Scalar, ColMajor, StorageIndex>        CholMatrixType;
+  typedef TriangularView<const CholMatrixType, Eigen::Lower>  MatrixL;
+  typedef TriangularView<const typename CholMatrixType::AdjointReturnType, Eigen::Upper>   MatrixU;
+  static inline MatrixL getL(const CholMatrixType& m) { return MatrixL(m); }
+  static inline MatrixU getU(const CholMatrixType& m) { return MatrixU(m.adjoint()); }
+};
+
+template<typename _MatrixType,int _UpLo, typename _Ordering> struct traits<SimplicialLDLT<_MatrixType,_UpLo,_Ordering> >
+{
+  typedef _MatrixType MatrixType;
+  typedef _Ordering OrderingType;
+  enum { UpLo = _UpLo };
+  typedef typename MatrixType::Scalar                             Scalar;
+  typedef typename MatrixType::StorageIndex                       StorageIndex;
+  typedef SparseMatrix<Scalar, ColMajor, StorageIndex>            CholMatrixType;
+  typedef TriangularView<const CholMatrixType, Eigen::UnitLower>  MatrixL;
+  typedef TriangularView<const typename CholMatrixType::AdjointReturnType, Eigen::UnitUpper> MatrixU;
+  static inline MatrixL getL(const CholMatrixType& m) { return MatrixL(m); }
+  static inline MatrixU getU(const CholMatrixType& m) { return MatrixU(m.adjoint()); }
+};
+
+template<typename _MatrixType, int _UpLo, typename _Ordering> struct traits<SimplicialCholesky<_MatrixType,_UpLo,_Ordering> >
+{
+  typedef _MatrixType MatrixType;
+  typedef _Ordering OrderingType;
+  enum { UpLo = _UpLo };
+};
+
+}
+
+/** \ingroup SparseCholesky_Module
+  * \class SimplicialLLT
+  * \brief A direct sparse LLT Cholesky factorizations
+  *
+  * This class provides a LL^T Cholesky factorizations of sparse matrices that are
+  * selfadjoint and positive definite. The factorization allows for solving A.X = B where
+  * X and B can be either dense or sparse.
+  * 
+  * In order to reduce the fill-in, a symmetric permutation P is applied prior to the factorization
+  * such that the factorized matrix is P A P^-1.
+  *
+  * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
+  * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
+  *               or Upper. Default is Lower.
+  * \tparam _Ordering The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<>
+  *
+  * \implsparsesolverconcept
+  *
+  * \sa class SimplicialLDLT, class AMDOrdering, class NaturalOrdering
+  */
+template<typename _MatrixType, int _UpLo, typename _Ordering>
+    class SimplicialLLT : public SimplicialCholeskyBase<SimplicialLLT<_MatrixType,_UpLo,_Ordering> >
+{
+public:
+    typedef _MatrixType MatrixType;
+    enum { UpLo = _UpLo };
+    typedef SimplicialCholeskyBase<SimplicialLLT> Base;
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename MatrixType::RealScalar RealScalar;
+    typedef typename MatrixType::StorageIndex StorageIndex;
+    typedef SparseMatrix<Scalar,ColMajor,Index> CholMatrixType;
+    typedef Matrix<Scalar,Dynamic,1> VectorType;
+    typedef internal::traits<SimplicialLLT> Traits;
+    typedef typename Traits::MatrixL  MatrixL;
+    typedef typename Traits::MatrixU  MatrixU;
+public:
+    /** Default constructor */
+    SimplicialLLT() : Base() {}
+    /** Constructs and performs the LLT factorization of \a matrix */
+    explicit SimplicialLLT(const MatrixType& matrix)
+        : Base(matrix) {}
+
+    /** \returns an expression of the factor L */
+    inline const MatrixL matrixL() const {
+        eigen_assert(Base::m_factorizationIsOk && "Simplicial LLT not factorized");
+        return Traits::getL(Base::m_matrix);
+    }
+
+    /** \returns an expression of the factor U (= L^*) */
+    inline const MatrixU matrixU() const {
+        eigen_assert(Base::m_factorizationIsOk && "Simplicial LLT not factorized");
+        return Traits::getU(Base::m_matrix);
+    }
+    
+    /** Computes the sparse Cholesky decomposition of \a matrix */
+    SimplicialLLT& compute(const MatrixType& matrix)
+    {
+      Base::template compute<false>(matrix);
+      return *this;
+    }
+
+    /** Performs a symbolic decomposition on the sparcity of \a matrix.
+      *
+      * This function is particularly useful when solving for several problems having the same structure.
+      *
+      * \sa factorize()
+      */
+    void analyzePattern(const MatrixType& a)
+    {
+      Base::analyzePattern(a, false);
+    }
+
+    /** Performs a numeric decomposition of \a matrix
+      *
+      * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed.
+      *
+      * \sa analyzePattern()
+      */
+    void factorize(const MatrixType& a)
+    {
+      Base::template factorize<false>(a);
+    }
+
+    /** \returns the determinant of the underlying matrix from the current factorization */
+    Scalar determinant() const
+    {
+      Scalar detL = Base::m_matrix.diagonal().prod();
+      return numext::abs2(detL);
+    }
+};
+
+/** \ingroup SparseCholesky_Module
+  * \class SimplicialLDLT
+  * \brief A direct sparse LDLT Cholesky factorizations without square root.
+  *
+  * This class provides a LDL^T Cholesky factorizations without square root of sparse matrices that are
+  * selfadjoint and positive definite. The factorization allows for solving A.X = B where
+  * X and B can be either dense or sparse.
+  * 
+  * In order to reduce the fill-in, a symmetric permutation P is applied prior to the factorization
+  * such that the factorized matrix is P A P^-1.
+  *
+  * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
+  * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
+  *               or Upper. Default is Lower.
+  * \tparam _Ordering The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<>
+  *
+  * \implsparsesolverconcept
+  *
+  * \sa class SimplicialLLT, class AMDOrdering, class NaturalOrdering
+  */
+template<typename _MatrixType, int _UpLo, typename _Ordering>
+    class SimplicialLDLT : public SimplicialCholeskyBase<SimplicialLDLT<_MatrixType,_UpLo,_Ordering> >
+{
+public:
+    typedef _MatrixType MatrixType;
+    enum { UpLo = _UpLo };
+    typedef SimplicialCholeskyBase<SimplicialLDLT> Base;
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename MatrixType::RealScalar RealScalar;
+    typedef typename MatrixType::StorageIndex StorageIndex;
+    typedef SparseMatrix<Scalar,ColMajor,StorageIndex> CholMatrixType;
+    typedef Matrix<Scalar,Dynamic,1> VectorType;
+    typedef internal::traits<SimplicialLDLT> Traits;
+    typedef typename Traits::MatrixL  MatrixL;
+    typedef typename Traits::MatrixU  MatrixU;
+public:
+    /** Default constructor */
+    SimplicialLDLT() : Base() {}
+
+    /** Constructs and performs the LLT factorization of \a matrix */
+    explicit SimplicialLDLT(const MatrixType& matrix)
+        : Base(matrix) {}
+
+    /** \returns a vector expression of the diagonal D */
+    inline const VectorType vectorD() const {
+        eigen_assert(Base::m_factorizationIsOk && "Simplicial LDLT not factorized");
+        return Base::m_diag;
+    }
+    /** \returns an expression of the factor L */
+    inline const MatrixL matrixL() const {
+        eigen_assert(Base::m_factorizationIsOk && "Simplicial LDLT not factorized");
+        return Traits::getL(Base::m_matrix);
+    }
+
+    /** \returns an expression of the factor U (= L^*) */
+    inline const MatrixU matrixU() const {
+        eigen_assert(Base::m_factorizationIsOk && "Simplicial LDLT not factorized");
+        return Traits::getU(Base::m_matrix);
+    }
+
+    /** Computes the sparse Cholesky decomposition of \a matrix */
+    SimplicialLDLT& compute(const MatrixType& matrix)
+    {
+      Base::template compute<true>(matrix);
+      return *this;
+    }
+    
+    /** Performs a symbolic decomposition on the sparcity of \a matrix.
+      *
+      * This function is particularly useful when solving for several problems having the same structure.
+      *
+      * \sa factorize()
+      */
+    void analyzePattern(const MatrixType& a)
+    {
+      Base::analyzePattern(a, true);
+    }
+
+    /** Performs a numeric decomposition of \a matrix
+      *
+      * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed.
+      *
+      * \sa analyzePattern()
+      */
+    void factorize(const MatrixType& a)
+    {
+      Base::template factorize<true>(a);
+    }
+
+    /** \returns the determinant of the underlying matrix from the current factorization */
+    Scalar determinant() const
+    {
+      return Base::m_diag.prod();
+    }
+};
+
+/** \deprecated use SimplicialLDLT or class SimplicialLLT
+  * \ingroup SparseCholesky_Module
+  * \class SimplicialCholesky
+  *
+  * \sa class SimplicialLDLT, class SimplicialLLT
+  */
+template<typename _MatrixType, int _UpLo, typename _Ordering>
+    class SimplicialCholesky : public SimplicialCholeskyBase<SimplicialCholesky<_MatrixType,_UpLo,_Ordering> >
+{
+public:
+    typedef _MatrixType MatrixType;
+    enum { UpLo = _UpLo };
+    typedef SimplicialCholeskyBase<SimplicialCholesky> Base;
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename MatrixType::RealScalar RealScalar;
+    typedef typename MatrixType::StorageIndex StorageIndex;
+    typedef SparseMatrix<Scalar,ColMajor,StorageIndex> CholMatrixType;
+    typedef Matrix<Scalar,Dynamic,1> VectorType;
+    typedef internal::traits<SimplicialCholesky> Traits;
+    typedef internal::traits<SimplicialLDLT<MatrixType,UpLo> > LDLTTraits;
+    typedef internal::traits<SimplicialLLT<MatrixType,UpLo>  > LLTTraits;
+  public:
+    SimplicialCholesky() : Base(), m_LDLT(true) {}
+
+    explicit SimplicialCholesky(const MatrixType& matrix)
+      : Base(), m_LDLT(true)
+    {
+      compute(matrix);
+    }
+
+    SimplicialCholesky& setMode(SimplicialCholeskyMode mode)
+    {
+      switch(mode)
+      {
+      case SimplicialCholeskyLLT:
+        m_LDLT = false;
+        break;
+      case SimplicialCholeskyLDLT:
+        m_LDLT = true;
+        break;
+      default:
+        break;
+      }
+
+      return *this;
+    }
+
+    inline const VectorType vectorD() const {
+        eigen_assert(Base::m_factorizationIsOk && "Simplicial Cholesky not factorized");
+        return Base::m_diag;
+    }
+    inline const CholMatrixType rawMatrix() const {
+        eigen_assert(Base::m_factorizationIsOk && "Simplicial Cholesky not factorized");
+        return Base::m_matrix;
+    }
+    
+    /** Computes the sparse Cholesky decomposition of \a matrix */
+    SimplicialCholesky& compute(const MatrixType& matrix)
+    {
+      if(m_LDLT)
+        Base::template compute<true>(matrix);
+      else
+        Base::template compute<false>(matrix);
+      return *this;
+    }
+
+    /** Performs a symbolic decomposition on the sparcity of \a matrix.
+      *
+      * This function is particularly useful when solving for several problems having the same structure.
+      *
+      * \sa factorize()
+      */
+    void analyzePattern(const MatrixType& a)
+    {
+      Base::analyzePattern(a, m_LDLT);
+    }
+
+    /** Performs a numeric decomposition of \a matrix
+      *
+      * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed.
+      *
+      * \sa analyzePattern()
+      */
+    void factorize(const MatrixType& a)
+    {
+      if(m_LDLT)
+        Base::template factorize<true>(a);
+      else
+        Base::template factorize<false>(a);
+    }
+
+    /** \internal */
+    template<typename Rhs,typename Dest>
+    void _solve_impl(const MatrixBase<Rhs> &b, MatrixBase<Dest> &dest) const
+    {
+      eigen_assert(Base::m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()");
+      eigen_assert(Base::m_matrix.rows()==b.rows());
+
+      if(Base::m_info!=Success)
+        return;
+
+      if(Base::m_P.size()>0)
+        dest = Base::m_P * b;
+      else
+        dest = b;
+
+      if(Base::m_matrix.nonZeros()>0) // otherwise L==I
+      {
+        if(m_LDLT)
+          LDLTTraits::getL(Base::m_matrix).solveInPlace(dest);
+        else
+          LLTTraits::getL(Base::m_matrix).solveInPlace(dest);
+      }
+
+      if(Base::m_diag.size()>0)
+        dest = Base::m_diag.real().asDiagonal().inverse() * dest;
+
+      if (Base::m_matrix.nonZeros()>0) // otherwise I==I
+      {
+        if(m_LDLT)
+          LDLTTraits::getU(Base::m_matrix).solveInPlace(dest);
+        else
+          LLTTraits::getU(Base::m_matrix).solveInPlace(dest);
+      }
+
+      if(Base::m_P.size()>0)
+        dest = Base::m_Pinv * dest;
+    }
+    
+    /** \internal */
+    template<typename Rhs,typename Dest>
+    void _solve_impl(const SparseMatrixBase<Rhs> &b, SparseMatrixBase<Dest> &dest) const
+    {
+      internal::solve_sparse_through_dense_panels(*this, b, dest);
+    }
+    
+    Scalar determinant() const
+    {
+      if(m_LDLT)
+      {
+        return Base::m_diag.prod();
+      }
+      else
+      {
+        Scalar detL = Diagonal<const CholMatrixType>(Base::m_matrix).prod();
+        return numext::abs2(detL);
+      }
+    }
+    
+  protected:
+    bool m_LDLT;
+};
+
+template<typename Derived>
+void SimplicialCholeskyBase<Derived>::ordering(const MatrixType& a, ConstCholMatrixPtr &pmat, CholMatrixType& ap)
+{
+  eigen_assert(a.rows()==a.cols());
+  const Index size = a.rows();
+  pmat = &ap;
+  // Note that ordering methods compute the inverse permutation
+  if(!internal::is_same<OrderingType,NaturalOrdering<Index> >::value)
+  {
+    {
+      CholMatrixType C;
+      C = a.template selfadjointView<UpLo>();
+      
+      OrderingType ordering;
+      ordering(C,m_Pinv);
+    }
+
+    if(m_Pinv.size()>0) m_P = m_Pinv.inverse();
+    else                m_P.resize(0);
+    
+    ap.resize(size,size);
+    ap.template selfadjointView<Upper>() = a.template selfadjointView<UpLo>().twistedBy(m_P);
+  }
+  else
+  {
+    m_Pinv.resize(0);
+    m_P.resize(0);
+    if(int(UpLo)==int(Lower) || MatrixType::IsRowMajor)
+    {
+      // we have to transpose the lower part to to the upper one
+      ap.resize(size,size);
+      ap.template selfadjointView<Upper>() = a.template selfadjointView<UpLo>();
+    }
+    else
+      internal::simplicial_cholesky_grab_input<CholMatrixType,MatrixType>::run(a, pmat, ap);
+  }  
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SIMPLICIAL_CHOLESKY_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h
new file mode 100644
index 0000000..72e1740
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h
@@ -0,0 +1,174 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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/.
+
+/*
+NOTE: these functions have been adapted from the LDL library:
+
+LDL Copyright (c) 2005 by Timothy A. Davis.  All Rights Reserved.
+
+The author of LDL, Timothy A. Davis., has executed a license with Google LLC
+to permit distribution of this code and derivative works as part of Eigen under
+the Mozilla Public License v. 2.0, as stated at the top of this file.
+ */
+
+#ifndef EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H
+#define EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H
+
+namespace Eigen {
+
+template<typename Derived>
+void SimplicialCholeskyBase<Derived>::analyzePattern_preordered(const CholMatrixType& ap, bool doLDLT)
+{
+  const StorageIndex size = StorageIndex(ap.rows());
+  m_matrix.resize(size, size);
+  m_parent.resize(size);
+  m_nonZerosPerCol.resize(size);
+
+  ei_declare_aligned_stack_constructed_variable(StorageIndex, tags, size, 0);
+
+  for(StorageIndex k = 0; k < size; ++k)
+  {
+    /* L(k,:) pattern: all nodes reachable in etree from nz in A(0:k-1,k) */
+    m_parent[k] = -1;             /* parent of k is not yet known */
+    tags[k] = k;                  /* mark node k as visited */
+    m_nonZerosPerCol[k] = 0;      /* count of nonzeros in column k of L */
+    for(typename CholMatrixType::InnerIterator it(ap,k); it; ++it)
+    {
+      StorageIndex i = it.index();
+      if(i < k)
+      {
+        /* follow path from i to root of etree, stop at flagged node */
+        for(; tags[i] != k; i = m_parent[i])
+        {
+          /* find parent of i if not yet determined */
+          if (m_parent[i] == -1)
+            m_parent[i] = k;
+          m_nonZerosPerCol[i]++;        /* L (k,i) is nonzero */
+          tags[i] = k;                  /* mark i as visited */
+        }
+      }
+    }
+  }
+
+  /* construct Lp index array from m_nonZerosPerCol column counts */
+  StorageIndex* Lp = m_matrix.outerIndexPtr();
+  Lp[0] = 0;
+  for(StorageIndex k = 0; k < size; ++k)
+    Lp[k+1] = Lp[k] + m_nonZerosPerCol[k] + (doLDLT ? 0 : 1);
+
+  m_matrix.resizeNonZeros(Lp[size]);
+
+  m_isInitialized     = true;
+  m_info              = Success;
+  m_analysisIsOk      = true;
+  m_factorizationIsOk = false;
+}
+
+
+template<typename Derived>
+template<bool DoLDLT>
+void SimplicialCholeskyBase<Derived>::factorize_preordered(const CholMatrixType& ap)
+{
+  using std::sqrt;
+
+  eigen_assert(m_analysisIsOk && "You must first call analyzePattern()");
+  eigen_assert(ap.rows()==ap.cols());
+  eigen_assert(m_parent.size()==ap.rows());
+  eigen_assert(m_nonZerosPerCol.size()==ap.rows());
+
+  const StorageIndex size = StorageIndex(ap.rows());
+  const StorageIndex* Lp = m_matrix.outerIndexPtr();
+  StorageIndex* Li = m_matrix.innerIndexPtr();
+  Scalar* Lx = m_matrix.valuePtr();
+
+  ei_declare_aligned_stack_constructed_variable(Scalar, y, size, 0);
+  ei_declare_aligned_stack_constructed_variable(StorageIndex,  pattern, size, 0);
+  ei_declare_aligned_stack_constructed_variable(StorageIndex,  tags, size, 0);
+
+  bool ok = true;
+  m_diag.resize(DoLDLT ? size : 0);
+
+  for(StorageIndex k = 0; k < size; ++k)
+  {
+    // compute nonzero pattern of kth row of L, in topological order
+    y[k] = Scalar(0);                     // Y(0:k) is now all zero
+    StorageIndex top = size;               // stack for pattern is empty
+    tags[k] = k;                    // mark node k as visited
+    m_nonZerosPerCol[k] = 0;        // count of nonzeros in column k of L
+    for(typename CholMatrixType::InnerIterator it(ap,k); it; ++it)
+    {
+      StorageIndex i = it.index();
+      if(i <= k)
+      {
+        y[i] += numext::conj(it.value());            /* scatter A(i,k) into Y (sum duplicates) */
+        Index len;
+        for(len = 0; tags[i] != k; i = m_parent[i])
+        {
+          pattern[len++] = i;     /* L(k,i) is nonzero */
+          tags[i] = k;            /* mark i as visited */
+        }
+        while(len > 0)
+          pattern[--top] = pattern[--len];
+      }
+    }
+
+    /* compute numerical values kth row of L (a sparse triangular solve) */
+
+    RealScalar d = numext::real(y[k]) * m_shiftScale + m_shiftOffset;    // get D(k,k), apply the shift function, and clear Y(k)
+    y[k] = Scalar(0);
+    for(; top < size; ++top)
+    {
+      Index i = pattern[top];       /* pattern[top:n-1] is pattern of L(:,k) */
+      Scalar yi = y[i];             /* get and clear Y(i) */
+      y[i] = Scalar(0);
+
+      /* the nonzero entry L(k,i) */
+      Scalar l_ki;
+      if(DoLDLT)
+        l_ki = yi / numext::real(m_diag[i]);
+      else
+        yi = l_ki = yi / Lx[Lp[i]];
+
+      Index p2 = Lp[i] + m_nonZerosPerCol[i];
+      Index p;
+      for(p = Lp[i] + (DoLDLT ? 0 : 1); p < p2; ++p)
+        y[Li[p]] -= numext::conj(Lx[p]) * yi;
+      d -= numext::real(l_ki * numext::conj(yi));
+      Li[p] = k;                          /* store L(k,i) in column form of L */
+      Lx[p] = l_ki;
+      ++m_nonZerosPerCol[i];              /* increment count of nonzeros in col i */
+    }
+    if(DoLDLT)
+    {
+      m_diag[k] = d;
+      if(d == RealScalar(0))
+      {
+        ok = false;                         /* failure, D(k,k) is zero */
+        break;
+      }
+    }
+    else
+    {
+      Index p = Lp[k] + m_nonZerosPerCol[k]++;
+      Li[p] = k ;                /* store L(k,k) = sqrt (d) in column k */
+      if(d <= RealScalar(0)) {
+        ok = false;              /* failure, matrix is not positive definite */
+        break;
+      }
+      Lx[p] = sqrt(d) ;
+    }
+  }
+
+  m_info = ok ? Success : NumericalIssue;
+  m_factorizationIsOk = true;
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SIMPLICIAL_CHOLESKY_IMPL_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/AmbiVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/AmbiVector.h
new file mode 100644
index 0000000..2cb7747
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/AmbiVector.h
@@ -0,0 +1,378 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_AMBIVECTOR_H
+#define EIGEN_AMBIVECTOR_H
+
+namespace Eigen { 
+
+namespace internal {
+
+/** \internal
+  * Hybrid sparse/dense vector class designed for intensive read-write operations.
+  *
+  * See BasicSparseLLT and SparseProduct for usage examples.
+  */
+template<typename _Scalar, typename _StorageIndex>
+class AmbiVector
+{
+  public:
+    typedef _Scalar Scalar;
+    typedef _StorageIndex StorageIndex;
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+
+    explicit AmbiVector(Index size)
+      : m_buffer(0), m_zero(0), m_size(0), m_end(0), m_allocatedSize(0), m_allocatedElements(0), m_mode(-1)
+    {
+      resize(size);
+    }
+
+    void init(double estimatedDensity);
+    void init(int mode);
+
+    Index nonZeros() const;
+
+    /** Specifies a sub-vector to work on */
+    void setBounds(Index start, Index end) { m_start = convert_index(start); m_end = convert_index(end); }
+
+    void setZero();
+
+    void restart();
+    Scalar& coeffRef(Index i);
+    Scalar& coeff(Index i);
+
+    class Iterator;
+
+    ~AmbiVector() { delete[] m_buffer; }
+
+    void resize(Index size)
+    {
+      if (m_allocatedSize < size)
+        reallocate(size);
+      m_size = convert_index(size);
+    }
+
+    StorageIndex size() const { return m_size; }
+
+  protected:
+    StorageIndex convert_index(Index idx)
+    {
+      return internal::convert_index<StorageIndex>(idx);
+    }
+
+    void reallocate(Index size)
+    {
+      // if the size of the matrix is not too large, let's allocate a bit more than needed such
+      // that we can handle dense vector even in sparse mode.
+      delete[] m_buffer;
+      if (size<1000)
+      {
+        Index allocSize = (size * sizeof(ListEl) + sizeof(Scalar) - 1)/sizeof(Scalar);
+        m_allocatedElements = convert_index((allocSize*sizeof(Scalar))/sizeof(ListEl));
+        m_buffer = new Scalar[allocSize];
+      }
+      else
+      {
+        m_allocatedElements = convert_index((size*sizeof(Scalar))/sizeof(ListEl));
+        m_buffer = new Scalar[size];
+      }
+      m_size = convert_index(size);
+      m_start = 0;
+      m_end = m_size;
+    }
+
+    void reallocateSparse()
+    {
+      Index copyElements = m_allocatedElements;
+      m_allocatedElements = (std::min)(StorageIndex(m_allocatedElements*1.5),m_size);
+      Index allocSize = m_allocatedElements * sizeof(ListEl);
+      allocSize = (allocSize + sizeof(Scalar) - 1)/sizeof(Scalar);
+      Scalar* newBuffer = new Scalar[allocSize];
+      std::memcpy(newBuffer,  m_buffer,  copyElements * sizeof(ListEl));
+      delete[] m_buffer;
+      m_buffer = newBuffer;
+    }
+
+  protected:
+    // element type of the linked list
+    struct ListEl
+    {
+      StorageIndex next;
+      StorageIndex index;
+      Scalar value;
+    };
+
+    // used to store data in both mode
+    Scalar* m_buffer;
+    Scalar m_zero;
+    StorageIndex m_size;
+    StorageIndex m_start;
+    StorageIndex m_end;
+    StorageIndex m_allocatedSize;
+    StorageIndex m_allocatedElements;
+    StorageIndex m_mode;
+
+    // linked list mode
+    StorageIndex m_llStart;
+    StorageIndex m_llCurrent;
+    StorageIndex m_llSize;
+};
+
+/** \returns the number of non zeros in the current sub vector */
+template<typename _Scalar,typename _StorageIndex>
+Index AmbiVector<_Scalar,_StorageIndex>::nonZeros() const
+{
+  if (m_mode==IsSparse)
+    return m_llSize;
+  else
+    return m_end - m_start;
+}
+
+template<typename _Scalar,typename _StorageIndex>
+void AmbiVector<_Scalar,_StorageIndex>::init(double estimatedDensity)
+{
+  if (estimatedDensity>0.1)
+    init(IsDense);
+  else
+    init(IsSparse);
+}
+
+template<typename _Scalar,typename _StorageIndex>
+void AmbiVector<_Scalar,_StorageIndex>::init(int mode)
+{
+  m_mode = mode;
+  // This is only necessary in sparse mode, but we set these unconditionally to avoid some maybe-uninitialized warnings
+  // if (m_mode==IsSparse)
+  {
+    m_llSize = 0;
+    m_llStart = -1;
+  }
+}
+
+/** Must be called whenever we might perform a write access
+  * with an index smaller than the previous one.
+  *
+  * Don't worry, this function is extremely cheap.
+  */
+template<typename _Scalar,typename _StorageIndex>
+void AmbiVector<_Scalar,_StorageIndex>::restart()
+{
+  m_llCurrent = m_llStart;
+}
+
+/** Set all coefficients of current subvector to zero */
+template<typename _Scalar,typename _StorageIndex>
+void AmbiVector<_Scalar,_StorageIndex>::setZero()
+{
+  if (m_mode==IsDense)
+  {
+    for (Index i=m_start; i<m_end; ++i)
+      m_buffer[i] = Scalar(0);
+  }
+  else
+  {
+    eigen_assert(m_mode==IsSparse);
+    m_llSize = 0;
+    m_llStart = -1;
+  }
+}
+
+template<typename _Scalar,typename _StorageIndex>
+_Scalar& AmbiVector<_Scalar,_StorageIndex>::coeffRef(Index i)
+{
+  if (m_mode==IsDense)
+    return m_buffer[i];
+  else
+  {
+    ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_buffer);
+    // TODO factorize the following code to reduce code generation
+    eigen_assert(m_mode==IsSparse);
+    if (m_llSize==0)
+    {
+      // this is the first element
+      m_llStart = 0;
+      m_llCurrent = 0;
+      ++m_llSize;
+      llElements[0].value = Scalar(0);
+      llElements[0].index = convert_index(i);
+      llElements[0].next = -1;
+      return llElements[0].value;
+    }
+    else if (i<llElements[m_llStart].index)
+    {
+      // this is going to be the new first element of the list
+      ListEl& el = llElements[m_llSize];
+      el.value = Scalar(0);
+      el.index = convert_index(i);
+      el.next = m_llStart;
+      m_llStart = m_llSize;
+      ++m_llSize;
+      m_llCurrent = m_llStart;
+      return el.value;
+    }
+    else
+    {
+      StorageIndex nextel = llElements[m_llCurrent].next;
+      eigen_assert(i>=llElements[m_llCurrent].index && "you must call restart() before inserting an element with lower or equal index");
+      while (nextel >= 0 && llElements[nextel].index<=i)
+      {
+        m_llCurrent = nextel;
+        nextel = llElements[nextel].next;
+      }
+
+      if (llElements[m_llCurrent].index==i)
+      {
+        // the coefficient already exists and we found it !
+        return llElements[m_llCurrent].value;
+      }
+      else
+      {
+        if (m_llSize>=m_allocatedElements)
+        {
+          reallocateSparse();
+          llElements = reinterpret_cast<ListEl*>(m_buffer);
+        }
+        eigen_internal_assert(m_llSize<m_allocatedElements && "internal error: overflow in sparse mode");
+        // let's insert a new coefficient
+        ListEl& el = llElements[m_llSize];
+        el.value = Scalar(0);
+        el.index = convert_index(i);
+        el.next = llElements[m_llCurrent].next;
+        llElements[m_llCurrent].next = m_llSize;
+        ++m_llSize;
+        return el.value;
+      }
+    }
+  }
+}
+
+template<typename _Scalar,typename _StorageIndex>
+_Scalar& AmbiVector<_Scalar,_StorageIndex>::coeff(Index i)
+{
+  if (m_mode==IsDense)
+    return m_buffer[i];
+  else
+  {
+    ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_buffer);
+    eigen_assert(m_mode==IsSparse);
+    if ((m_llSize==0) || (i<llElements[m_llStart].index))
+    {
+      return m_zero;
+    }
+    else
+    {
+      Index elid = m_llStart;
+      while (elid >= 0 && llElements[elid].index<i)
+        elid = llElements[elid].next;
+
+      if (llElements[elid].index==i)
+        return llElements[m_llCurrent].value;
+      else
+        return m_zero;
+    }
+  }
+}
+
+/** Iterator over the nonzero coefficients */
+template<typename _Scalar,typename _StorageIndex>
+class AmbiVector<_Scalar,_StorageIndex>::Iterator
+{
+  public:
+    typedef _Scalar Scalar;
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+
+    /** Default constructor
+      * \param vec the vector on which we iterate
+      * \param epsilon the minimal value used to prune zero coefficients.
+      * In practice, all coefficients having a magnitude smaller than \a epsilon
+      * are skipped.
+      */
+    explicit Iterator(const AmbiVector& vec, const RealScalar& epsilon = 0)
+      : m_vector(vec)
+    {
+      using std::abs;
+      m_epsilon = epsilon;
+      m_isDense = m_vector.m_mode==IsDense;
+      if (m_isDense)
+      {
+        m_currentEl = 0;   // this is to avoid a compilation warning
+        m_cachedValue = 0; // this is to avoid a compilation warning
+        m_cachedIndex = m_vector.m_start-1;
+        ++(*this);
+      }
+      else
+      {
+        ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_vector.m_buffer);
+        m_currentEl = m_vector.m_llStart;
+        while (m_currentEl>=0 && abs(llElements[m_currentEl].value)<=m_epsilon)
+          m_currentEl = llElements[m_currentEl].next;
+        if (m_currentEl<0)
+        {
+          m_cachedValue = 0; // this is to avoid a compilation warning
+          m_cachedIndex = -1;
+        }
+        else
+        {
+          m_cachedIndex = llElements[m_currentEl].index;
+          m_cachedValue = llElements[m_currentEl].value;
+        }
+      }
+    }
+
+    StorageIndex index() const { return m_cachedIndex; }
+    Scalar value() const { return m_cachedValue; }
+
+    operator bool() const { return m_cachedIndex>=0; }
+
+    Iterator& operator++()
+    {
+      using std::abs;
+      if (m_isDense)
+      {
+        do {
+          ++m_cachedIndex;
+        } while (m_cachedIndex<m_vector.m_end && abs(m_vector.m_buffer[m_cachedIndex])<=m_epsilon);
+        if (m_cachedIndex<m_vector.m_end)
+          m_cachedValue = m_vector.m_buffer[m_cachedIndex];
+        else
+          m_cachedIndex=-1;
+      }
+      else
+      {
+        ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_vector.m_buffer);
+        do {
+          m_currentEl = llElements[m_currentEl].next;
+        } while (m_currentEl>=0 && abs(llElements[m_currentEl].value)<=m_epsilon);
+        if (m_currentEl<0)
+        {
+          m_cachedIndex = -1;
+        }
+        else
+        {
+          m_cachedIndex = llElements[m_currentEl].index;
+          m_cachedValue = llElements[m_currentEl].value;
+        }
+      }
+      return *this;
+    }
+
+  protected:
+    const AmbiVector& m_vector; // the target vector
+    StorageIndex m_currentEl;   // the current element in sparse/linked-list mode
+    RealScalar m_epsilon;       // epsilon used to prune zero coefficients
+    StorageIndex m_cachedIndex; // current coordinate
+    Scalar m_cachedValue;       // current value
+    bool m_isDense;             // mode of the vector
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_AMBIVECTOR_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/CompressedStorage.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/CompressedStorage.h
new file mode 100644
index 0000000..acd986f
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/CompressedStorage.h
@@ -0,0 +1,274 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_COMPRESSED_STORAGE_H
+#define EIGEN_COMPRESSED_STORAGE_H
+
+namespace Eigen { 
+
+namespace internal {
+
+/** \internal
+  * Stores a sparse set of values as a list of values and a list of indices.
+  *
+  */
+template<typename _Scalar,typename _StorageIndex>
+class CompressedStorage
+{
+  public:
+
+    typedef _Scalar Scalar;
+    typedef _StorageIndex StorageIndex;
+
+  protected:
+
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+
+  public:
+
+    CompressedStorage()
+      : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0)
+    {}
+
+    explicit CompressedStorage(Index size)
+      : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0)
+    {
+      resize(size);
+    }
+
+    CompressedStorage(const CompressedStorage& other)
+      : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0)
+    {
+      *this = other;
+    }
+
+    CompressedStorage& operator=(const CompressedStorage& other)
+    {
+      resize(other.size());
+      if(other.size()>0)
+      {
+        internal::smart_copy(other.m_values,  other.m_values  + m_size, m_values);
+        internal::smart_copy(other.m_indices, other.m_indices + m_size, m_indices);
+      }
+      return *this;
+    }
+
+    void swap(CompressedStorage& other)
+    {
+      std::swap(m_values, other.m_values);
+      std::swap(m_indices, other.m_indices);
+      std::swap(m_size, other.m_size);
+      std::swap(m_allocatedSize, other.m_allocatedSize);
+    }
+
+    ~CompressedStorage()
+    {
+      delete[] m_values;
+      delete[] m_indices;
+    }
+
+    void reserve(Index size)
+    {
+      Index newAllocatedSize = m_size + size;
+      if (newAllocatedSize > m_allocatedSize)
+        reallocate(newAllocatedSize);
+    }
+
+    void squeeze()
+    {
+      if (m_allocatedSize>m_size)
+        reallocate(m_size);
+    }
+
+    void resize(Index size, double reserveSizeFactor = 0)
+    {
+      if (m_allocatedSize<size)
+      {
+        Index realloc_size = (std::min<Index>)(NumTraits<StorageIndex>::highest(),  size + Index(reserveSizeFactor*double(size)));
+        if(realloc_size<size)
+          internal::throw_std_bad_alloc();
+        reallocate(realloc_size);
+      }
+      m_size = size;
+    }
+
+    void append(const Scalar& v, Index i)
+    {
+      Index id = m_size;
+      resize(m_size+1, 1);
+      m_values[id] = v;
+      m_indices[id] = internal::convert_index<StorageIndex>(i);
+    }
+
+    inline Index size() const { return m_size; }
+    inline Index allocatedSize() const { return m_allocatedSize; }
+    inline void clear() { m_size = 0; }
+
+    const Scalar* valuePtr() const { return m_values; }
+    Scalar* valuePtr() { return m_values; }
+    const StorageIndex* indexPtr() const { return m_indices; }
+    StorageIndex* indexPtr() { return m_indices; }
+
+    inline Scalar& value(Index i) { eigen_internal_assert(m_values!=0); return m_values[i]; }
+    inline const Scalar& value(Index i) const { eigen_internal_assert(m_values!=0); return m_values[i]; }
+
+    inline StorageIndex& index(Index i) { eigen_internal_assert(m_indices!=0); return m_indices[i]; }
+    inline const StorageIndex& index(Index i) const { eigen_internal_assert(m_indices!=0); return m_indices[i]; }
+
+    /** \returns the largest \c k such that for all \c j in [0,k) index[\c j]\<\a key */
+    inline Index searchLowerIndex(Index key) const
+    {
+      return searchLowerIndex(0, m_size, key);
+    }
+
+    /** \returns the largest \c k in [start,end) such that for all \c j in [start,k) index[\c j]\<\a key */
+    inline Index searchLowerIndex(Index start, Index end, Index key) const
+    {
+      while(end>start)
+      {
+        Index mid = (end+start)>>1;
+        if (m_indices[mid]<key)
+          start = mid+1;
+        else
+          end = mid;
+      }
+      return start;
+    }
+
+    /** \returns the stored value at index \a key
+      * If the value does not exist, then the value \a defaultValue is returned without any insertion. */
+    inline Scalar at(Index key, const Scalar& defaultValue = Scalar(0)) const
+    {
+      if (m_size==0)
+        return defaultValue;
+      else if (key==m_indices[m_size-1])
+        return m_values[m_size-1];
+      // ^^  optimization: let's first check if it is the last coefficient
+      // (very common in high level algorithms)
+      const Index id = searchLowerIndex(0,m_size-1,key);
+      return ((id<m_size) && (m_indices[id]==key)) ? m_values[id] : defaultValue;
+    }
+
+    /** Like at(), but the search is performed in the range [start,end) */
+    inline Scalar atInRange(Index start, Index end, Index key, const Scalar &defaultValue = Scalar(0)) const
+    {
+      if (start>=end)
+        return defaultValue;
+      else if (end>start && key==m_indices[end-1])
+        return m_values[end-1];
+      // ^^  optimization: let's first check if it is the last coefficient
+      // (very common in high level algorithms)
+      const Index id = searchLowerIndex(start,end-1,key);
+      return ((id<end) && (m_indices[id]==key)) ? m_values[id] : defaultValue;
+    }
+
+    /** \returns a reference to the value at index \a key
+      * If the value does not exist, then the value \a defaultValue is inserted
+      * such that the keys are sorted. */
+    inline Scalar& atWithInsertion(Index key, const Scalar& defaultValue = Scalar(0))
+    {
+      Index id = searchLowerIndex(0,m_size,key);
+      if (id>=m_size || m_indices[id]!=key)
+      {
+        if (m_allocatedSize<m_size+1)
+        {
+          m_allocatedSize = 2*(m_size+1);
+          internal::scoped_array<Scalar> newValues(m_allocatedSize);
+          internal::scoped_array<StorageIndex> newIndices(m_allocatedSize);
+
+          // copy first chunk
+          internal::smart_copy(m_values,  m_values +id, newValues.ptr());
+          internal::smart_copy(m_indices, m_indices+id, newIndices.ptr());
+
+          // copy the rest
+          if(m_size>id)
+          {
+            internal::smart_copy(m_values +id,  m_values +m_size, newValues.ptr() +id+1);
+            internal::smart_copy(m_indices+id,  m_indices+m_size, newIndices.ptr()+id+1);
+          }
+          std::swap(m_values,newValues.ptr());
+          std::swap(m_indices,newIndices.ptr());
+        }
+        else if(m_size>id)
+        {
+          internal::smart_memmove(m_values +id, m_values +m_size, m_values +id+1);
+          internal::smart_memmove(m_indices+id, m_indices+m_size, m_indices+id+1);
+        }
+        m_size++;
+        m_indices[id] = internal::convert_index<StorageIndex>(key);
+        m_values[id] = defaultValue;
+      }
+      return m_values[id];
+    }
+
+    void moveChunk(Index from, Index to, Index chunkSize)
+    {
+      eigen_internal_assert(to+chunkSize <= m_size);
+      if(to>from && from+chunkSize>to)
+      {
+        // move backward
+        internal::smart_memmove(m_values+from,  m_values+from+chunkSize,  m_values+to);
+        internal::smart_memmove(m_indices+from, m_indices+from+chunkSize, m_indices+to);
+      }
+      else
+      {
+        internal::smart_copy(m_values+from,  m_values+from+chunkSize,  m_values+to);
+        internal::smart_copy(m_indices+from, m_indices+from+chunkSize, m_indices+to);
+      }
+    }
+
+    void prune(const Scalar& reference, const RealScalar& epsilon = NumTraits<RealScalar>::dummy_precision())
+    {
+      Index k = 0;
+      Index n = size();
+      for (Index i=0; i<n; ++i)
+      {
+        if (!internal::isMuchSmallerThan(value(i), reference, epsilon))
+        {
+          value(k) = value(i);
+          index(k) = index(i);
+          ++k;
+        }
+      }
+      resize(k,0);
+    }
+
+  protected:
+
+    inline void reallocate(Index size)
+    {
+      #ifdef EIGEN_SPARSE_COMPRESSED_STORAGE_REALLOCATE_PLUGIN
+        EIGEN_SPARSE_COMPRESSED_STORAGE_REALLOCATE_PLUGIN
+      #endif
+      eigen_internal_assert(size!=m_allocatedSize);
+      internal::scoped_array<Scalar> newValues(size);
+      internal::scoped_array<StorageIndex> newIndices(size);
+      Index copySize = (std::min)(size, m_size);
+      if (copySize>0) {
+        internal::smart_copy(m_values, m_values+copySize, newValues.ptr());
+        internal::smart_copy(m_indices, m_indices+copySize, newIndices.ptr());
+      }
+      std::swap(m_values,newValues.ptr());
+      std::swap(m_indices,newIndices.ptr());
+      m_allocatedSize = size;
+    }
+
+  protected:
+    Scalar* m_values;
+    StorageIndex* m_indices;
+    Index m_size;
+    Index m_allocatedSize;
+
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_COMPRESSED_STORAGE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h
new file mode 100644
index 0000000..9486502
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h
@@ -0,0 +1,352 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_CONSERVATIVESPARSESPARSEPRODUCT_H
+#define EIGEN_CONSERVATIVESPARSESPARSEPRODUCT_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename Lhs, typename Rhs, typename ResultType>
+static void conservative_sparse_sparse_product_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res, bool sortedInsertion = false)
+{
+  typedef typename remove_all<Lhs>::type::Scalar LhsScalar;
+  typedef typename remove_all<Rhs>::type::Scalar RhsScalar;
+  typedef typename remove_all<ResultType>::type::Scalar ResScalar;
+
+  // make sure to call innerSize/outerSize since we fake the storage order.
+  Index rows = lhs.innerSize();
+  Index cols = rhs.outerSize();
+  eigen_assert(lhs.outerSize() == rhs.innerSize());
+
+  ei_declare_aligned_stack_constructed_variable(bool,   mask,     rows, 0);
+  ei_declare_aligned_stack_constructed_variable(ResScalar, values,   rows, 0);
+  ei_declare_aligned_stack_constructed_variable(Index,  indices,  rows, 0);
+
+  std::memset(mask,0,sizeof(bool)*rows);
+
+  evaluator<Lhs> lhsEval(lhs);
+  evaluator<Rhs> rhsEval(rhs);
+
+  // estimate the number of non zero entries
+  // given a rhs column containing Y non zeros, we assume that the respective Y columns
+  // of the lhs differs in average of one non zeros, thus the number of non zeros for
+  // the product of a rhs column with the lhs is X+Y where X is the average number of non zero
+  // per column of the lhs.
+  // Therefore, we have nnz(lhs*rhs) = nnz(lhs) + nnz(rhs)
+  Index estimated_nnz_prod = lhsEval.nonZerosEstimate() + rhsEval.nonZerosEstimate();
+
+  res.setZero();
+  res.reserve(Index(estimated_nnz_prod));
+  // we compute each column of the result, one after the other
+  for (Index j=0; j<cols; ++j)
+  {
+
+    res.startVec(j);
+    Index nnz = 0;
+    for (typename evaluator<Rhs>::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt)
+    {
+      RhsScalar y = rhsIt.value();
+      Index k = rhsIt.index();
+      for (typename evaluator<Lhs>::InnerIterator lhsIt(lhsEval, k); lhsIt; ++lhsIt)
+      {
+        Index i = lhsIt.index();
+        LhsScalar x = lhsIt.value();
+        if(!mask[i])
+        {
+          mask[i] = true;
+          values[i] = x * y;
+          indices[nnz] = i;
+          ++nnz;
+        }
+        else
+          values[i] += x * y;
+      }
+    }
+    if(!sortedInsertion)
+    {
+      // unordered insertion
+      for(Index k=0; k<nnz; ++k)
+      {
+        Index i = indices[k];
+        res.insertBackByOuterInnerUnordered(j,i) = values[i];
+        mask[i] = false;
+      }
+    }
+    else
+    {
+      // alternative ordered insertion code:
+      const Index t200 = rows/11; // 11 == (log2(200)*1.39)
+      const Index t = (rows*100)/139;
+
+      // FIXME reserve nnz non zeros
+      // FIXME implement faster sorting algorithms for very small nnz
+      // if the result is sparse enough => use a quick sort
+      // otherwise => loop through the entire vector
+      // In order to avoid to perform an expensive log2 when the
+      // result is clearly very sparse we use a linear bound up to 200.
+      if((nnz<200 && nnz<t200) || nnz * numext::log2(int(nnz)) < t)
+      {
+        if(nnz>1) std::sort(indices,indices+nnz);
+        for(Index k=0; k<nnz; ++k)
+        {
+          Index i = indices[k];
+          res.insertBackByOuterInner(j,i) = values[i];
+          mask[i] = false;
+        }
+      }
+      else
+      {
+        // dense path
+        for(Index i=0; i<rows; ++i)
+        {
+          if(mask[i])
+          {
+            mask[i] = false;
+            res.insertBackByOuterInner(j,i) = values[i];
+          }
+        }
+      }
+    }
+  }
+  res.finalize();
+}
+
+
+} // end namespace internal
+
+namespace internal {
+
+template<typename Lhs, typename Rhs, typename ResultType,
+  int LhsStorageOrder = (traits<Lhs>::Flags&RowMajorBit) ? RowMajor : ColMajor,
+  int RhsStorageOrder = (traits<Rhs>::Flags&RowMajorBit) ? RowMajor : ColMajor,
+  int ResStorageOrder = (traits<ResultType>::Flags&RowMajorBit) ? RowMajor : ColMajor>
+struct conservative_sparse_sparse_product_selector;
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor,ColMajor>
+{
+  typedef typename remove_all<Lhs>::type LhsCleaned;
+  typedef typename LhsCleaned::Scalar Scalar;
+
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+  {
+    typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorMatrix;
+    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorMatrixAux;
+    typedef typename sparse_eval<ColMajorMatrixAux,ResultType::RowsAtCompileTime,ResultType::ColsAtCompileTime,ColMajorMatrixAux::Flags>::type ColMajorMatrix;
+
+    // If the result is tall and thin (in the extreme case a column vector)
+    // then it is faster to sort the coefficients inplace instead of transposing twice.
+    // FIXME, the following heuristic is probably not very good.
+    if(lhs.rows()>rhs.cols())
+    {
+      ColMajorMatrix resCol(lhs.rows(),rhs.cols());
+      // perform sorted insertion
+      internal::conservative_sparse_sparse_product_impl<Lhs,Rhs,ColMajorMatrix>(lhs, rhs, resCol, true);
+      res = resCol.markAsRValue();
+    }
+    else
+    {
+      ColMajorMatrixAux resCol(lhs.rows(),rhs.cols());
+      // resort to transpose to sort the entries
+      internal::conservative_sparse_sparse_product_impl<Lhs,Rhs,ColMajorMatrixAux>(lhs, rhs, resCol, false);
+      RowMajorMatrix resRow(resCol);
+      res = resRow.markAsRValue();
+    }
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,ColMajor,ColMajor>
+{
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+  {
+    typedef SparseMatrix<typename Rhs::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorRhs;
+    typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorRes;
+    RowMajorRhs rhsRow = rhs;
+    RowMajorRes resRow(lhs.rows(), rhs.cols());
+    internal::conservative_sparse_sparse_product_impl<RowMajorRhs,Lhs,RowMajorRes>(rhsRow, lhs, resRow);
+    res = resRow;
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,RowMajor,ColMajor>
+{
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+  {
+    typedef SparseMatrix<typename Lhs::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorLhs;
+    typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorRes;
+    RowMajorLhs lhsRow = lhs;
+    RowMajorRes resRow(lhs.rows(), rhs.cols());
+    internal::conservative_sparse_sparse_product_impl<Rhs,RowMajorLhs,RowMajorRes>(rhs, lhsRow, resRow);
+    res = resRow;
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor,ColMajor>
+{
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+  {
+    typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorMatrix;
+    RowMajorMatrix resRow(lhs.rows(), rhs.cols());
+    internal::conservative_sparse_sparse_product_impl<Rhs,Lhs,RowMajorMatrix>(rhs, lhs, resRow);
+    res = resRow;
+  }
+};
+
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor,RowMajor>
+{
+  typedef typename traits<typename remove_all<Lhs>::type>::Scalar Scalar;
+
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+  {
+    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorMatrix;
+    ColMajorMatrix resCol(lhs.rows(), rhs.cols());
+    internal::conservative_sparse_sparse_product_impl<Lhs,Rhs,ColMajorMatrix>(lhs, rhs, resCol);
+    res = resCol;
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,ColMajor,RowMajor>
+{
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+  {
+    typedef SparseMatrix<typename Lhs::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorLhs;
+    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorRes;
+    ColMajorLhs lhsCol = lhs;
+    ColMajorRes resCol(lhs.rows(), rhs.cols());
+    internal::conservative_sparse_sparse_product_impl<ColMajorLhs,Rhs,ColMajorRes>(lhsCol, rhs, resCol);
+    res = resCol;
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,RowMajor,RowMajor>
+{
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+  {
+    typedef SparseMatrix<typename Rhs::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorRhs;
+    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorRes;
+    ColMajorRhs rhsCol = rhs;
+    ColMajorRes resCol(lhs.rows(), rhs.cols());
+    internal::conservative_sparse_sparse_product_impl<Lhs,ColMajorRhs,ColMajorRes>(lhs, rhsCol, resCol);
+    res = resCol;
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor,RowMajor>
+{
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+  {
+    typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorMatrix;
+    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorMatrix;
+    RowMajorMatrix resRow(lhs.rows(),rhs.cols());
+    internal::conservative_sparse_sparse_product_impl<Rhs,Lhs,RowMajorMatrix>(rhs, lhs, resRow);
+    // sort the non zeros:
+    ColMajorMatrix resCol(resRow);
+    res = resCol;
+  }
+};
+
+} // end namespace internal
+
+
+namespace internal {
+
+template<typename Lhs, typename Rhs, typename ResultType>
+static void sparse_sparse_to_dense_product_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+{
+  typedef typename remove_all<Lhs>::type::Scalar LhsScalar;
+  typedef typename remove_all<Rhs>::type::Scalar RhsScalar;
+  Index cols = rhs.outerSize();
+  eigen_assert(lhs.outerSize() == rhs.innerSize());
+
+  evaluator<Lhs> lhsEval(lhs);
+  evaluator<Rhs> rhsEval(rhs);
+
+  for (Index j=0; j<cols; ++j)
+  {
+    for (typename evaluator<Rhs>::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt)
+    {
+      RhsScalar y = rhsIt.value();
+      Index k = rhsIt.index();
+      for (typename evaluator<Lhs>::InnerIterator lhsIt(lhsEval, k); lhsIt; ++lhsIt)
+      {
+        Index i = lhsIt.index();
+        LhsScalar x = lhsIt.value();
+        res.coeffRef(i,j) += x * y;
+      }
+    }
+  }
+}
+
+
+} // end namespace internal
+
+namespace internal {
+
+template<typename Lhs, typename Rhs, typename ResultType,
+  int LhsStorageOrder = (traits<Lhs>::Flags&RowMajorBit) ? RowMajor : ColMajor,
+  int RhsStorageOrder = (traits<Rhs>::Flags&RowMajorBit) ? RowMajor : ColMajor>
+struct sparse_sparse_to_dense_product_selector;
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_to_dense_product_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor>
+{
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+  {
+    internal::sparse_sparse_to_dense_product_impl<Lhs,Rhs,ResultType>(lhs, rhs, res);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_to_dense_product_selector<Lhs,Rhs,ResultType,RowMajor,ColMajor>
+{
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+  {
+    typedef SparseMatrix<typename Lhs::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorLhs;
+    ColMajorLhs lhsCol(lhs);
+    internal::sparse_sparse_to_dense_product_impl<ColMajorLhs,Rhs,ResultType>(lhsCol, rhs, res);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_to_dense_product_selector<Lhs,Rhs,ResultType,ColMajor,RowMajor>
+{
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+  {
+    typedef SparseMatrix<typename Rhs::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorRhs;
+    ColMajorRhs rhsCol(rhs);
+    internal::sparse_sparse_to_dense_product_impl<Lhs,ColMajorRhs,ResultType>(lhs, rhsCol, res);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_to_dense_product_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor>
+{
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+  {
+    Transpose<ResultType> trRes(res);
+    internal::sparse_sparse_to_dense_product_impl<Rhs,Lhs,Transpose<ResultType> >(rhs, lhs, trRes);
+  }
+};
+
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_CONSERVATIVESPARSESPARSEPRODUCT_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/MappedSparseMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/MappedSparseMatrix.h
new file mode 100644
index 0000000..67718c8
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/MappedSparseMatrix.h
@@ -0,0 +1,67 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_MAPPED_SPARSEMATRIX_H
+#define EIGEN_MAPPED_SPARSEMATRIX_H
+
+namespace Eigen {
+
+/** \deprecated Use Map<SparseMatrix<> >
+  * \class MappedSparseMatrix
+  *
+  * \brief Sparse matrix
+  *
+  * \param _Scalar the scalar type, i.e. the type of the coefficients
+  *
+  * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme.
+  *
+  */
+namespace internal {
+template<typename _Scalar, int _Flags, typename _StorageIndex>
+struct traits<MappedSparseMatrix<_Scalar, _Flags, _StorageIndex> > : traits<SparseMatrix<_Scalar, _Flags, _StorageIndex> >
+{};
+} // end namespace internal
+
+template<typename _Scalar, int _Flags, typename _StorageIndex>
+class MappedSparseMatrix
+  : public Map<SparseMatrix<_Scalar, _Flags, _StorageIndex> >
+{
+    typedef Map<SparseMatrix<_Scalar, _Flags, _StorageIndex> > Base;
+
+  public:
+    
+    typedef typename Base::StorageIndex StorageIndex;
+    typedef typename Base::Scalar Scalar;
+
+    inline MappedSparseMatrix(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr, StorageIndex* innerIndexPtr, Scalar* valuePtr, StorageIndex* innerNonZeroPtr = 0)
+      : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZeroPtr)
+    {}
+
+    /** Empty destructor */
+    inline ~MappedSparseMatrix() {}
+};
+
+namespace internal {
+
+template<typename _Scalar, int _Options, typename _StorageIndex>
+struct evaluator<MappedSparseMatrix<_Scalar,_Options,_StorageIndex> >
+  : evaluator<SparseCompressedBase<MappedSparseMatrix<_Scalar,_Options,_StorageIndex> > >
+{
+  typedef MappedSparseMatrix<_Scalar,_Options,_StorageIndex> XprType;
+  typedef evaluator<SparseCompressedBase<XprType> > Base;
+  
+  evaluator() : Base() {}
+  explicit evaluator(const XprType &mat) : Base(mat) {}
+};
+
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_MAPPED_SPARSEMATRIX_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseAssign.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseAssign.h
new file mode 100644
index 0000000..905485c
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseAssign.h
@@ -0,0 +1,270 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSEASSIGN_H
+#define EIGEN_SPARSEASSIGN_H
+
+namespace Eigen { 
+
+template<typename Derived>    
+template<typename OtherDerived>
+Derived& SparseMatrixBase<Derived>::operator=(const EigenBase<OtherDerived> &other)
+{
+  internal::call_assignment_no_alias(derived(), other.derived());
+  return derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+Derived& SparseMatrixBase<Derived>::operator=(const ReturnByValue<OtherDerived>& other)
+{
+  // TODO use the evaluator mechanism
+  other.evalTo(derived());
+  return derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+inline Derived& SparseMatrixBase<Derived>::operator=(const SparseMatrixBase<OtherDerived>& other)
+{
+  // by default sparse evaluation do not alias, so we can safely bypass the generic call_assignment routine
+  internal::Assignment<Derived,OtherDerived,internal::assign_op<Scalar,typename OtherDerived::Scalar> >
+          ::run(derived(), other.derived(), internal::assign_op<Scalar,typename OtherDerived::Scalar>());
+  return derived();
+}
+
+template<typename Derived>
+inline Derived& SparseMatrixBase<Derived>::operator=(const Derived& other)
+{
+  internal::call_assignment_no_alias(derived(), other.derived());
+  return derived();
+}
+
+namespace internal {
+
+template<>
+struct storage_kind_to_evaluator_kind<Sparse> {
+  typedef IteratorBased Kind;
+};
+
+template<>
+struct storage_kind_to_shape<Sparse> {
+  typedef SparseShape Shape;
+};
+
+struct Sparse2Sparse {};
+struct Sparse2Dense  {};
+
+template<> struct AssignmentKind<SparseShape, SparseShape>           { typedef Sparse2Sparse Kind; };
+template<> struct AssignmentKind<SparseShape, SparseTriangularShape> { typedef Sparse2Sparse Kind; };
+template<> struct AssignmentKind<DenseShape,  SparseShape>           { typedef Sparse2Dense  Kind; };
+template<> struct AssignmentKind<DenseShape,  SparseTriangularShape> { typedef Sparse2Dense  Kind; };
+
+
+template<typename DstXprType, typename SrcXprType>
+void assign_sparse_to_sparse(DstXprType &dst, const SrcXprType &src)
+{
+  typedef typename DstXprType::Scalar Scalar;
+  typedef internal::evaluator<DstXprType> DstEvaluatorType;
+  typedef internal::evaluator<SrcXprType> SrcEvaluatorType;
+
+  SrcEvaluatorType srcEvaluator(src);
+
+  const bool transpose = (DstEvaluatorType::Flags & RowMajorBit) != (SrcEvaluatorType::Flags & RowMajorBit);
+  const Index outerEvaluationSize = (SrcEvaluatorType::Flags&RowMajorBit) ? src.rows() : src.cols();
+  if ((!transpose) && src.isRValue())
+  {
+    // eval without temporary
+    dst.resize(src.rows(), src.cols());
+    dst.setZero();
+    dst.reserve((std::min)(src.rows()*src.cols(), (std::max)(src.rows(),src.cols())*2));
+    for (Index j=0; j<outerEvaluationSize; ++j)
+    {
+      dst.startVec(j);
+      for (typename SrcEvaluatorType::InnerIterator it(srcEvaluator, j); it; ++it)
+      {
+        Scalar v = it.value();
+        dst.insertBackByOuterInner(j,it.index()) = v;
+      }
+    }
+    dst.finalize();
+  }
+  else
+  {
+    // eval through a temporary
+    eigen_assert(( ((internal::traits<DstXprType>::SupportedAccessPatterns & OuterRandomAccessPattern)==OuterRandomAccessPattern) ||
+              (!((DstEvaluatorType::Flags & RowMajorBit) != (SrcEvaluatorType::Flags & RowMajorBit)))) &&
+              "the transpose operation is supposed to be handled in SparseMatrix::operator=");
+
+    enum { Flip = (DstEvaluatorType::Flags & RowMajorBit) != (SrcEvaluatorType::Flags & RowMajorBit) };
+
+    
+    DstXprType temp(src.rows(), src.cols());
+
+    temp.reserve((std::min)(src.rows()*src.cols(), (std::max)(src.rows(),src.cols())*2));
+    for (Index j=0; j<outerEvaluationSize; ++j)
+    {
+      temp.startVec(j);
+      for (typename SrcEvaluatorType::InnerIterator it(srcEvaluator, j); it; ++it)
+      {
+        Scalar v = it.value();
+        temp.insertBackByOuterInner(Flip?it.index():j,Flip?j:it.index()) = v;
+      }
+    }
+    temp.finalize();
+
+    dst = temp.markAsRValue();
+  }
+}
+
+// Generic Sparse to Sparse assignment
+template< typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, Sparse2Sparse>
+{
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
+  {
+    assign_sparse_to_sparse(dst.derived(), src.derived());
+  }
+};
+
+// Generic Sparse to Dense assignment
+template< typename DstXprType, typename SrcXprType, typename Functor, typename Weak>
+struct Assignment<DstXprType, SrcXprType, Functor, Sparse2Dense, Weak>
+{
+  static void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
+  {
+    if(internal::is_same<Functor,internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> >::value)
+      dst.setZero();
+    
+    internal::evaluator<SrcXprType> srcEval(src);
+    resize_if_allowed(dst, src, func);
+    internal::evaluator<DstXprType> dstEval(dst);
+    
+    const Index outerEvaluationSize = (internal::evaluator<SrcXprType>::Flags&RowMajorBit) ? src.rows() : src.cols();
+    for (Index j=0; j<outerEvaluationSize; ++j)
+      for (typename internal::evaluator<SrcXprType>::InnerIterator i(srcEval,j); i; ++i)
+        func.assignCoeff(dstEval.coeffRef(i.row(),i.col()), i.value());
+  }
+};
+
+// Specialization for dense ?= dense +/- sparse and dense ?= sparse +/- dense
+template<typename DstXprType, typename Func1, typename Func2>
+struct assignment_from_dense_op_sparse
+{
+  template<typename SrcXprType, typename InitialFunc>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  void run(DstXprType &dst, const SrcXprType &src, const InitialFunc& /*func*/)
+  {
+    #ifdef EIGEN_SPARSE_ASSIGNMENT_FROM_DENSE_OP_SPARSE_PLUGIN
+    EIGEN_SPARSE_ASSIGNMENT_FROM_DENSE_OP_SPARSE_PLUGIN
+    #endif
+
+    call_assignment_no_alias(dst, src.lhs(), Func1());
+    call_assignment_no_alias(dst, src.rhs(), Func2());
+  }
+
+  // Specialization for dense1 = sparse + dense2; -> dense1 = dense2; dense1 += sparse;
+  template<typename Lhs, typename Rhs, typename Scalar>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  typename internal::enable_if<internal::is_same<typename internal::evaluator_traits<Rhs>::Shape,DenseShape>::value>::type
+  run(DstXprType &dst, const CwiseBinaryOp<internal::scalar_sum_op<Scalar,Scalar>, const Lhs, const Rhs> &src,
+      const internal::assign_op<typename DstXprType::Scalar,Scalar>& /*func*/)
+  {
+    #ifdef EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_ADD_DENSE_PLUGIN
+    EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_ADD_DENSE_PLUGIN
+    #endif
+
+    // Apply the dense matrix first, then the sparse one.
+    call_assignment_no_alias(dst, src.rhs(), Func1());
+    call_assignment_no_alias(dst, src.lhs(), Func2());
+  }
+
+  // Specialization for dense1 = sparse - dense2; -> dense1 = -dense2; dense1 += sparse;
+  template<typename Lhs, typename Rhs, typename Scalar>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  typename internal::enable_if<internal::is_same<typename internal::evaluator_traits<Rhs>::Shape,DenseShape>::value>::type
+  run(DstXprType &dst, const CwiseBinaryOp<internal::scalar_difference_op<Scalar,Scalar>, const Lhs, const Rhs> &src,
+      const internal::assign_op<typename DstXprType::Scalar,Scalar>& /*func*/)
+  {
+    #ifdef EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_SUB_DENSE_PLUGIN
+    EIGEN_SPARSE_ASSIGNMENT_FROM_SPARSE_SUB_DENSE_PLUGIN
+    #endif
+
+    // Apply the dense matrix first, then the sparse one.
+    call_assignment_no_alias(dst, -src.rhs(), Func1());
+    call_assignment_no_alias(dst,  src.lhs(), add_assign_op<typename DstXprType::Scalar,typename Lhs::Scalar>());
+  }
+};
+
+#define EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(ASSIGN_OP,BINOP,ASSIGN_OP2) \
+  template< typename DstXprType, typename Lhs, typename Rhs, typename Scalar> \
+  struct Assignment<DstXprType, CwiseBinaryOp<internal::BINOP<Scalar,Scalar>, const Lhs, const Rhs>, internal::ASSIGN_OP<typename DstXprType::Scalar,Scalar>, \
+                    Sparse2Dense, \
+                    typename internal::enable_if<   internal::is_same<typename internal::evaluator_traits<Lhs>::Shape,DenseShape>::value \
+                                                 || internal::is_same<typename internal::evaluator_traits<Rhs>::Shape,DenseShape>::value>::type> \
+    : assignment_from_dense_op_sparse<DstXprType, internal::ASSIGN_OP<typename DstXprType::Scalar,typename Lhs::Scalar>, internal::ASSIGN_OP2<typename DstXprType::Scalar,typename Rhs::Scalar> > \
+  {}
+
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(assign_op,    scalar_sum_op,add_assign_op);
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(add_assign_op,scalar_sum_op,add_assign_op);
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(sub_assign_op,scalar_sum_op,sub_assign_op);
+
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(assign_op,    scalar_difference_op,sub_assign_op);
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(add_assign_op,scalar_difference_op,sub_assign_op);
+EIGEN_CATCH_ASSIGN_DENSE_OP_SPARSE(sub_assign_op,scalar_difference_op,add_assign_op);
+
+
+// Specialization for "dst = dec.solve(rhs)"
+// NOTE we need to specialize it for Sparse2Sparse to avoid ambiguous specialization error
+template<typename DstXprType, typename DecType, typename RhsType, typename Scalar>
+struct Assignment<DstXprType, Solve<DecType,RhsType>, internal::assign_op<Scalar,Scalar>, Sparse2Sparse>
+{
+  typedef Solve<DecType,RhsType> SrcXprType;
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &)
+  {
+    Index dstRows = src.rows();
+    Index dstCols = src.cols();
+    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
+      dst.resize(dstRows, dstCols);
+
+    src.dec()._solve_impl(src.rhs(), dst);
+  }
+};
+
+struct Diagonal2Sparse {};
+
+template<> struct AssignmentKind<SparseShape,DiagonalShape> { typedef Diagonal2Sparse Kind; };
+
+template< typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, Diagonal2Sparse>
+{
+  typedef typename DstXprType::StorageIndex StorageIndex;
+  typedef typename DstXprType::Scalar Scalar;
+
+  template<int Options, typename AssignFunc>
+  static void run(SparseMatrix<Scalar,Options,StorageIndex> &dst, const SrcXprType &src, const AssignFunc &func)
+  { dst.assignDiagonal(src.diagonal(), func); }
+  
+  template<typename DstDerived>
+  static void run(SparseMatrixBase<DstDerived> &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
+  { dst.derived().diagonal() = src.diagonal(); }
+  
+  template<typename DstDerived>
+  static void run(SparseMatrixBase<DstDerived> &dst, const SrcXprType &src, const internal::add_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
+  { dst.derived().diagonal() += src.diagonal(); }
+  
+  template<typename DstDerived>
+  static void run(SparseMatrixBase<DstDerived> &dst, const SrcXprType &src, const internal::sub_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
+  { dst.derived().diagonal() -= src.diagonal(); }
+};
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSEASSIGN_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseBlock.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseBlock.h
new file mode 100644
index 0000000..5b4f6cc
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseBlock.h
@@ -0,0 +1,571 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_BLOCK_H
+#define EIGEN_SPARSE_BLOCK_H
+
+namespace Eigen {
+
+// Subset of columns or rows
+template<typename XprType, int BlockRows, int BlockCols>
+class BlockImpl<XprType,BlockRows,BlockCols,true,Sparse>
+  : public SparseMatrixBase<Block<XprType,BlockRows,BlockCols,true> >
+{
+    typedef typename internal::remove_all<typename XprType::Nested>::type _MatrixTypeNested;
+    typedef Block<XprType, BlockRows, BlockCols, true> BlockType;
+public:
+    enum { IsRowMajor = internal::traits<BlockType>::IsRowMajor };
+protected:
+    enum { OuterSize = IsRowMajor ? BlockRows : BlockCols };
+    typedef SparseMatrixBase<BlockType> Base;
+    using Base::convert_index;
+public:
+    EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType)
+
+    inline BlockImpl(XprType& xpr, Index i)
+      : m_matrix(xpr), m_outerStart(convert_index(i)), m_outerSize(OuterSize)
+    {}
+
+    inline BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
+      : m_matrix(xpr), m_outerStart(convert_index(IsRowMajor ? startRow : startCol)), m_outerSize(convert_index(IsRowMajor ? blockRows : blockCols))
+    {}
+
+    EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
+    EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+
+    Index nonZeros() const
+    {
+      typedef internal::evaluator<XprType> EvaluatorType;
+      EvaluatorType matEval(m_matrix);
+      Index nnz = 0;
+      Index end = m_outerStart + m_outerSize.value();
+      for(Index j=m_outerStart; j<end; ++j)
+        for(typename EvaluatorType::InnerIterator it(matEval, j); it; ++it)
+          ++nnz;
+      return nnz;
+    }
+
+    inline const Scalar coeff(Index row, Index col) const
+    {
+      return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 :  m_outerStart));
+    }
+
+    inline const Scalar coeff(Index index) const
+    {
+      return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index :  m_outerStart);
+    }
+
+    inline const XprType& nestedExpression() const { return m_matrix; }
+    inline XprType& nestedExpression() { return m_matrix; }
+    Index startRow() const { return IsRowMajor ? m_outerStart : 0; }
+    Index startCol() const { return IsRowMajor ? 0 : m_outerStart; }
+    Index blockRows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
+    Index blockCols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+
+  protected:
+
+    typename internal::ref_selector<XprType>::non_const_type m_matrix;
+    Index m_outerStart;
+    const internal::variable_if_dynamic<Index, OuterSize> m_outerSize;
+
+  protected:
+    // Disable assignment with clear error message.
+    // Note that simply removing operator= yields compilation errors with ICC+MSVC
+    template<typename T>
+    BlockImpl& operator=(const T&)
+    {
+      EIGEN_STATIC_ASSERT(sizeof(T)==0, THIS_SPARSE_BLOCK_SUBEXPRESSION_IS_READ_ONLY);
+      return *this;
+    }
+};
+
+
+/***************************************************************************
+* specialization for SparseMatrix
+***************************************************************************/
+
+namespace internal {
+
+template<typename SparseMatrixType, int BlockRows, int BlockCols>
+class sparse_matrix_block_impl
+  : public SparseCompressedBase<Block<SparseMatrixType,BlockRows,BlockCols,true> >
+{
+    typedef typename internal::remove_all<typename SparseMatrixType::Nested>::type _MatrixTypeNested;
+    typedef Block<SparseMatrixType, BlockRows, BlockCols, true> BlockType;
+    typedef SparseCompressedBase<Block<SparseMatrixType,BlockRows,BlockCols,true> > Base;
+    using Base::convert_index;
+public:
+    enum { IsRowMajor = internal::traits<BlockType>::IsRowMajor };
+    EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType)
+protected:
+    typedef typename Base::IndexVector IndexVector;
+    enum { OuterSize = IsRowMajor ? BlockRows : BlockCols };
+public:
+
+    inline sparse_matrix_block_impl(SparseMatrixType& xpr, Index i)
+      : m_matrix(xpr), m_outerStart(convert_index(i)), m_outerSize(OuterSize)
+    {}
+
+    inline sparse_matrix_block_impl(SparseMatrixType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
+      : m_matrix(xpr), m_outerStart(convert_index(IsRowMajor ? startRow : startCol)), m_outerSize(convert_index(IsRowMajor ? blockRows : blockCols))
+    {}
+
+    template<typename OtherDerived>
+    inline BlockType& operator=(const SparseMatrixBase<OtherDerived>& other)
+    {
+      typedef typename internal::remove_all<typename SparseMatrixType::Nested>::type _NestedMatrixType;
+      _NestedMatrixType& matrix = m_matrix;
+      // This assignment is slow if this vector set is not empty
+      // and/or it is not at the end of the nonzeros of the underlying matrix.
+
+      // 1 - eval to a temporary to avoid transposition and/or aliasing issues
+      Ref<const SparseMatrix<Scalar, IsRowMajor ? RowMajor : ColMajor, StorageIndex> > tmp(other.derived());
+      eigen_internal_assert(tmp.outerSize()==m_outerSize.value());
+
+      // 2 - let's check whether there is enough allocated memory
+      Index nnz           = tmp.nonZeros();
+      Index start         = m_outerStart==0 ? 0 : m_matrix.outerIndexPtr()[m_outerStart]; // starting position of the current block
+      Index end           = m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]; // ending position of the current block
+      Index block_size    = end - start;                                                // available room in the current block
+      Index tail_size     = m_matrix.outerIndexPtr()[m_matrix.outerSize()] - end;
+
+      Index free_size     = m_matrix.isCompressed()
+                          ? Index(matrix.data().allocatedSize()) + block_size
+                          : block_size;
+
+      Index tmp_start = tmp.outerIndexPtr()[0];
+
+      bool update_trailing_pointers = false;
+      if(nnz>free_size)
+      {
+        // realloc manually to reduce copies
+        typename SparseMatrixType::Storage newdata(m_matrix.data().allocatedSize() - block_size + nnz);
+
+        internal::smart_copy(m_matrix.valuePtr(),       m_matrix.valuePtr() + start,      newdata.valuePtr());
+        internal::smart_copy(m_matrix.innerIndexPtr(),  m_matrix.innerIndexPtr() + start, newdata.indexPtr());
+
+        internal::smart_copy(tmp.valuePtr() + tmp_start,      tmp.valuePtr() + tmp_start + nnz,       newdata.valuePtr() + start);
+        internal::smart_copy(tmp.innerIndexPtr() + tmp_start, tmp.innerIndexPtr() + tmp_start + nnz,  newdata.indexPtr() + start);
+
+        internal::smart_copy(matrix.valuePtr()+end,       matrix.valuePtr()+end + tail_size,      newdata.valuePtr()+start+nnz);
+        internal::smart_copy(matrix.innerIndexPtr()+end,  matrix.innerIndexPtr()+end + tail_size, newdata.indexPtr()+start+nnz);
+
+        newdata.resize(m_matrix.outerIndexPtr()[m_matrix.outerSize()] - block_size + nnz);
+
+        matrix.data().swap(newdata);
+
+        update_trailing_pointers = true;
+      }
+      else
+      {
+        if(m_matrix.isCompressed() && nnz!=block_size)
+        {
+          // no need to realloc, simply copy the tail at its respective position and insert tmp
+          matrix.data().resize(start + nnz + tail_size);
+
+          internal::smart_memmove(matrix.valuePtr()+end,      matrix.valuePtr() + end+tail_size,      matrix.valuePtr() + start+nnz);
+          internal::smart_memmove(matrix.innerIndexPtr()+end, matrix.innerIndexPtr() + end+tail_size, matrix.innerIndexPtr() + start+nnz);
+
+          update_trailing_pointers = true;
+        }
+
+        internal::smart_copy(tmp.valuePtr() + tmp_start,      tmp.valuePtr() + tmp_start + nnz,       matrix.valuePtr() + start);
+        internal::smart_copy(tmp.innerIndexPtr() + tmp_start, tmp.innerIndexPtr() + tmp_start + nnz,  matrix.innerIndexPtr() + start);
+      }
+
+      // update outer index pointers and innerNonZeros
+      if(IsVectorAtCompileTime)
+      {
+        if(!m_matrix.isCompressed())
+          matrix.innerNonZeroPtr()[m_outerStart] = StorageIndex(nnz);
+        matrix.outerIndexPtr()[m_outerStart] = StorageIndex(start);
+      }
+      else
+      {
+        StorageIndex p = StorageIndex(start);
+        for(Index k=0; k<m_outerSize.value(); ++k)
+        {
+          StorageIndex nnz_k = internal::convert_index<StorageIndex>(tmp.innerVector(k).nonZeros());
+          if(!m_matrix.isCompressed())
+            matrix.innerNonZeroPtr()[m_outerStart+k] = nnz_k;
+          matrix.outerIndexPtr()[m_outerStart+k] = p;
+          p += nnz_k;
+        }
+      }
+
+      if(update_trailing_pointers)
+      {
+        StorageIndex offset = internal::convert_index<StorageIndex>(nnz - block_size);
+        for(Index k = m_outerStart + m_outerSize.value(); k<=matrix.outerSize(); ++k)
+        {
+          matrix.outerIndexPtr()[k] += offset;
+        }
+      }
+
+      return derived();
+    }
+
+    inline BlockType& operator=(const BlockType& other)
+    {
+      return operator=<BlockType>(other);
+    }
+
+    inline const Scalar* valuePtr() const
+    { return m_matrix.valuePtr(); }
+    inline Scalar* valuePtr()
+    { return m_matrix.valuePtr(); }
+
+    inline const StorageIndex* innerIndexPtr() const
+    { return m_matrix.innerIndexPtr(); }
+    inline StorageIndex* innerIndexPtr()
+    { return m_matrix.innerIndexPtr(); }
+
+    inline const StorageIndex* outerIndexPtr() const
+    { return m_matrix.outerIndexPtr() + m_outerStart; }
+    inline StorageIndex* outerIndexPtr()
+    { return m_matrix.outerIndexPtr() + m_outerStart; }
+
+    inline const StorageIndex* innerNonZeroPtr() const
+    { return isCompressed() ? 0 : (m_matrix.innerNonZeroPtr()+m_outerStart); }
+    inline StorageIndex* innerNonZeroPtr()
+    { return isCompressed() ? 0 : (m_matrix.innerNonZeroPtr()+m_outerStart); }
+
+    bool isCompressed() const { return m_matrix.innerNonZeroPtr()==0; }
+
+    inline Scalar& coeffRef(Index row, Index col)
+    {
+      return m_matrix.coeffRef(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 :  m_outerStart));
+    }
+
+    inline const Scalar coeff(Index row, Index col) const
+    {
+      return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 :  m_outerStart));
+    }
+
+    inline const Scalar coeff(Index index) const
+    {
+      return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index :  m_outerStart);
+    }
+
+    const Scalar& lastCoeff() const
+    {
+      EIGEN_STATIC_ASSERT_VECTOR_ONLY(sparse_matrix_block_impl);
+      eigen_assert(Base::nonZeros()>0);
+      if(m_matrix.isCompressed())
+        return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1];
+      else
+        return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1];
+    }
+
+    EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
+    EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+
+    inline const SparseMatrixType& nestedExpression() const { return m_matrix; }
+    inline SparseMatrixType& nestedExpression() { return m_matrix; }
+    Index startRow() const { return IsRowMajor ? m_outerStart : 0; }
+    Index startCol() const { return IsRowMajor ? 0 : m_outerStart; }
+    Index blockRows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
+    Index blockCols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+
+  protected:
+
+    typename internal::ref_selector<SparseMatrixType>::non_const_type m_matrix;
+    Index m_outerStart;
+    const internal::variable_if_dynamic<Index, OuterSize> m_outerSize;
+
+};
+
+} // namespace internal
+
+template<typename _Scalar, int _Options, typename _StorageIndex, int BlockRows, int BlockCols>
+class BlockImpl<SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true,Sparse>
+  : public internal::sparse_matrix_block_impl<SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols>
+{
+public:
+  typedef _StorageIndex StorageIndex;
+  typedef SparseMatrix<_Scalar, _Options, _StorageIndex> SparseMatrixType;
+  typedef internal::sparse_matrix_block_impl<SparseMatrixType,BlockRows,BlockCols> Base;
+  inline BlockImpl(SparseMatrixType& xpr, Index i)
+    : Base(xpr, i)
+  {}
+
+  inline BlockImpl(SparseMatrixType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
+    : Base(xpr, startRow, startCol, blockRows, blockCols)
+  {}
+
+  using Base::operator=;
+};
+
+template<typename _Scalar, int _Options, typename _StorageIndex, int BlockRows, int BlockCols>
+class BlockImpl<const SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true,Sparse>
+  : public internal::sparse_matrix_block_impl<const SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols>
+{
+public:
+  typedef _StorageIndex StorageIndex;
+  typedef const SparseMatrix<_Scalar, _Options, _StorageIndex> SparseMatrixType;
+  typedef internal::sparse_matrix_block_impl<SparseMatrixType,BlockRows,BlockCols> Base;
+  inline BlockImpl(SparseMatrixType& xpr, Index i)
+    : Base(xpr, i)
+  {}
+
+  inline BlockImpl(SparseMatrixType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
+    : Base(xpr, startRow, startCol, blockRows, blockCols)
+  {}
+
+  using Base::operator=;
+private:
+  template<typename Derived> BlockImpl(const SparseMatrixBase<Derived>& xpr, Index i);
+  template<typename Derived> BlockImpl(const SparseMatrixBase<Derived>& xpr);
+};
+
+//----------
+
+/** Generic implementation of sparse Block expression.
+  * Real-only.
+  */
+template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
+class BlockImpl<XprType,BlockRows,BlockCols,InnerPanel,Sparse>
+  : public SparseMatrixBase<Block<XprType,BlockRows,BlockCols,InnerPanel> >, internal::no_assignment_operator
+{
+    typedef Block<XprType, BlockRows, BlockCols, InnerPanel> BlockType;
+    typedef SparseMatrixBase<BlockType> Base;
+    using Base::convert_index;
+public:
+    enum { IsRowMajor = internal::traits<BlockType>::IsRowMajor };
+    EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType)
+
+    typedef typename internal::remove_all<typename XprType::Nested>::type _MatrixTypeNested;
+
+    /** Column or Row constructor
+      */
+    inline BlockImpl(XprType& xpr, Index i)
+      : m_matrix(xpr),
+        m_startRow( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? convert_index(i) : 0),
+        m_startCol( (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? convert_index(i) : 0),
+        m_blockRows(BlockRows==1 ? 1 : xpr.rows()),
+        m_blockCols(BlockCols==1 ? 1 : xpr.cols())
+    {}
+
+    /** Dynamic-size constructor
+      */
+    inline BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
+      : m_matrix(xpr), m_startRow(convert_index(startRow)), m_startCol(convert_index(startCol)), m_blockRows(convert_index(blockRows)), m_blockCols(convert_index(blockCols))
+    {}
+
+    inline Index rows() const { return m_blockRows.value(); }
+    inline Index cols() const { return m_blockCols.value(); }
+
+    inline Scalar& coeffRef(Index row, Index col)
+    {
+      return m_matrix.coeffRef(row + m_startRow.value(), col + m_startCol.value());
+    }
+
+    inline const Scalar coeff(Index row, Index col) const
+    {
+      return m_matrix.coeff(row + m_startRow.value(), col + m_startCol.value());
+    }
+
+    inline Scalar& coeffRef(Index index)
+    {
+      return m_matrix.coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+                               m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+    }
+
+    inline const Scalar coeff(Index index) const
+    {
+      return m_matrix.coeff(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+                            m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+    }
+
+    inline const XprType& nestedExpression() const { return m_matrix; }
+    inline XprType& nestedExpression() { return m_matrix; }
+    Index startRow() const { return m_startRow.value(); }
+    Index startCol() const { return m_startCol.value(); }
+    Index blockRows() const { return m_blockRows.value(); }
+    Index blockCols() const { return m_blockCols.value(); }
+
+  protected:
+//     friend class internal::GenericSparseBlockInnerIteratorImpl<XprType,BlockRows,BlockCols,InnerPanel>;
+    friend struct internal::unary_evaluator<Block<XprType,BlockRows,BlockCols,InnerPanel>, internal::IteratorBased, Scalar >;
+
+    Index nonZeros() const { return Dynamic; }
+
+    typename internal::ref_selector<XprType>::non_const_type m_matrix;
+    const internal::variable_if_dynamic<Index, XprType::RowsAtCompileTime == 1 ? 0 : Dynamic> m_startRow;
+    const internal::variable_if_dynamic<Index, XprType::ColsAtCompileTime == 1 ? 0 : Dynamic> m_startCol;
+    const internal::variable_if_dynamic<Index, RowsAtCompileTime> m_blockRows;
+    const internal::variable_if_dynamic<Index, ColsAtCompileTime> m_blockCols;
+
+  protected:
+    // Disable assignment with clear error message.
+    // Note that simply removing operator= yields compilation errors with ICC+MSVC
+    template<typename T>
+    BlockImpl& operator=(const T&)
+    {
+      EIGEN_STATIC_ASSERT(sizeof(T)==0, THIS_SPARSE_BLOCK_SUBEXPRESSION_IS_READ_ONLY);
+      return *this;
+    }
+
+};
+
+namespace internal {
+
+template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
+struct unary_evaluator<Block<ArgType,BlockRows,BlockCols,InnerPanel>, IteratorBased >
+ : public evaluator_base<Block<ArgType,BlockRows,BlockCols,InnerPanel> >
+{
+    class InnerVectorInnerIterator;
+    class OuterVectorInnerIterator;
+  public:
+    typedef Block<ArgType,BlockRows,BlockCols,InnerPanel> XprType;
+    typedef typename XprType::StorageIndex StorageIndex;
+    typedef typename XprType::Scalar Scalar;
+
+    enum {
+      IsRowMajor = XprType::IsRowMajor,
+
+      OuterVector =  (BlockCols==1 && ArgType::IsRowMajor)
+                    | // FIXME | instead of || to please GCC 4.4.0 stupid warning "suggest parentheses around &&".
+                      // revert to || as soon as not needed anymore.
+                     (BlockRows==1 && !ArgType::IsRowMajor),
+
+      CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
+      Flags = XprType::Flags
+    };
+
+    typedef typename internal::conditional<OuterVector,OuterVectorInnerIterator,InnerVectorInnerIterator>::type InnerIterator;
+
+    explicit unary_evaluator(const XprType& op)
+      : m_argImpl(op.nestedExpression()), m_block(op)
+    {}
+
+    inline Index nonZerosEstimate() const {
+      const Index nnz = m_block.nonZeros();
+      if(nnz < 0) {
+        // Scale the non-zero estimate for the underlying expression linearly with block size.
+        // Return zero if the underlying block is empty.
+        const Index nested_sz = m_block.nestedExpression().size();        
+        return nested_sz == 0 ? 0 : m_argImpl.nonZerosEstimate() * m_block.size() / nested_sz;
+      }
+      return nnz;
+    }
+
+  protected:
+    typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
+
+    evaluator<ArgType> m_argImpl;
+    const XprType &m_block;
+};
+
+template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
+class unary_evaluator<Block<ArgType,BlockRows,BlockCols,InnerPanel>, IteratorBased>::InnerVectorInnerIterator
+ : public EvalIterator
+{
+  // NOTE MSVC fails to compile if we don't explicitely "import" IsRowMajor from unary_evaluator
+  //      because the base class EvalIterator has a private IsRowMajor enum too. (bug #1786)
+  // NOTE We cannot call it IsRowMajor because it would shadow unary_evaluator::IsRowMajor
+  enum { XprIsRowMajor = unary_evaluator::IsRowMajor };
+  const XprType& m_block;
+  Index m_end;
+public:
+
+  EIGEN_STRONG_INLINE InnerVectorInnerIterator(const unary_evaluator& aEval, Index outer)
+    : EvalIterator(aEval.m_argImpl, outer + (XprIsRowMajor ? aEval.m_block.startRow() : aEval.m_block.startCol())),
+      m_block(aEval.m_block),
+      m_end(XprIsRowMajor ? aEval.m_block.startCol()+aEval.m_block.blockCols() : aEval.m_block.startRow()+aEval.m_block.blockRows())
+  {
+    while( (EvalIterator::operator bool()) && (EvalIterator::index() < (XprIsRowMajor ? m_block.startCol() : m_block.startRow())) )
+      EvalIterator::operator++();
+  }
+
+  inline StorageIndex index() const { return EvalIterator::index() - convert_index<StorageIndex>(XprIsRowMajor ? m_block.startCol() : m_block.startRow()); }
+  inline Index outer()  const { return EvalIterator::outer() - (XprIsRowMajor ? m_block.startRow() : m_block.startCol()); }
+  inline Index row()    const { return EvalIterator::row()   - m_block.startRow(); }
+  inline Index col()    const { return EvalIterator::col()   - m_block.startCol(); }
+
+  inline operator bool() const { return EvalIterator::operator bool() && EvalIterator::index() < m_end; }
+};
+
+template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
+class unary_evaluator<Block<ArgType,BlockRows,BlockCols,InnerPanel>, IteratorBased>::OuterVectorInnerIterator
+{
+  // NOTE see above
+  enum { XprIsRowMajor = unary_evaluator::IsRowMajor };
+  const unary_evaluator& m_eval;
+  Index m_outerPos;
+  const Index m_innerIndex;
+  Index m_end;
+  EvalIterator m_it;
+public:
+
+  EIGEN_STRONG_INLINE OuterVectorInnerIterator(const unary_evaluator& aEval, Index outer)
+    : m_eval(aEval),
+      m_outerPos( (XprIsRowMajor ? aEval.m_block.startCol() : aEval.m_block.startRow()) ),
+      m_innerIndex(XprIsRowMajor ? aEval.m_block.startRow() : aEval.m_block.startCol()),
+      m_end(XprIsRowMajor ? aEval.m_block.startCol()+aEval.m_block.blockCols() : aEval.m_block.startRow()+aEval.m_block.blockRows()),
+      m_it(m_eval.m_argImpl, m_outerPos)
+  {
+    EIGEN_UNUSED_VARIABLE(outer);
+    eigen_assert(outer==0);
+
+    while(m_it && m_it.index() < m_innerIndex) ++m_it;
+    if((!m_it) || (m_it.index()!=m_innerIndex))
+      ++(*this);
+  }
+
+  inline StorageIndex index() const { return convert_index<StorageIndex>(m_outerPos - (XprIsRowMajor ? m_eval.m_block.startCol() : m_eval.m_block.startRow())); }
+  inline Index outer()  const { return 0; }
+  inline Index row()    const { return XprIsRowMajor ? 0 : index(); }
+  inline Index col()    const { return XprIsRowMajor ? index() : 0; }
+
+  inline Scalar value() const { return m_it.value(); }
+  inline Scalar& valueRef() { return m_it.valueRef(); }
+
+  inline OuterVectorInnerIterator& operator++()
+  {
+    // search next non-zero entry
+    while(++m_outerPos<m_end)
+    {
+      // Restart iterator at the next inner-vector:
+      m_it.~EvalIterator();
+      ::new (&m_it) EvalIterator(m_eval.m_argImpl, m_outerPos);
+      // search for the key m_innerIndex in the current outer-vector
+      while(m_it && m_it.index() < m_innerIndex) ++m_it;
+      if(m_it && m_it.index()==m_innerIndex) break;
+    }
+    return *this;
+  }
+
+  inline operator bool() const { return m_outerPos < m_end; }
+};
+
+template<typename _Scalar, int _Options, typename _StorageIndex, int BlockRows, int BlockCols>
+struct unary_evaluator<Block<SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true>, IteratorBased>
+  : evaluator<SparseCompressedBase<Block<SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true> > >
+{
+  typedef Block<SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true> XprType;
+  typedef evaluator<SparseCompressedBase<XprType> > Base;
+  explicit unary_evaluator(const XprType &xpr) : Base(xpr) {}
+};
+
+template<typename _Scalar, int _Options, typename _StorageIndex, int BlockRows, int BlockCols>
+struct unary_evaluator<Block<const SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true>, IteratorBased>
+  : evaluator<SparseCompressedBase<Block<const SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true> > >
+{
+  typedef Block<const SparseMatrix<_Scalar, _Options, _StorageIndex>,BlockRows,BlockCols,true> XprType;
+  typedef evaluator<SparseCompressedBase<XprType> > Base;
+  explicit unary_evaluator(const XprType &xpr) : Base(xpr) {}
+};
+
+} // end namespace internal
+
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_BLOCK_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseColEtree.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseColEtree.h
new file mode 100644
index 0000000..ebe02d1
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseColEtree.h
@@ -0,0 +1,206 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+
+/* 
+ 
+ * NOTE: This file is the modified version of sp_coletree.c file in SuperLU 
+ 
+ * -- SuperLU routine (version 3.1) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * August 1, 2008
+ *
+ * Copyright (c) 1994 by Xerox Corporation.  All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+#ifndef SPARSE_COLETREE_H
+#define SPARSE_COLETREE_H
+
+namespace Eigen {
+
+namespace internal {
+
+/** Find the root of the tree/set containing the vertex i : Use Path halving */ 
+template<typename Index, typename IndexVector>
+Index etree_find (Index i, IndexVector& pp)
+{
+  Index p = pp(i); // Parent 
+  Index gp = pp(p); // Grand parent 
+  while (gp != p) 
+  {
+    pp(i) = gp; // Parent pointer on find path is changed to former grand parent
+    i = gp; 
+    p = pp(i);
+    gp = pp(p);
+  }
+  return p; 
+}
+
+/** Compute the column elimination tree of a sparse matrix
+  * \param mat The matrix in column-major format. 
+  * \param parent The elimination tree
+  * \param firstRowElt The column index of the first element in each row
+  * \param perm The permutation to apply to the column of \b mat
+  */
+template <typename MatrixType, typename IndexVector>
+int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowElt, typename MatrixType::StorageIndex *perm=0)
+{
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  StorageIndex nc = convert_index<StorageIndex>(mat.cols()); // Number of columns
+  StorageIndex m = convert_index<StorageIndex>(mat.rows());
+  StorageIndex diagSize = (std::min)(nc,m);
+  IndexVector root(nc); // root of subtree of etree 
+  root.setZero();
+  IndexVector pp(nc); // disjoint sets 
+  pp.setZero(); // Initialize disjoint sets 
+  parent.resize(mat.cols());
+  //Compute first nonzero column in each row 
+  firstRowElt.resize(m);
+  firstRowElt.setConstant(nc);
+  firstRowElt.segment(0, diagSize).setLinSpaced(diagSize, 0, diagSize-1);
+  bool found_diag;
+  for (StorageIndex col = 0; col < nc; col++)
+  {
+    StorageIndex pcol = col;
+    if(perm) pcol  = perm[col];
+    for (typename MatrixType::InnerIterator it(mat, pcol); it; ++it)
+    { 
+      Index row = it.row();
+      firstRowElt(row) = (std::min)(firstRowElt(row), col);
+    }
+  }
+  /* Compute etree by Liu's algorithm for symmetric matrices,
+          except use (firstRowElt[r],c) in place of an edge (r,c) of A.
+    Thus each row clique in A'*A is replaced by a star
+    centered at its first vertex, which has the same fill. */
+  StorageIndex rset, cset, rroot;
+  for (StorageIndex col = 0; col < nc; col++) 
+  {
+    found_diag = col>=m;
+    pp(col) = col; 
+    cset = col; 
+    root(cset) = col; 
+    parent(col) = nc; 
+    /* The diagonal element is treated here even if it does not exist in the matrix
+     * hence the loop is executed once more */ 
+    StorageIndex pcol = col;
+    if(perm) pcol  = perm[col];
+    for (typename MatrixType::InnerIterator it(mat, pcol); it||!found_diag; ++it)
+    { //  A sequence of interleaved find and union is performed 
+      Index i = col;
+      if(it) i = it.index();
+      if (i == col) found_diag = true;
+      
+      StorageIndex row = firstRowElt(i);
+      if (row >= col) continue; 
+      rset = internal::etree_find(row, pp); // Find the name of the set containing row
+      rroot = root(rset);
+      if (rroot != col) 
+      {
+        parent(rroot) = col; 
+        pp(cset) = rset; 
+        cset = rset; 
+        root(cset) = col; 
+      }
+    }
+  }
+  return 0;  
+}
+
+/** 
+  * Depth-first search from vertex n.  No recursion.
+  * This routine was contributed by Cédric Doucet, CEDRAT Group, Meylan, France.
+*/
+template <typename IndexVector>
+void nr_etdfs (typename IndexVector::Scalar n, IndexVector& parent, IndexVector& first_kid, IndexVector& next_kid, IndexVector& post, typename IndexVector::Scalar postnum)
+{
+  typedef typename IndexVector::Scalar StorageIndex;
+  StorageIndex current = n, first, next;
+  while (postnum != n) 
+  {
+    // No kid for the current node
+    first = first_kid(current);
+    
+    // no kid for the current node
+    if (first == -1) 
+    {
+      // Numbering this node because it has no kid 
+      post(current) = postnum++;
+      
+      // looking for the next kid 
+      next = next_kid(current); 
+      while (next == -1) 
+      {
+        // No more kids : back to the parent node
+        current = parent(current); 
+        // numbering the parent node 
+        post(current) = postnum++;
+        
+        // Get the next kid 
+        next = next_kid(current); 
+      }
+      // stopping criterion 
+      if (postnum == n+1) return; 
+      
+      // Updating current node 
+      current = next; 
+    }
+    else 
+    {
+      current = first; 
+    }
+  }
+}
+
+
+/**
+  * \brief Post order a tree 
+  * \param n the number of nodes
+  * \param parent Input tree
+  * \param post postordered tree
+  */
+template <typename IndexVector>
+void treePostorder(typename IndexVector::Scalar n, IndexVector& parent, IndexVector& post)
+{
+  typedef typename IndexVector::Scalar StorageIndex;
+  IndexVector first_kid, next_kid; // Linked list of children 
+  StorageIndex postnum; 
+  // Allocate storage for working arrays and results 
+  first_kid.resize(n+1); 
+  next_kid.setZero(n+1);
+  post.setZero(n+1);
+  
+  // Set up structure describing children
+  first_kid.setConstant(-1); 
+  for (StorageIndex v = n-1; v >= 0; v--) 
+  {
+    StorageIndex dad = parent(v);
+    next_kid(v) = first_kid(dad); 
+    first_kid(dad) = v; 
+  }
+  
+  // Depth-first search from dummy root vertex #n
+  postnum = 0; 
+  internal::nr_etdfs(n, parent, first_kid, next_kid, post, postnum);
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // SPARSE_COLETREE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCompressedBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCompressedBase.h
new file mode 100644
index 0000000..6a2c7a8
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCompressedBase.h
@@ -0,0 +1,370 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_COMPRESSED_BASE_H
+#define EIGEN_SPARSE_COMPRESSED_BASE_H
+
+namespace Eigen { 
+
+template<typename Derived> class SparseCompressedBase;
+  
+namespace internal {
+
+template<typename Derived>
+struct traits<SparseCompressedBase<Derived> > : traits<Derived>
+{};
+
+} // end namespace internal
+
+/** \ingroup SparseCore_Module
+  * \class SparseCompressedBase
+  * \brief Common base class for sparse [compressed]-{row|column}-storage format.
+  *
+  * This class defines the common interface for all derived classes implementing the compressed sparse storage format, such as:
+  *  - SparseMatrix
+  *  - Ref<SparseMatrixType,Options>
+  *  - Map<SparseMatrixType>
+  *
+  */
+template<typename Derived>
+class SparseCompressedBase
+  : public SparseMatrixBase<Derived>
+{
+  public:
+    typedef SparseMatrixBase<Derived> Base;
+    EIGEN_SPARSE_PUBLIC_INTERFACE(SparseCompressedBase)
+    using Base::operator=;
+    using Base::IsRowMajor;
+    
+    class InnerIterator;
+    class ReverseInnerIterator;
+    
+  protected:
+    typedef typename Base::IndexVector IndexVector;
+    Eigen::Map<IndexVector> innerNonZeros() { return Eigen::Map<IndexVector>(innerNonZeroPtr(), isCompressed()?0:derived().outerSize()); }
+    const  Eigen::Map<const IndexVector> innerNonZeros() const { return Eigen::Map<const IndexVector>(innerNonZeroPtr(), isCompressed()?0:derived().outerSize()); }
+        
+  public:
+    
+    /** \returns the number of non zero coefficients */
+    inline Index nonZeros() const
+    {
+      if(Derived::IsVectorAtCompileTime && outerIndexPtr()==0)
+        return derived().nonZeros();
+      else if(isCompressed())
+        return outerIndexPtr()[derived().outerSize()]-outerIndexPtr()[0];
+      else if(derived().outerSize()==0)
+        return 0;
+      else
+        return innerNonZeros().sum();
+    }
+    
+    /** \returns a const pointer to the array of values.
+      * This function is aimed at interoperability with other libraries.
+      * \sa innerIndexPtr(), outerIndexPtr() */
+    inline const Scalar* valuePtr() const { return derived().valuePtr(); }
+    /** \returns a non-const pointer to the array of values.
+      * This function is aimed at interoperability with other libraries.
+      * \sa innerIndexPtr(), outerIndexPtr() */
+    inline Scalar* valuePtr() { return derived().valuePtr(); }
+
+    /** \returns a const pointer to the array of inner indices.
+      * This function is aimed at interoperability with other libraries.
+      * \sa valuePtr(), outerIndexPtr() */
+    inline const StorageIndex* innerIndexPtr() const { return derived().innerIndexPtr(); }
+    /** \returns a non-const pointer to the array of inner indices.
+      * This function is aimed at interoperability with other libraries.
+      * \sa valuePtr(), outerIndexPtr() */
+    inline StorageIndex* innerIndexPtr() { return derived().innerIndexPtr(); }
+
+    /** \returns a const pointer to the array of the starting positions of the inner vectors.
+      * This function is aimed at interoperability with other libraries.
+      * \warning it returns the null pointer 0 for SparseVector
+      * \sa valuePtr(), innerIndexPtr() */
+    inline const StorageIndex* outerIndexPtr() const { return derived().outerIndexPtr(); }
+    /** \returns a non-const pointer to the array of the starting positions of the inner vectors.
+      * This function is aimed at interoperability with other libraries.
+      * \warning it returns the null pointer 0 for SparseVector
+      * \sa valuePtr(), innerIndexPtr() */
+    inline StorageIndex* outerIndexPtr() { return derived().outerIndexPtr(); }
+
+    /** \returns a const pointer to the array of the number of non zeros of the inner vectors.
+      * This function is aimed at interoperability with other libraries.
+      * \warning it returns the null pointer 0 in compressed mode */
+    inline const StorageIndex* innerNonZeroPtr() const { return derived().innerNonZeroPtr(); }
+    /** \returns a non-const pointer to the array of the number of non zeros of the inner vectors.
+      * This function is aimed at interoperability with other libraries.
+      * \warning it returns the null pointer 0 in compressed mode */
+    inline StorageIndex* innerNonZeroPtr() { return derived().innerNonZeroPtr(); }
+    
+    /** \returns whether \c *this is in compressed form. */
+    inline bool isCompressed() const { return innerNonZeroPtr()==0; }
+
+    /** \returns a read-only view of the stored coefficients as a 1D array expression.
+      *
+      * \warning this method is for \b compressed \b storage \b only, and it will trigger an assertion otherwise.
+      *
+      * \sa valuePtr(), isCompressed() */
+    const Map<const Array<Scalar,Dynamic,1> > coeffs() const { eigen_assert(isCompressed()); return Array<Scalar,Dynamic,1>::Map(valuePtr(),nonZeros()); }
+
+    /** \returns a read-write view of the stored coefficients as a 1D array expression
+      *
+      * \warning this method is for \b compressed \b storage \b only, and it will trigger an assertion otherwise.
+      *
+      * Here is an example:
+      * \include SparseMatrix_coeffs.cpp
+      * and the output is:
+      * \include SparseMatrix_coeffs.out
+      *
+      * \sa valuePtr(), isCompressed() */
+    Map<Array<Scalar,Dynamic,1> > coeffs() { eigen_assert(isCompressed()); return Array<Scalar,Dynamic,1>::Map(valuePtr(),nonZeros()); }
+
+  protected:
+    /** Default constructor. Do nothing. */
+    SparseCompressedBase() {}
+
+    /** \internal return the index of the coeff at (row,col) or just before if it does not exist.
+      * This is an analogue of std::lower_bound.
+      */
+    internal::LowerBoundIndex lower_bound(Index row, Index col) const
+    {
+      eigen_internal_assert(row>=0 && row<this->rows() && col>=0 && col<this->cols());
+
+      const Index outer = Derived::IsRowMajor ? row : col;
+      const Index inner = Derived::IsRowMajor ? col : row;
+
+      Index start = this->outerIndexPtr()[outer];
+      Index end = this->isCompressed() ? this->outerIndexPtr()[outer+1] : this->outerIndexPtr()[outer] + this->innerNonZeroPtr()[outer];
+      eigen_assert(end>=start && "you are using a non finalized sparse matrix or written coefficient does not exist");
+      internal::LowerBoundIndex p;
+      p.value = std::lower_bound(this->innerIndexPtr()+start, this->innerIndexPtr()+end,inner) - this->innerIndexPtr();
+      p.found = (p.value<end) && (this->innerIndexPtr()[p.value]==inner);
+      return p;
+    }
+
+    friend struct internal::evaluator<SparseCompressedBase<Derived> >;
+
+  private:
+    template<typename OtherDerived> explicit SparseCompressedBase(const SparseCompressedBase<OtherDerived>&);
+};
+
+template<typename Derived>
+class SparseCompressedBase<Derived>::InnerIterator
+{
+  public:
+    InnerIterator()
+      : m_values(0), m_indices(0), m_outer(0), m_id(0), m_end(0)
+    {}
+
+    InnerIterator(const InnerIterator& other)
+      : m_values(other.m_values), m_indices(other.m_indices), m_outer(other.m_outer), m_id(other.m_id), m_end(other.m_end)
+    {}
+
+    InnerIterator& operator=(const InnerIterator& other)
+    {
+      m_values = other.m_values;
+      m_indices = other.m_indices;
+      const_cast<OuterType&>(m_outer).setValue(other.m_outer.value());
+      m_id = other.m_id;
+      m_end = other.m_end;
+      return *this;
+    }
+
+    InnerIterator(const SparseCompressedBase& mat, Index outer)
+      : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(outer)
+    {
+      if(Derived::IsVectorAtCompileTime && mat.outerIndexPtr()==0)
+      {
+        m_id = 0;
+        m_end = mat.nonZeros();
+      }
+      else
+      {
+        m_id = mat.outerIndexPtr()[outer];
+        if(mat.isCompressed())
+          m_end = mat.outerIndexPtr()[outer+1];
+        else
+          m_end = m_id + mat.innerNonZeroPtr()[outer];
+      }
+    }
+
+    explicit InnerIterator(const SparseCompressedBase& mat)
+      : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(0), m_id(0), m_end(mat.nonZeros())
+    {
+      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+    }
+
+    explicit InnerIterator(const internal::CompressedStorage<Scalar,StorageIndex>& data)
+      : m_values(data.valuePtr()), m_indices(data.indexPtr()), m_outer(0), m_id(0), m_end(data.size())
+    {
+      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+    }
+
+    inline InnerIterator& operator++() { m_id++; return *this; }
+    inline InnerIterator& operator+=(Index i) { m_id += i ; return *this; }
+
+    inline InnerIterator operator+(Index i) 
+    { 
+        InnerIterator result = *this;
+        result += i;
+        return result;
+    }
+
+    inline const Scalar& value() const { return m_values[m_id]; }
+    inline Scalar& valueRef() { return const_cast<Scalar&>(m_values[m_id]); }
+
+    inline StorageIndex index() const { return m_indices[m_id]; }
+    inline Index outer() const { return m_outer.value(); }
+    inline Index row() const { return IsRowMajor ? m_outer.value() : index(); }
+    inline Index col() const { return IsRowMajor ? index() : m_outer.value(); }
+
+    inline operator bool() const { return (m_id < m_end); }
+
+  protected:
+    const Scalar* m_values;
+    const StorageIndex* m_indices;
+    typedef internal::variable_if_dynamic<Index,Derived::IsVectorAtCompileTime?0:Dynamic> OuterType;
+    const OuterType m_outer;
+    Index m_id;
+    Index m_end;
+  private:
+    // If you get here, then you're not using the right InnerIterator type, e.g.:
+    //   SparseMatrix<double,RowMajor> A;
+    //   SparseMatrix<double>::InnerIterator it(A,0);
+    template<typename T> InnerIterator(const SparseMatrixBase<T>&, Index outer);
+};
+
+template<typename Derived>
+class SparseCompressedBase<Derived>::ReverseInnerIterator
+{
+  public:
+    ReverseInnerIterator(const SparseCompressedBase& mat, Index outer)
+      : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(outer)
+    {
+      if(Derived::IsVectorAtCompileTime && mat.outerIndexPtr()==0)
+      {
+        m_start = 0;
+        m_id = mat.nonZeros();
+      }
+      else
+      {
+        m_start = mat.outerIndexPtr()[outer];
+        if(mat.isCompressed())
+          m_id = mat.outerIndexPtr()[outer+1];
+        else
+          m_id = m_start + mat.innerNonZeroPtr()[outer];
+      }
+    }
+
+    explicit ReverseInnerIterator(const SparseCompressedBase& mat)
+      : m_values(mat.valuePtr()), m_indices(mat.innerIndexPtr()), m_outer(0), m_start(0), m_id(mat.nonZeros())
+    {
+      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+    }
+
+    explicit ReverseInnerIterator(const internal::CompressedStorage<Scalar,StorageIndex>& data)
+      : m_values(data.valuePtr()), m_indices(data.indexPtr()), m_outer(0), m_start(0), m_id(data.size())
+    {
+      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+    }
+
+    inline ReverseInnerIterator& operator--() { --m_id; return *this; }
+    inline ReverseInnerIterator& operator-=(Index i) { m_id -= i; return *this; }
+
+    inline ReverseInnerIterator operator-(Index i) 
+    {
+        ReverseInnerIterator result = *this;
+        result -= i;
+        return result;
+    }
+
+    inline const Scalar& value() const { return m_values[m_id-1]; }
+    inline Scalar& valueRef() { return const_cast<Scalar&>(m_values[m_id-1]); }
+
+    inline StorageIndex index() const { return m_indices[m_id-1]; }
+    inline Index outer() const { return m_outer.value(); }
+    inline Index row() const { return IsRowMajor ? m_outer.value() : index(); }
+    inline Index col() const { return IsRowMajor ? index() : m_outer.value(); }
+
+    inline operator bool() const { return (m_id > m_start); }
+
+  protected:
+    const Scalar* m_values;
+    const StorageIndex* m_indices;
+    typedef internal::variable_if_dynamic<Index,Derived::IsVectorAtCompileTime?0:Dynamic> OuterType;
+    const OuterType m_outer;
+    Index m_start;
+    Index m_id;
+};
+
+namespace internal {
+
+template<typename Derived>
+struct evaluator<SparseCompressedBase<Derived> >
+  : evaluator_base<Derived>
+{
+  typedef typename Derived::Scalar Scalar;
+  typedef typename Derived::InnerIterator InnerIterator;
+  
+  enum {
+    CoeffReadCost = NumTraits<Scalar>::ReadCost,
+    Flags = Derived::Flags
+  };
+  
+  evaluator() : m_matrix(0), m_zero(0)
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+  explicit evaluator(const Derived &mat) : m_matrix(&mat), m_zero(0)
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+  
+  inline Index nonZerosEstimate() const {
+    return m_matrix->nonZeros();
+  }
+  
+  operator Derived&() { return m_matrix->const_cast_derived(); }
+  operator const Derived&() const { return *m_matrix; }
+  
+  typedef typename DenseCoeffsBase<Derived,ReadOnlyAccessors>::CoeffReturnType CoeffReturnType;
+  const Scalar& coeff(Index row, Index col) const
+  {
+    Index p = find(row,col);
+
+    if(p==Dynamic)
+      return m_zero;
+    else
+      return m_matrix->const_cast_derived().valuePtr()[p];
+  }
+
+  Scalar& coeffRef(Index row, Index col)
+  {
+    Index p = find(row,col);
+    eigen_assert(p!=Dynamic && "written coefficient does not exist");
+    return m_matrix->const_cast_derived().valuePtr()[p];
+  }
+
+protected:
+
+  Index find(Index row, Index col) const
+  {
+    internal::LowerBoundIndex p = m_matrix->lower_bound(row,col);
+    return p.found ? p.value : Dynamic;
+  }
+
+  const Derived *m_matrix;
+  const Scalar m_zero;
+};
+
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_COMPRESSED_BASE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseBinaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseBinaryOp.h
new file mode 100644
index 0000000..9b0d3f9
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseBinaryOp.h
@@ -0,0 +1,722 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_CWISE_BINARY_OP_H
+#define EIGEN_SPARSE_CWISE_BINARY_OP_H
+
+namespace Eigen { 
+
+// Here we have to handle 3 cases:
+//  1 - sparse op dense
+//  2 - dense op sparse
+//  3 - sparse op sparse
+// We also need to implement a 4th iterator for:
+//  4 - dense op dense
+// Finally, we also need to distinguish between the product and other operations :
+//                configuration      returned mode
+//  1 - sparse op dense    product      sparse
+//                         generic      dense
+//  2 - dense op sparse    product      sparse
+//                         generic      dense
+//  3 - sparse op sparse   product      sparse
+//                         generic      sparse
+//  4 - dense op dense     product      dense
+//                         generic      dense
+//
+// TODO to ease compiler job, we could specialize product/quotient with a scalar
+//      and fallback to cwise-unary evaluator using bind1st_op and bind2nd_op.
+
+template<typename BinaryOp, typename Lhs, typename Rhs>
+class CwiseBinaryOpImpl<BinaryOp, Lhs, Rhs, Sparse>
+  : public SparseMatrixBase<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
+{
+  public:
+    typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> Derived;
+    typedef SparseMatrixBase<Derived> Base;
+    EIGEN_SPARSE_PUBLIC_INTERFACE(Derived)
+    CwiseBinaryOpImpl()
+    {
+      EIGEN_STATIC_ASSERT((
+                (!internal::is_same<typename internal::traits<Lhs>::StorageKind,
+                                    typename internal::traits<Rhs>::StorageKind>::value)
+            ||  ((internal::evaluator<Lhs>::Flags&RowMajorBit) == (internal::evaluator<Rhs>::Flags&RowMajorBit))),
+            THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH);
+    }
+};
+
+namespace internal {
+
+  
+// Generic "sparse OP sparse"
+template<typename XprType> struct binary_sparse_evaluator;
+
+template<typename BinaryOp, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<BinaryOp, Lhs, Rhs>, IteratorBased, IteratorBased>
+  : evaluator_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
+{
+protected:
+  typedef typename evaluator<Lhs>::InnerIterator  LhsIterator;
+  typedef typename evaluator<Rhs>::InnerIterator  RhsIterator;
+  typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> XprType;
+  typedef typename traits<XprType>::Scalar Scalar;
+  typedef typename XprType::StorageIndex StorageIndex;
+public:
+
+  class InnerIterator
+  {
+  public:
+    
+    EIGEN_STRONG_INLINE InnerIterator(const binary_evaluator& aEval, Index outer)
+      : m_lhsIter(aEval.m_lhsImpl,outer), m_rhsIter(aEval.m_rhsImpl,outer), m_functor(aEval.m_functor)
+    {
+      this->operator++();
+    }
+
+    EIGEN_STRONG_INLINE InnerIterator& operator++()
+    {
+      if (m_lhsIter && m_rhsIter && (m_lhsIter.index() == m_rhsIter.index()))
+      {
+        m_id = m_lhsIter.index();
+        m_value = m_functor(m_lhsIter.value(), m_rhsIter.value());
+        ++m_lhsIter;
+        ++m_rhsIter;
+      }
+      else if (m_lhsIter && (!m_rhsIter || (m_lhsIter.index() < m_rhsIter.index())))
+      {
+        m_id = m_lhsIter.index();
+        m_value = m_functor(m_lhsIter.value(), Scalar(0));
+        ++m_lhsIter;
+      }
+      else if (m_rhsIter && (!m_lhsIter || (m_lhsIter.index() > m_rhsIter.index())))
+      {
+        m_id = m_rhsIter.index();
+        m_value = m_functor(Scalar(0), m_rhsIter.value());
+        ++m_rhsIter;
+      }
+      else
+      {
+        m_value = Scalar(0); // this is to avoid a compilation warning
+        m_id = -1;
+      }
+      return *this;
+    }
+
+    EIGEN_STRONG_INLINE Scalar value() const { return m_value; }
+
+    EIGEN_STRONG_INLINE StorageIndex index() const { return m_id; }
+    EIGEN_STRONG_INLINE Index outer() const { return m_lhsIter.outer(); }
+    EIGEN_STRONG_INLINE Index row() const { return Lhs::IsRowMajor ? m_lhsIter.row() : index(); }
+    EIGEN_STRONG_INLINE Index col() const { return Lhs::IsRowMajor ? index() : m_lhsIter.col(); }
+
+    EIGEN_STRONG_INLINE operator bool() const { return m_id>=0; }
+
+  protected:
+    LhsIterator m_lhsIter;
+    RhsIterator m_rhsIter;
+    const BinaryOp& m_functor;
+    Scalar m_value;
+    StorageIndex m_id;
+  };
+  
+  
+  enum {
+    CoeffReadCost = int(evaluator<Lhs>::CoeffReadCost) + int(evaluator<Rhs>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+    Flags = XprType::Flags
+  };
+  
+  explicit binary_evaluator(const XprType& xpr)
+    : m_functor(xpr.functor()),
+      m_lhsImpl(xpr.lhs()), 
+      m_rhsImpl(xpr.rhs())  
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+  
+  inline Index nonZerosEstimate() const {
+    return m_lhsImpl.nonZerosEstimate() + m_rhsImpl.nonZerosEstimate();
+  }
+
+protected:
+  const BinaryOp m_functor;
+  evaluator<Lhs> m_lhsImpl;
+  evaluator<Rhs> m_rhsImpl;
+};
+
+// dense op sparse
+template<typename BinaryOp, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<BinaryOp, Lhs, Rhs>, IndexBased, IteratorBased>
+  : evaluator_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
+{
+protected:
+  typedef typename evaluator<Rhs>::InnerIterator  RhsIterator;
+  typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> XprType;
+  typedef typename traits<XprType>::Scalar Scalar;
+  typedef typename XprType::StorageIndex StorageIndex;
+public:
+
+  class InnerIterator
+  {
+    enum { IsRowMajor = (int(Rhs::Flags)&RowMajorBit)==RowMajorBit };
+  public:
+
+    EIGEN_STRONG_INLINE InnerIterator(const binary_evaluator& aEval, Index outer)
+      : m_lhsEval(aEval.m_lhsImpl), m_rhsIter(aEval.m_rhsImpl,outer), m_functor(aEval.m_functor), m_value(0), m_id(-1), m_innerSize(aEval.m_expr.rhs().innerSize())
+    {
+      this->operator++();
+    }
+
+    EIGEN_STRONG_INLINE InnerIterator& operator++()
+    {
+      ++m_id;
+      if(m_id<m_innerSize)
+      {
+        Scalar lhsVal = m_lhsEval.coeff(IsRowMajor?m_rhsIter.outer():m_id,
+                                        IsRowMajor?m_id:m_rhsIter.outer());
+        if(m_rhsIter && m_rhsIter.index()==m_id)
+        {
+          m_value = m_functor(lhsVal, m_rhsIter.value());
+          ++m_rhsIter;
+        }
+        else
+          m_value = m_functor(lhsVal, Scalar(0));
+      }
+
+      return *this;
+    }
+
+    EIGEN_STRONG_INLINE Scalar value() const { eigen_internal_assert(m_id<m_innerSize); return m_value; }
+
+    EIGEN_STRONG_INLINE StorageIndex index() const { return m_id; }
+    EIGEN_STRONG_INLINE Index outer() const { return m_rhsIter.outer(); }
+    EIGEN_STRONG_INLINE Index row() const { return IsRowMajor ? m_rhsIter.outer() : m_id; }
+    EIGEN_STRONG_INLINE Index col() const { return IsRowMajor ? m_id : m_rhsIter.outer(); }
+
+    EIGEN_STRONG_INLINE operator bool() const { return m_id<m_innerSize; }
+
+  protected:
+    const evaluator<Lhs> &m_lhsEval;
+    RhsIterator m_rhsIter;
+    const BinaryOp& m_functor;
+    Scalar m_value;
+    StorageIndex m_id;
+    StorageIndex m_innerSize;
+  };
+
+
+  enum {
+    CoeffReadCost = int(evaluator<Lhs>::CoeffReadCost) + int(evaluator<Rhs>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+    Flags = XprType::Flags
+  };
+
+  explicit binary_evaluator(const XprType& xpr)
+    : m_functor(xpr.functor()),
+      m_lhsImpl(xpr.lhs()),
+      m_rhsImpl(xpr.rhs()),
+      m_expr(xpr)
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+
+  inline Index nonZerosEstimate() const {
+    return m_expr.size();
+  }
+
+protected:
+  const BinaryOp m_functor;
+  evaluator<Lhs> m_lhsImpl;
+  evaluator<Rhs> m_rhsImpl;
+  const XprType &m_expr;
+};
+
+// sparse op dense
+template<typename BinaryOp, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<BinaryOp, Lhs, Rhs>, IteratorBased, IndexBased>
+  : evaluator_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
+{
+protected:
+  typedef typename evaluator<Lhs>::InnerIterator  LhsIterator;
+  typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> XprType;
+  typedef typename traits<XprType>::Scalar Scalar;
+  typedef typename XprType::StorageIndex StorageIndex;
+public:
+
+  class InnerIterator
+  {
+    enum { IsRowMajor = (int(Lhs::Flags)&RowMajorBit)==RowMajorBit };
+  public:
+
+    EIGEN_STRONG_INLINE InnerIterator(const binary_evaluator& aEval, Index outer)
+      : m_lhsIter(aEval.m_lhsImpl,outer), m_rhsEval(aEval.m_rhsImpl), m_functor(aEval.m_functor), m_value(0), m_id(-1), m_innerSize(aEval.m_expr.lhs().innerSize())
+    {
+      this->operator++();
+    }
+
+    EIGEN_STRONG_INLINE InnerIterator& operator++()
+    {
+      ++m_id;
+      if(m_id<m_innerSize)
+      {
+        Scalar rhsVal = m_rhsEval.coeff(IsRowMajor?m_lhsIter.outer():m_id,
+                                        IsRowMajor?m_id:m_lhsIter.outer());
+        if(m_lhsIter && m_lhsIter.index()==m_id)
+        {
+          m_value = m_functor(m_lhsIter.value(), rhsVal);
+          ++m_lhsIter;
+        }
+        else
+          m_value = m_functor(Scalar(0),rhsVal);
+      }
+
+      return *this;
+    }
+
+    EIGEN_STRONG_INLINE Scalar value() const { eigen_internal_assert(m_id<m_innerSize); return m_value; }
+
+    EIGEN_STRONG_INLINE StorageIndex index() const { return m_id; }
+    EIGEN_STRONG_INLINE Index outer() const { return m_lhsIter.outer(); }
+    EIGEN_STRONG_INLINE Index row() const { return IsRowMajor ? m_lhsIter.outer() : m_id; }
+    EIGEN_STRONG_INLINE Index col() const { return IsRowMajor ? m_id : m_lhsIter.outer(); }
+
+    EIGEN_STRONG_INLINE operator bool() const { return m_id<m_innerSize; }
+
+  protected:
+    LhsIterator m_lhsIter;
+    const evaluator<Rhs> &m_rhsEval;
+    const BinaryOp& m_functor;
+    Scalar m_value;
+    StorageIndex m_id;
+    StorageIndex m_innerSize;
+  };
+
+
+  enum {
+    CoeffReadCost = int(evaluator<Lhs>::CoeffReadCost) + int(evaluator<Rhs>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+    Flags = XprType::Flags
+  };
+
+  explicit binary_evaluator(const XprType& xpr)
+    : m_functor(xpr.functor()),
+      m_lhsImpl(xpr.lhs()),
+      m_rhsImpl(xpr.rhs()),
+      m_expr(xpr)
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+
+  inline Index nonZerosEstimate() const {
+    return m_expr.size();
+  }
+
+protected:
+  const BinaryOp m_functor;
+  evaluator<Lhs> m_lhsImpl;
+  evaluator<Rhs> m_rhsImpl;
+  const XprType &m_expr;
+};
+
+template<typename T,
+         typename LhsKind   = typename evaluator_traits<typename T::Lhs>::Kind,
+         typename RhsKind   = typename evaluator_traits<typename T::Rhs>::Kind,
+         typename LhsScalar = typename traits<typename T::Lhs>::Scalar,
+         typename RhsScalar = typename traits<typename T::Rhs>::Scalar> struct sparse_conjunction_evaluator;
+
+// "sparse .* sparse"
+template<typename T1, typename T2, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs>, IteratorBased, IteratorBased>
+  : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> >
+{
+  typedef CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> XprType;
+  typedef sparse_conjunction_evaluator<XprType> Base;
+  explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
+};
+// "dense .* sparse"
+template<typename T1, typename T2, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs>, IndexBased, IteratorBased>
+  : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> >
+{
+  typedef CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> XprType;
+  typedef sparse_conjunction_evaluator<XprType> Base;
+  explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
+};
+// "sparse .* dense"
+template<typename T1, typename T2, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs>, IteratorBased, IndexBased>
+  : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> >
+{
+  typedef CwiseBinaryOp<scalar_product_op<T1,T2>, Lhs, Rhs> XprType;
+  typedef sparse_conjunction_evaluator<XprType> Base;
+  explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
+};
+
+// "sparse ./ dense"
+template<typename T1, typename T2, typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_quotient_op<T1,T2>, Lhs, Rhs>, IteratorBased, IndexBased>
+  : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_quotient_op<T1,T2>, Lhs, Rhs> >
+{
+  typedef CwiseBinaryOp<scalar_quotient_op<T1,T2>, Lhs, Rhs> XprType;
+  typedef sparse_conjunction_evaluator<XprType> Base;
+  explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
+};
+
+// "sparse && sparse"
+template<typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs>, IteratorBased, IteratorBased>
+  : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> >
+{
+  typedef CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> XprType;
+  typedef sparse_conjunction_evaluator<XprType> Base;
+  explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
+};
+// "dense && sparse"
+template<typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs>, IndexBased, IteratorBased>
+  : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> >
+{
+  typedef CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> XprType;
+  typedef sparse_conjunction_evaluator<XprType> Base;
+  explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
+};
+// "sparse && dense"
+template<typename Lhs, typename Rhs>
+struct binary_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs>, IteratorBased, IndexBased>
+  : sparse_conjunction_evaluator<CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> >
+{
+  typedef CwiseBinaryOp<scalar_boolean_and_op, Lhs, Rhs> XprType;
+  typedef sparse_conjunction_evaluator<XprType> Base;
+  explicit binary_evaluator(const XprType& xpr) : Base(xpr) {}
+};
+
+// "sparse ^ sparse"
+template<typename XprType>
+struct sparse_conjunction_evaluator<XprType, IteratorBased, IteratorBased>
+  : evaluator_base<XprType>
+{
+protected:
+  typedef typename XprType::Functor BinaryOp;
+  typedef typename XprType::Lhs LhsArg;
+  typedef typename XprType::Rhs RhsArg;
+  typedef typename evaluator<LhsArg>::InnerIterator  LhsIterator;
+  typedef typename evaluator<RhsArg>::InnerIterator  RhsIterator;
+  typedef typename XprType::StorageIndex StorageIndex;
+  typedef typename traits<XprType>::Scalar Scalar;
+public:
+
+  class InnerIterator
+  {
+  public:
+    
+    EIGEN_STRONG_INLINE InnerIterator(const sparse_conjunction_evaluator& aEval, Index outer)
+      : m_lhsIter(aEval.m_lhsImpl,outer), m_rhsIter(aEval.m_rhsImpl,outer), m_functor(aEval.m_functor)
+    {
+      while (m_lhsIter && m_rhsIter && (m_lhsIter.index() != m_rhsIter.index()))
+      {
+        if (m_lhsIter.index() < m_rhsIter.index())
+          ++m_lhsIter;
+        else
+          ++m_rhsIter;
+      }
+    }
+
+    EIGEN_STRONG_INLINE InnerIterator& operator++()
+    {
+      ++m_lhsIter;
+      ++m_rhsIter;
+      while (m_lhsIter && m_rhsIter && (m_lhsIter.index() != m_rhsIter.index()))
+      {
+        if (m_lhsIter.index() < m_rhsIter.index())
+          ++m_lhsIter;
+        else
+          ++m_rhsIter;
+      }
+      return *this;
+    }
+    
+    EIGEN_STRONG_INLINE Scalar value() const { return m_functor(m_lhsIter.value(), m_rhsIter.value()); }
+
+    EIGEN_STRONG_INLINE StorageIndex index() const { return m_lhsIter.index(); }
+    EIGEN_STRONG_INLINE Index outer() const { return m_lhsIter.outer(); }
+    EIGEN_STRONG_INLINE Index row() const { return m_lhsIter.row(); }
+    EIGEN_STRONG_INLINE Index col() const { return m_lhsIter.col(); }
+
+    EIGEN_STRONG_INLINE operator bool() const { return (m_lhsIter && m_rhsIter); }
+
+  protected:
+    LhsIterator m_lhsIter;
+    RhsIterator m_rhsIter;
+    const BinaryOp& m_functor;
+  };
+  
+  
+  enum {
+    CoeffReadCost = int(evaluator<LhsArg>::CoeffReadCost) + int(evaluator<RhsArg>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+    Flags = XprType::Flags
+  };
+  
+  explicit sparse_conjunction_evaluator(const XprType& xpr)
+    : m_functor(xpr.functor()),
+      m_lhsImpl(xpr.lhs()), 
+      m_rhsImpl(xpr.rhs())  
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+  
+  inline Index nonZerosEstimate() const {
+    return (std::min)(m_lhsImpl.nonZerosEstimate(), m_rhsImpl.nonZerosEstimate());
+  }
+
+protected:
+  const BinaryOp m_functor;
+  evaluator<LhsArg> m_lhsImpl;
+  evaluator<RhsArg> m_rhsImpl;
+};
+
+// "dense ^ sparse"
+template<typename XprType>
+struct sparse_conjunction_evaluator<XprType, IndexBased, IteratorBased>
+  : evaluator_base<XprType>
+{
+protected:
+  typedef typename XprType::Functor BinaryOp;
+  typedef typename XprType::Lhs LhsArg;
+  typedef typename XprType::Rhs RhsArg;
+  typedef evaluator<LhsArg> LhsEvaluator;
+  typedef typename evaluator<RhsArg>::InnerIterator  RhsIterator;
+  typedef typename XprType::StorageIndex StorageIndex;
+  typedef typename traits<XprType>::Scalar Scalar;
+public:
+
+  class InnerIterator
+  {
+    enum { IsRowMajor = (int(RhsArg::Flags)&RowMajorBit)==RowMajorBit };
+
+  public:
+    
+    EIGEN_STRONG_INLINE InnerIterator(const sparse_conjunction_evaluator& aEval, Index outer)
+      : m_lhsEval(aEval.m_lhsImpl), m_rhsIter(aEval.m_rhsImpl,outer), m_functor(aEval.m_functor), m_outer(outer)
+    {}
+
+    EIGEN_STRONG_INLINE InnerIterator& operator++()
+    {
+      ++m_rhsIter;
+      return *this;
+    }
+
+    EIGEN_STRONG_INLINE Scalar value() const
+    { return m_functor(m_lhsEval.coeff(IsRowMajor?m_outer:m_rhsIter.index(),IsRowMajor?m_rhsIter.index():m_outer), m_rhsIter.value()); }
+
+    EIGEN_STRONG_INLINE StorageIndex index() const { return m_rhsIter.index(); }
+    EIGEN_STRONG_INLINE Index outer() const { return m_rhsIter.outer(); }
+    EIGEN_STRONG_INLINE Index row() const { return m_rhsIter.row(); }
+    EIGEN_STRONG_INLINE Index col() const { return m_rhsIter.col(); }
+
+    EIGEN_STRONG_INLINE operator bool() const { return m_rhsIter; }
+    
+  protected:
+    const LhsEvaluator &m_lhsEval;
+    RhsIterator m_rhsIter;
+    const BinaryOp& m_functor;
+    const Index m_outer;
+  };
+  
+  
+  enum {
+    CoeffReadCost = int(evaluator<LhsArg>::CoeffReadCost) + int(evaluator<RhsArg>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+    Flags = XprType::Flags
+  };
+  
+  explicit sparse_conjunction_evaluator(const XprType& xpr)
+    : m_functor(xpr.functor()),
+      m_lhsImpl(xpr.lhs()), 
+      m_rhsImpl(xpr.rhs())  
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+  
+  inline Index nonZerosEstimate() const {
+    return m_rhsImpl.nonZerosEstimate();
+  }
+
+protected:
+  const BinaryOp m_functor;
+  evaluator<LhsArg> m_lhsImpl;
+  evaluator<RhsArg> m_rhsImpl;
+};
+
+// "sparse ^ dense"
+template<typename XprType>
+struct sparse_conjunction_evaluator<XprType, IteratorBased, IndexBased>
+  : evaluator_base<XprType>
+{
+protected:
+  typedef typename XprType::Functor BinaryOp;
+  typedef typename XprType::Lhs LhsArg;
+  typedef typename XprType::Rhs RhsArg;
+  typedef typename evaluator<LhsArg>::InnerIterator LhsIterator;
+  typedef evaluator<RhsArg> RhsEvaluator;
+  typedef typename XprType::StorageIndex StorageIndex;
+  typedef typename traits<XprType>::Scalar Scalar;
+public:
+
+  class InnerIterator
+  {
+    enum { IsRowMajor = (int(LhsArg::Flags)&RowMajorBit)==RowMajorBit };
+
+  public:
+    
+    EIGEN_STRONG_INLINE InnerIterator(const sparse_conjunction_evaluator& aEval, Index outer)
+      : m_lhsIter(aEval.m_lhsImpl,outer), m_rhsEval(aEval.m_rhsImpl), m_functor(aEval.m_functor), m_outer(outer)
+    {}
+
+    EIGEN_STRONG_INLINE InnerIterator& operator++()
+    {
+      ++m_lhsIter;
+      return *this;
+    }
+
+    EIGEN_STRONG_INLINE Scalar value() const
+    { return m_functor(m_lhsIter.value(),
+                       m_rhsEval.coeff(IsRowMajor?m_outer:m_lhsIter.index(),IsRowMajor?m_lhsIter.index():m_outer)); }
+
+    EIGEN_STRONG_INLINE StorageIndex index() const { return m_lhsIter.index(); }
+    EIGEN_STRONG_INLINE Index outer() const { return m_lhsIter.outer(); }
+    EIGEN_STRONG_INLINE Index row() const { return m_lhsIter.row(); }
+    EIGEN_STRONG_INLINE Index col() const { return m_lhsIter.col(); }
+
+    EIGEN_STRONG_INLINE operator bool() const { return m_lhsIter; }
+    
+  protected:
+    LhsIterator m_lhsIter;
+    const evaluator<RhsArg> &m_rhsEval;
+    const BinaryOp& m_functor;
+    const Index m_outer;
+  };
+  
+  
+  enum {
+    CoeffReadCost = int(evaluator<LhsArg>::CoeffReadCost) + int(evaluator<RhsArg>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+    Flags = XprType::Flags
+  };
+  
+  explicit sparse_conjunction_evaluator(const XprType& xpr)
+    : m_functor(xpr.functor()),
+      m_lhsImpl(xpr.lhs()), 
+      m_rhsImpl(xpr.rhs())  
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+  
+  inline Index nonZerosEstimate() const {
+    return m_lhsImpl.nonZerosEstimate();
+  }
+
+protected:
+  const BinaryOp m_functor;
+  evaluator<LhsArg> m_lhsImpl;
+  evaluator<RhsArg> m_rhsImpl;
+};
+
+}
+
+/***************************************************************************
+* Implementation of SparseMatrixBase and SparseCwise functions/operators
+***************************************************************************/
+
+template<typename Derived>
+template<typename OtherDerived>
+Derived& SparseMatrixBase<Derived>::operator+=(const EigenBase<OtherDerived> &other)
+{
+  call_assignment(derived(), other.derived(), internal::add_assign_op<Scalar,typename OtherDerived::Scalar>());
+  return derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+Derived& SparseMatrixBase<Derived>::operator-=(const EigenBase<OtherDerived> &other)
+{
+  call_assignment(derived(), other.derived(), internal::assign_op<Scalar,typename OtherDerived::Scalar>());
+  return derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE Derived &
+SparseMatrixBase<Derived>::operator-=(const SparseMatrixBase<OtherDerived> &other)
+{
+  return derived() = derived() - other.derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE Derived &
+SparseMatrixBase<Derived>::operator+=(const SparseMatrixBase<OtherDerived>& other)
+{
+  return derived() = derived() + other.derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+Derived& SparseMatrixBase<Derived>::operator+=(const DiagonalBase<OtherDerived>& other)
+{
+  call_assignment_no_alias(derived(), other.derived(), internal::add_assign_op<Scalar,typename OtherDerived::Scalar>());
+  return derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+Derived& SparseMatrixBase<Derived>::operator-=(const DiagonalBase<OtherDerived>& other)
+{
+  call_assignment_no_alias(derived(), other.derived(), internal::sub_assign_op<Scalar,typename OtherDerived::Scalar>());
+  return derived();
+}
+    
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE const typename SparseMatrixBase<Derived>::template CwiseProductDenseReturnType<OtherDerived>::Type
+SparseMatrixBase<Derived>::cwiseProduct(const MatrixBase<OtherDerived> &other) const
+{
+  return typename CwiseProductDenseReturnType<OtherDerived>::Type(derived(), other.derived());
+}
+
+template<typename DenseDerived, typename SparseDerived>
+EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_sum_op<typename DenseDerived::Scalar,typename SparseDerived::Scalar>, const DenseDerived, const SparseDerived>
+operator+(const MatrixBase<DenseDerived> &a, const SparseMatrixBase<SparseDerived> &b)
+{
+  return CwiseBinaryOp<internal::scalar_sum_op<typename DenseDerived::Scalar,typename SparseDerived::Scalar>, const DenseDerived, const SparseDerived>(a.derived(), b.derived());
+}
+
+template<typename SparseDerived, typename DenseDerived>
+EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_sum_op<typename SparseDerived::Scalar,typename DenseDerived::Scalar>, const SparseDerived, const DenseDerived>
+operator+(const SparseMatrixBase<SparseDerived> &a, const MatrixBase<DenseDerived> &b)
+{
+  return CwiseBinaryOp<internal::scalar_sum_op<typename SparseDerived::Scalar,typename DenseDerived::Scalar>, const SparseDerived, const DenseDerived>(a.derived(), b.derived());
+}
+
+template<typename DenseDerived, typename SparseDerived>
+EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_difference_op<typename DenseDerived::Scalar,typename SparseDerived::Scalar>, const DenseDerived, const SparseDerived>
+operator-(const MatrixBase<DenseDerived> &a, const SparseMatrixBase<SparseDerived> &b)
+{
+  return CwiseBinaryOp<internal::scalar_difference_op<typename DenseDerived::Scalar,typename SparseDerived::Scalar>, const DenseDerived, const SparseDerived>(a.derived(), b.derived());
+}
+
+template<typename SparseDerived, typename DenseDerived>
+EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_difference_op<typename SparseDerived::Scalar,typename DenseDerived::Scalar>, const SparseDerived, const DenseDerived>
+operator-(const SparseMatrixBase<SparseDerived> &a, const MatrixBase<DenseDerived> &b)
+{
+  return CwiseBinaryOp<internal::scalar_difference_op<typename SparseDerived::Scalar,typename DenseDerived::Scalar>, const SparseDerived, const DenseDerived>(a.derived(), b.derived());
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_CWISE_BINARY_OP_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseUnaryOp.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseUnaryOp.h
new file mode 100644
index 0000000..32dac0f
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseCwiseUnaryOp.h
@@ -0,0 +1,150 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_CWISE_UNARY_OP_H
+#define EIGEN_SPARSE_CWISE_UNARY_OP_H
+
+namespace Eigen { 
+
+namespace internal {
+  
+template<typename UnaryOp, typename ArgType>
+struct unary_evaluator<CwiseUnaryOp<UnaryOp,ArgType>, IteratorBased>
+  : public evaluator_base<CwiseUnaryOp<UnaryOp,ArgType> >
+{
+  public:
+    typedef CwiseUnaryOp<UnaryOp, ArgType> XprType;
+
+    class InnerIterator;
+    
+    enum {
+      CoeffReadCost = int(evaluator<ArgType>::CoeffReadCost) + int(functor_traits<UnaryOp>::Cost),
+      Flags = XprType::Flags
+    };
+    
+    explicit unary_evaluator(const XprType& op) : m_functor(op.functor()), m_argImpl(op.nestedExpression())
+    {
+      EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<UnaryOp>::Cost);
+      EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+    }
+    
+    inline Index nonZerosEstimate() const {
+      return m_argImpl.nonZerosEstimate();
+    }
+
+  protected:
+    typedef typename evaluator<ArgType>::InnerIterator        EvalIterator;
+    
+    const UnaryOp m_functor;
+    evaluator<ArgType> m_argImpl;
+};
+
+template<typename UnaryOp, typename ArgType>
+class unary_evaluator<CwiseUnaryOp<UnaryOp,ArgType>, IteratorBased>::InnerIterator
+    : public unary_evaluator<CwiseUnaryOp<UnaryOp,ArgType>, IteratorBased>::EvalIterator
+{
+  protected:
+    typedef typename XprType::Scalar Scalar;
+    typedef typename unary_evaluator<CwiseUnaryOp<UnaryOp,ArgType>, IteratorBased>::EvalIterator Base;
+  public:
+
+    EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer)
+      : Base(unaryOp.m_argImpl,outer), m_functor(unaryOp.m_functor)
+    {}
+
+    EIGEN_STRONG_INLINE InnerIterator& operator++()
+    { Base::operator++(); return *this; }
+
+    EIGEN_STRONG_INLINE Scalar value() const { return m_functor(Base::value()); }
+
+  protected:
+    const UnaryOp m_functor;
+  private:
+    Scalar& valueRef();
+};
+
+template<typename ViewOp, typename ArgType>
+struct unary_evaluator<CwiseUnaryView<ViewOp,ArgType>, IteratorBased>
+  : public evaluator_base<CwiseUnaryView<ViewOp,ArgType> >
+{
+  public:
+    typedef CwiseUnaryView<ViewOp, ArgType> XprType;
+
+    class InnerIterator;
+    
+    enum {
+      CoeffReadCost = int(evaluator<ArgType>::CoeffReadCost) + int(functor_traits<ViewOp>::Cost),
+      Flags = XprType::Flags
+    };
+    
+    explicit unary_evaluator(const XprType& op) : m_functor(op.functor()), m_argImpl(op.nestedExpression())
+    {
+      EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<ViewOp>::Cost);
+      EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+    }
+
+  protected:
+    typedef typename evaluator<ArgType>::InnerIterator        EvalIterator;
+    
+    const ViewOp m_functor;
+    evaluator<ArgType> m_argImpl;
+};
+
+template<typename ViewOp, typename ArgType>
+class unary_evaluator<CwiseUnaryView<ViewOp,ArgType>, IteratorBased>::InnerIterator
+    : public unary_evaluator<CwiseUnaryView<ViewOp,ArgType>, IteratorBased>::EvalIterator
+{
+  protected:
+    typedef typename XprType::Scalar Scalar;
+    typedef typename unary_evaluator<CwiseUnaryView<ViewOp,ArgType>, IteratorBased>::EvalIterator Base;
+  public:
+
+    EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer)
+      : Base(unaryOp.m_argImpl,outer), m_functor(unaryOp.m_functor)
+    {}
+
+    EIGEN_STRONG_INLINE InnerIterator& operator++()
+    { Base::operator++(); return *this; }
+
+    EIGEN_STRONG_INLINE Scalar value() const { return m_functor(Base::value()); }
+    EIGEN_STRONG_INLINE Scalar& valueRef() { return m_functor(Base::valueRef()); }
+
+  protected:
+    const ViewOp m_functor;
+};
+
+} // end namespace internal
+
+template<typename Derived>
+EIGEN_STRONG_INLINE Derived&
+SparseMatrixBase<Derived>::operator*=(const Scalar& other)
+{
+  typedef typename internal::evaluator<Derived>::InnerIterator EvalIterator;
+  internal::evaluator<Derived> thisEval(derived());
+  for (Index j=0; j<outerSize(); ++j)
+    for (EvalIterator i(thisEval,j); i; ++i)
+      i.valueRef() *= other;
+  return derived();
+}
+
+template<typename Derived>
+EIGEN_STRONG_INLINE Derived&
+SparseMatrixBase<Derived>::operator/=(const Scalar& other)
+{
+  typedef typename internal::evaluator<Derived>::InnerIterator EvalIterator;
+  internal::evaluator<Derived> thisEval(derived());
+  for (Index j=0; j<outerSize(); ++j)
+    for (EvalIterator i(thisEval,j); i; ++i)
+      i.valueRef() /= other;
+  return derived();
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_CWISE_UNARY_OP_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDenseProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDenseProduct.h
new file mode 100644
index 0000000..f005a18
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDenseProduct.h
@@ -0,0 +1,342 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSEDENSEPRODUCT_H
+#define EIGEN_SPARSEDENSEPRODUCT_H
+
+namespace Eigen { 
+
+namespace internal {
+
+template <> struct product_promote_storage_type<Sparse,Dense, OuterProduct> { typedef Sparse ret; };
+template <> struct product_promote_storage_type<Dense,Sparse, OuterProduct> { typedef Sparse ret; };
+
+template<typename SparseLhsType, typename DenseRhsType, typename DenseResType,
+         typename AlphaType,
+         int LhsStorageOrder = ((SparseLhsType::Flags&RowMajorBit)==RowMajorBit) ? RowMajor : ColMajor,
+         bool ColPerCol = ((DenseRhsType::Flags&RowMajorBit)==0) || DenseRhsType::ColsAtCompileTime==1>
+struct sparse_time_dense_product_impl;
+
+template<typename SparseLhsType, typename DenseRhsType, typename DenseResType>
+struct sparse_time_dense_product_impl<SparseLhsType,DenseRhsType,DenseResType, typename DenseResType::Scalar, RowMajor, true>
+{
+  typedef typename internal::remove_all<SparseLhsType>::type Lhs;
+  typedef typename internal::remove_all<DenseRhsType>::type Rhs;
+  typedef typename internal::remove_all<DenseResType>::type Res;
+  typedef typename evaluator<Lhs>::InnerIterator LhsInnerIterator;
+  typedef evaluator<Lhs> LhsEval;
+  static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const typename Res::Scalar& alpha)
+  {
+    LhsEval lhsEval(lhs);
+    
+    Index n = lhs.outerSize();
+#ifdef EIGEN_HAS_OPENMP
+    Eigen::initParallel();
+    Index threads = Eigen::nbThreads();
+#endif
+    
+    for(Index c=0; c<rhs.cols(); ++c)
+    {
+#ifdef EIGEN_HAS_OPENMP
+      // This 20000 threshold has been found experimentally on 2D and 3D Poisson problems.
+      // It basically represents the minimal amount of work to be done to be worth it.
+      if(threads>1 && lhsEval.nonZerosEstimate() > 20000)
+      {
+        #pragma omp parallel for schedule(dynamic,(n+threads*4-1)/(threads*4)) num_threads(threads)
+        for(Index i=0; i<n; ++i)
+          processRow(lhsEval,rhs,res,alpha,i,c);
+      }
+      else
+#endif
+      {
+        for(Index i=0; i<n; ++i)
+          processRow(lhsEval,rhs,res,alpha,i,c);
+      }
+    }
+  }
+  
+  static void processRow(const LhsEval& lhsEval, const DenseRhsType& rhs, DenseResType& res, const typename Res::Scalar& alpha, Index i, Index col)
+  {
+    typename Res::Scalar tmp(0);
+    for(LhsInnerIterator it(lhsEval,i); it ;++it)
+      tmp += it.value() * rhs.coeff(it.index(),col);
+    res.coeffRef(i,col) += alpha * tmp;
+  }
+  
+};
+
+// FIXME: what is the purpose of the following specialization? Is it for the BlockedSparse format?
+// -> let's disable it for now as it is conflicting with generic scalar*matrix and matrix*scalar operators
+// template<typename T1, typename T2/*, int _Options, typename _StrideType*/>
+// struct ScalarBinaryOpTraits<T1, Ref<T2/*, _Options, _StrideType*/> >
+// {
+//   enum {
+//     Defined = 1
+//   };
+//   typedef typename CwiseUnaryOp<scalar_multiple2_op<T1, typename T2::Scalar>, T2>::PlainObject ReturnType;
+// };
+
+template<typename SparseLhsType, typename DenseRhsType, typename DenseResType, typename AlphaType>
+struct sparse_time_dense_product_impl<SparseLhsType,DenseRhsType,DenseResType, AlphaType, ColMajor, true>
+{
+  typedef typename internal::remove_all<SparseLhsType>::type Lhs;
+  typedef typename internal::remove_all<DenseRhsType>::type Rhs;
+  typedef typename internal::remove_all<DenseResType>::type Res;
+  typedef evaluator<Lhs> LhsEval;
+  typedef typename LhsEval::InnerIterator LhsInnerIterator;
+  static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const AlphaType& alpha)
+  {
+    LhsEval lhsEval(lhs);
+    for(Index c=0; c<rhs.cols(); ++c)
+    {
+      for(Index j=0; j<lhs.outerSize(); ++j)
+      {
+//        typename Res::Scalar rhs_j = alpha * rhs.coeff(j,c);
+        typename ScalarBinaryOpTraits<AlphaType, typename Rhs::Scalar>::ReturnType rhs_j(alpha * rhs.coeff(j,c));
+        for(LhsInnerIterator it(lhsEval,j); it ;++it)
+          res.coeffRef(it.index(),c) += it.value() * rhs_j;
+      }
+    }
+  }
+};
+
+template<typename SparseLhsType, typename DenseRhsType, typename DenseResType>
+struct sparse_time_dense_product_impl<SparseLhsType,DenseRhsType,DenseResType, typename DenseResType::Scalar, RowMajor, false>
+{
+  typedef typename internal::remove_all<SparseLhsType>::type Lhs;
+  typedef typename internal::remove_all<DenseRhsType>::type Rhs;
+  typedef typename internal::remove_all<DenseResType>::type Res;
+  typedef evaluator<Lhs> LhsEval;
+  typedef typename LhsEval::InnerIterator LhsInnerIterator;
+  static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const typename Res::Scalar& alpha)
+  {
+    Index n = lhs.rows();
+    LhsEval lhsEval(lhs);
+
+#ifdef EIGEN_HAS_OPENMP
+    Eigen::initParallel();
+    Index threads = Eigen::nbThreads();
+    // This 20000 threshold has been found experimentally on 2D and 3D Poisson problems.
+    // It basically represents the minimal amount of work to be done to be worth it.
+    if(threads>1 && lhsEval.nonZerosEstimate()*rhs.cols() > 20000)
+    {
+      #pragma omp parallel for schedule(dynamic,(n+threads*4-1)/(threads*4)) num_threads(threads)
+      for(Index i=0; i<n; ++i)
+        processRow(lhsEval,rhs,res,alpha,i);
+    }
+    else
+#endif
+    {
+      for(Index i=0; i<n; ++i)
+        processRow(lhsEval, rhs, res, alpha, i);
+    }
+  }
+
+  static void processRow(const LhsEval& lhsEval, const DenseRhsType& rhs, Res& res, const typename Res::Scalar& alpha, Index i)
+  {
+    typename Res::RowXpr res_i(res.row(i));
+    for(LhsInnerIterator it(lhsEval,i); it ;++it)
+      res_i += (alpha*it.value()) * rhs.row(it.index());
+  }
+};
+
+template<typename SparseLhsType, typename DenseRhsType, typename DenseResType>
+struct sparse_time_dense_product_impl<SparseLhsType,DenseRhsType,DenseResType, typename DenseResType::Scalar, ColMajor, false>
+{
+  typedef typename internal::remove_all<SparseLhsType>::type Lhs;
+  typedef typename internal::remove_all<DenseRhsType>::type Rhs;
+  typedef typename internal::remove_all<DenseResType>::type Res;
+  typedef typename evaluator<Lhs>::InnerIterator LhsInnerIterator;
+  static void run(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const typename Res::Scalar& alpha)
+  {
+    evaluator<Lhs> lhsEval(lhs);
+    for(Index j=0; j<lhs.outerSize(); ++j)
+    {
+      typename Rhs::ConstRowXpr rhs_j(rhs.row(j));
+      for(LhsInnerIterator it(lhsEval,j); it ;++it)
+        res.row(it.index()) += (alpha*it.value()) * rhs_j;
+    }
+  }
+};
+
+template<typename SparseLhsType, typename DenseRhsType, typename DenseResType,typename AlphaType>
+inline void sparse_time_dense_product(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const AlphaType& alpha)
+{
+  sparse_time_dense_product_impl<SparseLhsType,DenseRhsType,DenseResType, AlphaType>::run(lhs, rhs, res, alpha);
+}
+
+} // end namespace internal
+
+namespace internal {
+
+template<typename Lhs, typename Rhs, int ProductType>
+struct generic_product_impl<Lhs, Rhs, SparseShape, DenseShape, ProductType>
+ : generic_product_impl_base<Lhs,Rhs,generic_product_impl<Lhs,Rhs,SparseShape,DenseShape,ProductType> >
+{
+  typedef typename Product<Lhs,Rhs>::Scalar Scalar;
+  
+  template<typename Dest>
+  static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
+  {
+    typedef typename nested_eval<Lhs,((Rhs::Flags&RowMajorBit)==0) ? 1 : Rhs::ColsAtCompileTime>::type LhsNested;
+    typedef typename nested_eval<Rhs,((Lhs::Flags&RowMajorBit)==0) ? 1 : Dynamic>::type RhsNested;
+    LhsNested lhsNested(lhs);
+    RhsNested rhsNested(rhs);
+    internal::sparse_time_dense_product(lhsNested, rhsNested, dst, alpha);
+  }
+};
+
+template<typename Lhs, typename Rhs, int ProductType>
+struct generic_product_impl<Lhs, Rhs, SparseTriangularShape, DenseShape, ProductType>
+  : generic_product_impl<Lhs, Rhs, SparseShape, DenseShape, ProductType>
+{};
+
+template<typename Lhs, typename Rhs, int ProductType>
+struct generic_product_impl<Lhs, Rhs, DenseShape, SparseShape, ProductType>
+  : generic_product_impl_base<Lhs,Rhs,generic_product_impl<Lhs,Rhs,DenseShape,SparseShape,ProductType> >
+{
+  typedef typename Product<Lhs,Rhs>::Scalar Scalar;
+  
+  template<typename Dst>
+  static void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
+  {
+    typedef typename nested_eval<Lhs,((Rhs::Flags&RowMajorBit)==0) ? Dynamic : 1>::type LhsNested;
+    typedef typename nested_eval<Rhs,((Lhs::Flags&RowMajorBit)==RowMajorBit) ? 1 : Lhs::RowsAtCompileTime>::type RhsNested;
+    LhsNested lhsNested(lhs);
+    RhsNested rhsNested(rhs);
+    
+    // transpose everything
+    Transpose<Dst> dstT(dst);
+    internal::sparse_time_dense_product(rhsNested.transpose(), lhsNested.transpose(), dstT, alpha);
+  }
+};
+
+template<typename Lhs, typename Rhs, int ProductType>
+struct generic_product_impl<Lhs, Rhs, DenseShape, SparseTriangularShape, ProductType>
+  : generic_product_impl<Lhs, Rhs, DenseShape, SparseShape, ProductType>
+{};
+
+template<typename LhsT, typename RhsT, bool NeedToTranspose>
+struct sparse_dense_outer_product_evaluator
+{
+protected:
+  typedef typename conditional<NeedToTranspose,RhsT,LhsT>::type Lhs1;
+  typedef typename conditional<NeedToTranspose,LhsT,RhsT>::type ActualRhs;
+  typedef Product<LhsT,RhsT,DefaultProduct> ProdXprType;
+  
+  // if the actual left-hand side is a dense vector,
+  // then build a sparse-view so that we can seamlessly iterate over it.
+  typedef typename conditional<is_same<typename internal::traits<Lhs1>::StorageKind,Sparse>::value,
+            Lhs1, SparseView<Lhs1> >::type ActualLhs;
+  typedef typename conditional<is_same<typename internal::traits<Lhs1>::StorageKind,Sparse>::value,
+            Lhs1 const&, SparseView<Lhs1> >::type LhsArg;
+            
+  typedef evaluator<ActualLhs> LhsEval;
+  typedef evaluator<ActualRhs> RhsEval;
+  typedef typename evaluator<ActualLhs>::InnerIterator LhsIterator;
+  typedef typename ProdXprType::Scalar Scalar;
+  
+public:
+  enum {
+    Flags = NeedToTranspose ? RowMajorBit : 0,
+    CoeffReadCost = HugeCost
+  };
+  
+  class InnerIterator : public LhsIterator
+  {
+  public:
+    InnerIterator(const sparse_dense_outer_product_evaluator &xprEval, Index outer)
+      : LhsIterator(xprEval.m_lhsXprImpl, 0),
+        m_outer(outer),
+        m_empty(false),
+        m_factor(get(xprEval.m_rhsXprImpl, outer, typename internal::traits<ActualRhs>::StorageKind() ))
+    {}
+    
+    EIGEN_STRONG_INLINE Index outer() const { return m_outer; }
+    EIGEN_STRONG_INLINE Index row()   const { return NeedToTranspose ? m_outer : LhsIterator::index(); }
+    EIGEN_STRONG_INLINE Index col()   const { return NeedToTranspose ? LhsIterator::index() : m_outer; }
+
+    EIGEN_STRONG_INLINE Scalar value() const { return LhsIterator::value() * m_factor; }
+    EIGEN_STRONG_INLINE operator bool() const { return LhsIterator::operator bool() && (!m_empty); }
+    
+  protected:
+    Scalar get(const RhsEval &rhs, Index outer, Dense = Dense()) const
+    {
+      return rhs.coeff(outer);
+    }
+    
+    Scalar get(const RhsEval &rhs, Index outer, Sparse = Sparse())
+    {
+      typename RhsEval::InnerIterator it(rhs, outer);
+      if (it && it.index()==0 && it.value()!=Scalar(0))
+        return it.value();
+      m_empty = true;
+      return Scalar(0);
+    }
+    
+    Index m_outer;
+    bool m_empty;
+    Scalar m_factor;
+  };
+  
+  sparse_dense_outer_product_evaluator(const Lhs1 &lhs, const ActualRhs &rhs)
+     : m_lhs(lhs), m_lhsXprImpl(m_lhs), m_rhsXprImpl(rhs)
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+  
+  // transpose case
+  sparse_dense_outer_product_evaluator(const ActualRhs &rhs, const Lhs1 &lhs)
+     : m_lhs(lhs), m_lhsXprImpl(m_lhs), m_rhsXprImpl(rhs)
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+    
+protected:
+  const LhsArg m_lhs;
+  evaluator<ActualLhs> m_lhsXprImpl;
+  evaluator<ActualRhs> m_rhsXprImpl;
+};
+
+// sparse * dense outer product
+template<typename Lhs, typename Rhs>
+struct product_evaluator<Product<Lhs, Rhs, DefaultProduct>, OuterProduct, SparseShape, DenseShape>
+  : sparse_dense_outer_product_evaluator<Lhs,Rhs, Lhs::IsRowMajor>
+{
+  typedef sparse_dense_outer_product_evaluator<Lhs,Rhs, Lhs::IsRowMajor> Base;
+  
+  typedef Product<Lhs, Rhs> XprType;
+  typedef typename XprType::PlainObject PlainObject;
+
+  explicit product_evaluator(const XprType& xpr)
+    : Base(xpr.lhs(), xpr.rhs())
+  {}
+  
+};
+
+template<typename Lhs, typename Rhs>
+struct product_evaluator<Product<Lhs, Rhs, DefaultProduct>, OuterProduct, DenseShape, SparseShape>
+  : sparse_dense_outer_product_evaluator<Lhs,Rhs, Rhs::IsRowMajor>
+{
+  typedef sparse_dense_outer_product_evaluator<Lhs,Rhs, Rhs::IsRowMajor> Base;
+  
+  typedef Product<Lhs, Rhs> XprType;
+  typedef typename XprType::PlainObject PlainObject;
+
+  explicit product_evaluator(const XprType& xpr)
+    : Base(xpr.lhs(), xpr.rhs())
+  {}
+  
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSEDENSEPRODUCT_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDiagonalProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDiagonalProduct.h
new file mode 100644
index 0000000..941c03b
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDiagonalProduct.h
@@ -0,0 +1,138 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_DIAGONAL_PRODUCT_H
+#define EIGEN_SPARSE_DIAGONAL_PRODUCT_H
+
+namespace Eigen { 
+
+// The product of a diagonal matrix with a sparse matrix can be easily
+// implemented using expression template.
+// We have two consider very different cases:
+// 1 - diag * row-major sparse
+//     => each inner vector <=> scalar * sparse vector product
+//     => so we can reuse CwiseUnaryOp::InnerIterator
+// 2 - diag * col-major sparse
+//     => each inner vector <=> densevector * sparse vector cwise product
+//     => again, we can reuse specialization of CwiseBinaryOp::InnerIterator
+//        for that particular case
+// The two other cases are symmetric.
+
+namespace internal {
+
+enum {
+  SDP_AsScalarProduct,
+  SDP_AsCwiseProduct
+};
+  
+template<typename SparseXprType, typename DiagonalCoeffType, int SDP_Tag>
+struct sparse_diagonal_product_evaluator;
+
+template<typename Lhs, typename Rhs, int ProductTag>
+struct product_evaluator<Product<Lhs, Rhs, DefaultProduct>, ProductTag, DiagonalShape, SparseShape>
+  : public sparse_diagonal_product_evaluator<Rhs, typename Lhs::DiagonalVectorType, Rhs::Flags&RowMajorBit?SDP_AsScalarProduct:SDP_AsCwiseProduct>
+{
+  typedef Product<Lhs, Rhs, DefaultProduct> XprType;
+  enum { CoeffReadCost = HugeCost, Flags = Rhs::Flags&RowMajorBit, Alignment = 0 }; // FIXME CoeffReadCost & Flags
+  
+  typedef sparse_diagonal_product_evaluator<Rhs, typename Lhs::DiagonalVectorType, Rhs::Flags&RowMajorBit?SDP_AsScalarProduct:SDP_AsCwiseProduct> Base;
+  explicit product_evaluator(const XprType& xpr) : Base(xpr.rhs(), xpr.lhs().diagonal()) {}
+};
+
+template<typename Lhs, typename Rhs, int ProductTag>
+struct product_evaluator<Product<Lhs, Rhs, DefaultProduct>, ProductTag, SparseShape, DiagonalShape>
+  : public sparse_diagonal_product_evaluator<Lhs, Transpose<const typename Rhs::DiagonalVectorType>, Lhs::Flags&RowMajorBit?SDP_AsCwiseProduct:SDP_AsScalarProduct>
+{
+  typedef Product<Lhs, Rhs, DefaultProduct> XprType;
+  enum { CoeffReadCost = HugeCost, Flags = Lhs::Flags&RowMajorBit, Alignment = 0 }; // FIXME CoeffReadCost & Flags
+  
+  typedef sparse_diagonal_product_evaluator<Lhs, Transpose<const typename Rhs::DiagonalVectorType>, Lhs::Flags&RowMajorBit?SDP_AsCwiseProduct:SDP_AsScalarProduct> Base;
+  explicit product_evaluator(const XprType& xpr) : Base(xpr.lhs(), xpr.rhs().diagonal().transpose()) {}
+};
+
+template<typename SparseXprType, typename DiagonalCoeffType>
+struct sparse_diagonal_product_evaluator<SparseXprType, DiagonalCoeffType, SDP_AsScalarProduct>
+{
+protected:
+  typedef typename evaluator<SparseXprType>::InnerIterator SparseXprInnerIterator;
+  typedef typename SparseXprType::Scalar Scalar;
+  
+public:
+  class InnerIterator : public SparseXprInnerIterator
+  {
+  public:
+    InnerIterator(const sparse_diagonal_product_evaluator &xprEval, Index outer)
+      : SparseXprInnerIterator(xprEval.m_sparseXprImpl, outer),
+        m_coeff(xprEval.m_diagCoeffImpl.coeff(outer))
+    {}
+    
+    EIGEN_STRONG_INLINE Scalar value() const { return m_coeff * SparseXprInnerIterator::value(); }
+  protected:
+    typename DiagonalCoeffType::Scalar m_coeff;
+  };
+  
+  sparse_diagonal_product_evaluator(const SparseXprType &sparseXpr, const DiagonalCoeffType &diagCoeff)
+    : m_sparseXprImpl(sparseXpr), m_diagCoeffImpl(diagCoeff)
+  {}
+
+  Index nonZerosEstimate() const { return m_sparseXprImpl.nonZerosEstimate(); }
+    
+protected:
+  evaluator<SparseXprType> m_sparseXprImpl;
+  evaluator<DiagonalCoeffType> m_diagCoeffImpl;
+};
+
+
+template<typename SparseXprType, typename DiagCoeffType>
+struct sparse_diagonal_product_evaluator<SparseXprType, DiagCoeffType, SDP_AsCwiseProduct>
+{
+  typedef typename SparseXprType::Scalar Scalar;
+  typedef typename SparseXprType::StorageIndex StorageIndex;
+  
+  typedef typename nested_eval<DiagCoeffType,SparseXprType::IsRowMajor ? SparseXprType::RowsAtCompileTime
+                                                                       : SparseXprType::ColsAtCompileTime>::type DiagCoeffNested;
+  
+  class InnerIterator
+  {
+    typedef typename evaluator<SparseXprType>::InnerIterator SparseXprIter;
+  public:
+    InnerIterator(const sparse_diagonal_product_evaluator &xprEval, Index outer)
+      : m_sparseIter(xprEval.m_sparseXprEval, outer), m_diagCoeffNested(xprEval.m_diagCoeffNested)
+    {}
+    
+    inline Scalar value() const { return m_sparseIter.value() * m_diagCoeffNested.coeff(index()); }
+    inline StorageIndex index() const  { return m_sparseIter.index(); }
+    inline Index outer() const  { return m_sparseIter.outer(); }
+    inline Index col() const    { return SparseXprType::IsRowMajor ? m_sparseIter.index() : m_sparseIter.outer(); }
+    inline Index row() const    { return SparseXprType::IsRowMajor ? m_sparseIter.outer() : m_sparseIter.index(); }
+    
+    EIGEN_STRONG_INLINE InnerIterator& operator++() { ++m_sparseIter; return *this; }
+    inline operator bool() const  { return m_sparseIter; }
+    
+  protected:
+    SparseXprIter m_sparseIter;
+    DiagCoeffNested m_diagCoeffNested;
+  };
+  
+  sparse_diagonal_product_evaluator(const SparseXprType &sparseXpr, const DiagCoeffType &diagCoeff)
+    : m_sparseXprEval(sparseXpr), m_diagCoeffNested(diagCoeff)
+  {}
+
+  Index nonZerosEstimate() const { return m_sparseXprEval.nonZerosEstimate(); }
+    
+protected:
+  evaluator<SparseXprType> m_sparseXprEval;
+  DiagCoeffNested m_diagCoeffNested;
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_DIAGONAL_PRODUCT_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDot.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDot.h
new file mode 100644
index 0000000..38bc4aa
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseDot.h
@@ -0,0 +1,98 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_DOT_H
+#define EIGEN_SPARSE_DOT_H
+
+namespace Eigen { 
+
+template<typename Derived>
+template<typename OtherDerived>
+typename internal::traits<Derived>::Scalar
+SparseMatrixBase<Derived>::dot(const MatrixBase<OtherDerived>& other) const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived)
+  EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
+    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+
+  eigen_assert(size() == other.size());
+  eigen_assert(other.size()>0 && "you are using a non initialized vector");
+
+  internal::evaluator<Derived> thisEval(derived());
+  typename internal::evaluator<Derived>::InnerIterator i(thisEval, 0);
+  Scalar res(0);
+  while (i)
+  {
+    res += numext::conj(i.value()) * other.coeff(i.index());
+    ++i;
+  }
+  return res;
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+typename internal::traits<Derived>::Scalar
+SparseMatrixBase<Derived>::dot(const SparseMatrixBase<OtherDerived>& other) const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived)
+  EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
+    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+
+  eigen_assert(size() == other.size());
+
+  internal::evaluator<Derived> thisEval(derived());
+  typename internal::evaluator<Derived>::InnerIterator i(thisEval, 0);
+  
+  internal::evaluator<OtherDerived>  otherEval(other.derived());
+  typename internal::evaluator<OtherDerived>::InnerIterator j(otherEval, 0);
+
+  Scalar res(0);
+  while (i && j)
+  {
+    if (i.index()==j.index())
+    {
+      res += numext::conj(i.value()) * j.value();
+      ++i; ++j;
+    }
+    else if (i.index()<j.index())
+      ++i;
+    else
+      ++j;
+  }
+  return res;
+}
+
+template<typename Derived>
+inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
+SparseMatrixBase<Derived>::squaredNorm() const
+{
+  return numext::real((*this).cwiseAbs2().sum());
+}
+
+template<typename Derived>
+inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
+SparseMatrixBase<Derived>::norm() const
+{
+  using std::sqrt;
+  return sqrt(squaredNorm());
+}
+
+template<typename Derived>
+inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
+SparseMatrixBase<Derived>::blueNorm() const
+{
+  return internal::blueNorm_impl(*this);
+}
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_DOT_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseFuzzy.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseFuzzy.h
new file mode 100644
index 0000000..7d47eb9
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseFuzzy.h
@@ -0,0 +1,29 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_FUZZY_H
+#define EIGEN_SPARSE_FUZZY_H
+
+namespace Eigen {
+  
+template<typename Derived>
+template<typename OtherDerived>
+bool SparseMatrixBase<Derived>::isApprox(const SparseMatrixBase<OtherDerived>& other, const RealScalar &prec) const
+{
+  const typename internal::nested_eval<Derived,2,PlainObject>::type actualA(derived());
+  typename internal::conditional<bool(IsRowMajor)==bool(OtherDerived::IsRowMajor),
+    const typename internal::nested_eval<OtherDerived,2,PlainObject>::type,
+    const PlainObject>::type actualB(other.derived());
+
+  return (actualA - actualB).squaredNorm() <= prec * prec * numext::mini(actualA.squaredNorm(), actualB.squaredNorm());
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_FUZZY_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMap.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMap.h
new file mode 100644
index 0000000..f99be33
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMap.h
@@ -0,0 +1,305 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_MAP_H
+#define EIGEN_SPARSE_MAP_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct traits<Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+  : public traits<SparseMatrix<MatScalar,MatOptions,MatIndex> >
+{
+  typedef SparseMatrix<MatScalar,MatOptions,MatIndex> PlainObjectType;
+  typedef traits<PlainObjectType> TraitsBase;
+  enum {
+    Flags = TraitsBase::Flags & (~NestByRefBit)
+  };
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct traits<Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+  : public traits<SparseMatrix<MatScalar,MatOptions,MatIndex> >
+{
+  typedef SparseMatrix<MatScalar,MatOptions,MatIndex> PlainObjectType;
+  typedef traits<PlainObjectType> TraitsBase;
+  enum {
+    Flags = TraitsBase::Flags & (~ (NestByRefBit | LvalueBit))
+  };
+};
+
+} // end namespace internal
+
+template<typename Derived,
+         int Level = internal::accessors_level<Derived>::has_write_access ? WriteAccessors : ReadOnlyAccessors
+> class SparseMapBase;
+
+/** \ingroup SparseCore_Module
+  * class SparseMapBase
+  * \brief Common base class for Map and Ref instance of sparse matrix and vector.
+  */
+template<typename Derived>
+class SparseMapBase<Derived,ReadOnlyAccessors>
+  : public SparseCompressedBase<Derived>
+{
+  public:
+    typedef SparseCompressedBase<Derived> Base;
+    typedef typename Base::Scalar Scalar;
+    typedef typename Base::StorageIndex StorageIndex;
+    enum { IsRowMajor = Base::IsRowMajor };
+    using Base::operator=;
+  protected:
+    
+    typedef typename internal::conditional<
+                         bool(internal::is_lvalue<Derived>::value),
+                         Scalar *, const Scalar *>::type ScalarPointer;
+    typedef typename internal::conditional<
+                         bool(internal::is_lvalue<Derived>::value),
+                         StorageIndex *, const StorageIndex *>::type IndexPointer;
+
+    Index   m_outerSize;
+    Index   m_innerSize;
+    Array<StorageIndex,2,1>  m_zero_nnz;
+    IndexPointer  m_outerIndex;
+    IndexPointer  m_innerIndices;
+    ScalarPointer m_values;
+    IndexPointer  m_innerNonZeros;
+
+  public:
+
+    /** \copydoc SparseMatrixBase::rows() */
+    inline Index rows() const { return IsRowMajor ? m_outerSize : m_innerSize; }
+    /** \copydoc SparseMatrixBase::cols() */
+    inline Index cols() const { return IsRowMajor ? m_innerSize : m_outerSize; }
+    /** \copydoc SparseMatrixBase::innerSize() */
+    inline Index innerSize() const { return m_innerSize; }
+    /** \copydoc SparseMatrixBase::outerSize() */
+    inline Index outerSize() const { return m_outerSize; }
+    /** \copydoc SparseCompressedBase::nonZeros */
+    inline Index nonZeros() const { return m_zero_nnz[1]; }
+    
+    /** \copydoc SparseCompressedBase::isCompressed */
+    bool isCompressed() const { return m_innerNonZeros==0; }
+
+    //----------------------------------------
+    // direct access interface
+    /** \copydoc SparseMatrix::valuePtr */
+    inline const Scalar* valuePtr() const { return m_values; }
+    /** \copydoc SparseMatrix::innerIndexPtr */
+    inline const StorageIndex* innerIndexPtr() const { return m_innerIndices; }
+    /** \copydoc SparseMatrix::outerIndexPtr */
+    inline const StorageIndex* outerIndexPtr() const { return m_outerIndex; }
+    /** \copydoc SparseMatrix::innerNonZeroPtr */
+    inline const StorageIndex* innerNonZeroPtr() const { return m_innerNonZeros; }
+    //----------------------------------------
+
+    /** \copydoc SparseMatrix::coeff */
+    inline Scalar coeff(Index row, Index col) const
+    {
+      const Index outer = IsRowMajor ? row : col;
+      const Index inner = IsRowMajor ? col : row;
+
+      Index start = m_outerIndex[outer];
+      Index end = isCompressed() ? m_outerIndex[outer+1] : start + m_innerNonZeros[outer];
+      if (start==end)
+        return Scalar(0);
+      else if (end>0 && inner==m_innerIndices[end-1])
+        return m_values[end-1];
+      // ^^  optimization: let's first check if it is the last coefficient
+      // (very common in high level algorithms)
+
+      const StorageIndex* r = std::lower_bound(&m_innerIndices[start],&m_innerIndices[end-1],inner);
+      const Index id = r-&m_innerIndices[0];
+      return ((*r==inner) && (id<end)) ? m_values[id] : Scalar(0);
+    }
+
+    inline SparseMapBase(Index rows, Index cols, Index nnz, IndexPointer outerIndexPtr, IndexPointer innerIndexPtr,
+                              ScalarPointer valuePtr, IndexPointer innerNonZerosPtr = 0)
+      : m_outerSize(IsRowMajor?rows:cols), m_innerSize(IsRowMajor?cols:rows), m_zero_nnz(0,internal::convert_index<StorageIndex>(nnz)), m_outerIndex(outerIndexPtr),
+        m_innerIndices(innerIndexPtr), m_values(valuePtr), m_innerNonZeros(innerNonZerosPtr)
+    {}
+
+    // for vectors
+    inline SparseMapBase(Index size, Index nnz, IndexPointer innerIndexPtr, ScalarPointer valuePtr)
+      : m_outerSize(1), m_innerSize(size), m_zero_nnz(0,internal::convert_index<StorageIndex>(nnz)), m_outerIndex(m_zero_nnz.data()),
+        m_innerIndices(innerIndexPtr), m_values(valuePtr), m_innerNonZeros(0)
+    {}
+
+    /** Empty destructor */
+    inline ~SparseMapBase() {}
+
+  protected:
+    inline SparseMapBase() {}
+};
+
+/** \ingroup SparseCore_Module
+  * class SparseMapBase
+  * \brief Common base class for writable Map and Ref instance of sparse matrix and vector.
+  */
+template<typename Derived>
+class SparseMapBase<Derived,WriteAccessors>
+  : public SparseMapBase<Derived,ReadOnlyAccessors>
+{
+    typedef MapBase<Derived, ReadOnlyAccessors> ReadOnlyMapBase;
+    
+  public:
+    typedef SparseMapBase<Derived, ReadOnlyAccessors> Base;
+    typedef typename Base::Scalar Scalar;
+    typedef typename Base::StorageIndex StorageIndex;
+    enum { IsRowMajor = Base::IsRowMajor };
+    
+    using Base::operator=;
+
+  public:
+    
+    //----------------------------------------
+    // direct access interface
+    using Base::valuePtr;
+    using Base::innerIndexPtr;
+    using Base::outerIndexPtr;
+    using Base::innerNonZeroPtr;
+    /** \copydoc SparseMatrix::valuePtr */
+    inline Scalar* valuePtr()              { return Base::m_values; }
+    /** \copydoc SparseMatrix::innerIndexPtr */
+    inline StorageIndex* innerIndexPtr()   { return Base::m_innerIndices; }
+    /** \copydoc SparseMatrix::outerIndexPtr */
+    inline StorageIndex* outerIndexPtr()   { return Base::m_outerIndex; }
+    /** \copydoc SparseMatrix::innerNonZeroPtr */
+    inline StorageIndex* innerNonZeroPtr() { return Base::m_innerNonZeros; }
+    //----------------------------------------
+
+    /** \copydoc SparseMatrix::coeffRef */
+    inline Scalar& coeffRef(Index row, Index col)
+    {
+      const Index outer = IsRowMajor ? row : col;
+      const Index inner = IsRowMajor ? col : row;
+
+      Index start = Base::m_outerIndex[outer];
+      Index end = Base::isCompressed() ? Base::m_outerIndex[outer+1] : start + Base::m_innerNonZeros[outer];
+      eigen_assert(end>=start && "you probably called coeffRef on a non finalized matrix");
+      eigen_assert(end>start && "coeffRef cannot be called on a zero coefficient");
+      StorageIndex* r = std::lower_bound(&Base::m_innerIndices[start],&Base::m_innerIndices[end],inner);
+      const Index id = r - &Base::m_innerIndices[0];
+      eigen_assert((*r==inner) && (id<end) && "coeffRef cannot be called on a zero coefficient");
+      return const_cast<Scalar*>(Base::m_values)[id];
+    }
+    
+    inline SparseMapBase(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr, StorageIndex* innerIndexPtr,
+                         Scalar* valuePtr, StorageIndex* innerNonZerosPtr = 0)
+      : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZerosPtr)
+    {}
+
+    // for vectors
+    inline SparseMapBase(Index size, Index nnz, StorageIndex* innerIndexPtr, Scalar* valuePtr)
+      : Base(size, nnz, innerIndexPtr, valuePtr)
+    {}
+
+    /** Empty destructor */
+    inline ~SparseMapBase() {}
+
+  protected:
+    inline SparseMapBase() {}
+};
+
+/** \ingroup SparseCore_Module
+  *
+  * \brief Specialization of class Map for SparseMatrix-like storage.
+  *
+  * \tparam SparseMatrixType the equivalent sparse matrix type of the referenced data, it must be a template instance of class SparseMatrix.
+  *
+  * \sa class Map, class SparseMatrix, class Ref<SparseMatrixType,Options>
+  */
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType>
+  : public SparseMapBase<Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+#else
+template<typename SparseMatrixType>
+class Map<SparseMatrixType>
+  : public SparseMapBase<Derived,WriteAccessors>
+#endif
+{
+  public:
+    typedef SparseMapBase<Map> Base;
+    EIGEN_SPARSE_PUBLIC_INTERFACE(Map)
+    enum { IsRowMajor = Base::IsRowMajor };
+
+  public:
+
+    /** Constructs a read-write Map to a sparse matrix of size \a rows x \a cols, containing \a nnz non-zero coefficients,
+      * stored as a sparse format as defined by the pointers \a outerIndexPtr, \a innerIndexPtr, and \a valuePtr.
+      * If the optional parameter \a innerNonZerosPtr is the null pointer, then a standard compressed format is assumed.
+      *
+      * This constructor is available only if \c SparseMatrixType is non-const.
+      *
+      * More details on the expected storage schemes are given in the \ref TutorialSparse "manual pages".
+      */
+    inline Map(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr,
+               StorageIndex* innerIndexPtr, Scalar* valuePtr, StorageIndex* innerNonZerosPtr = 0)
+      : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZerosPtr)
+    {}
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** Empty destructor */
+    inline ~Map() {}
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType>
+  : public SparseMapBase<Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+{
+  public:
+    typedef SparseMapBase<Map> Base;
+    EIGEN_SPARSE_PUBLIC_INTERFACE(Map)
+    enum { IsRowMajor = Base::IsRowMajor };
+
+  public:
+#endif
+    /** This is the const version of the above constructor.
+      *
+      * This constructor is available only if \c SparseMatrixType is const, e.g.:
+      * \code Map<const SparseMatrix<double> >  \endcode
+      */
+    inline Map(Index rows, Index cols, Index nnz, const StorageIndex* outerIndexPtr,
+               const StorageIndex* innerIndexPtr, const Scalar* valuePtr, const StorageIndex* innerNonZerosPtr = 0)
+      : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZerosPtr)
+    {}
+
+    /** Empty destructor */
+    inline ~Map() {}
+};
+
+namespace internal {
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+  : evaluator<SparseCompressedBase<Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
+{
+  typedef evaluator<SparseCompressedBase<Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
+  typedef Map<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;  
+  evaluator() : Base() {}
+  explicit evaluator(const XprType &mat) : Base(mat) {}
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+  : evaluator<SparseCompressedBase<Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
+{
+  typedef evaluator<SparseCompressedBase<Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
+  typedef Map<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;  
+  evaluator() : Base() {}
+  explicit evaluator(const XprType &mat) : Base(mat) {}
+};
+
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_MAP_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h
new file mode 100644
index 0000000..616b4a0
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrix.h
@@ -0,0 +1,1518 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSEMATRIX_H
+#define EIGEN_SPARSEMATRIX_H
+
+namespace Eigen { 
+
+/** \ingroup SparseCore_Module
+  *
+  * \class SparseMatrix
+  *
+  * \brief A versatible sparse matrix representation
+  *
+  * This class implements a more versatile variants of the common \em compressed row/column storage format.
+  * Each colmun's (resp. row) non zeros are stored as a pair of value with associated row (resp. colmiun) index.
+  * All the non zeros are stored in a single large buffer. Unlike the \em compressed format, there might be extra
+  * space in between the nonzeros of two successive colmuns (resp. rows) such that insertion of new non-zero
+  * can be done with limited memory reallocation and copies.
+  *
+  * A call to the function makeCompressed() turns the matrix into the standard \em compressed format
+  * compatible with many library.
+  *
+  * More details on this storage sceheme are given in the \ref TutorialSparse "manual pages".
+  *
+  * \tparam _Scalar the scalar type, i.e. the type of the coefficients
+  * \tparam _Options Union of bit flags controlling the storage scheme. Currently the only possibility
+  *                 is ColMajor or RowMajor. The default is 0 which means column-major.
+  * \tparam _StorageIndex the type of the indices. It has to be a \b signed type (e.g., short, int, std::ptrdiff_t). Default is \c int.
+  *
+  * \warning In %Eigen 3.2, the undocumented type \c SparseMatrix::Index was improperly defined as the storage index type (e.g., int),
+  *          whereas it is now (starting from %Eigen 3.3) deprecated and always defined as Eigen::Index.
+  *          Codes making use of \c SparseMatrix::Index, might thus likely have to be changed to use \c SparseMatrix::StorageIndex instead.
+  *
+  * This class can be extended with the help of the plugin mechanism described on the page
+  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_SPARSEMATRIX_PLUGIN.
+  */
+
+namespace internal {
+template<typename _Scalar, int _Options, typename _StorageIndex>
+struct traits<SparseMatrix<_Scalar, _Options, _StorageIndex> >
+{
+  typedef _Scalar Scalar;
+  typedef _StorageIndex StorageIndex;
+  typedef Sparse StorageKind;
+  typedef MatrixXpr XprKind;
+  enum {
+    RowsAtCompileTime = Dynamic,
+    ColsAtCompileTime = Dynamic,
+    MaxRowsAtCompileTime = Dynamic,
+    MaxColsAtCompileTime = Dynamic,
+    Flags = _Options | NestByRefBit | LvalueBit | CompressedAccessBit,
+    SupportedAccessPatterns = InnerRandomAccessPattern
+  };
+};
+
+template<typename _Scalar, int _Options, typename _StorageIndex, int DiagIndex>
+struct traits<Diagonal<SparseMatrix<_Scalar, _Options, _StorageIndex>, DiagIndex> >
+{
+  typedef SparseMatrix<_Scalar, _Options, _StorageIndex> MatrixType;
+  typedef typename ref_selector<MatrixType>::type MatrixTypeNested;
+  typedef typename remove_reference<MatrixTypeNested>::type _MatrixTypeNested;
+
+  typedef _Scalar Scalar;
+  typedef Dense StorageKind;
+  typedef _StorageIndex StorageIndex;
+  typedef MatrixXpr XprKind;
+
+  enum {
+    RowsAtCompileTime = Dynamic,
+    ColsAtCompileTime = 1,
+    MaxRowsAtCompileTime = Dynamic,
+    MaxColsAtCompileTime = 1,
+    Flags = LvalueBit
+  };
+};
+
+template<typename _Scalar, int _Options, typename _StorageIndex, int DiagIndex>
+struct traits<Diagonal<const SparseMatrix<_Scalar, _Options, _StorageIndex>, DiagIndex> >
+ : public traits<Diagonal<SparseMatrix<_Scalar, _Options, _StorageIndex>, DiagIndex> >
+{
+  enum {
+    Flags = 0
+  };
+};
+
+} // end namespace internal
+
+template<typename _Scalar, int _Options, typename _StorageIndex>
+class SparseMatrix
+  : public SparseCompressedBase<SparseMatrix<_Scalar, _Options, _StorageIndex> >
+{
+    typedef SparseCompressedBase<SparseMatrix> Base;
+    using Base::convert_index;
+    friend class SparseVector<_Scalar,0,_StorageIndex>;
+    template<typename, typename, typename, typename, typename>
+    friend struct internal::Assignment;
+  public:
+    using Base::isCompressed;
+    using Base::nonZeros;
+    EIGEN_SPARSE_PUBLIC_INTERFACE(SparseMatrix)
+    using Base::operator+=;
+    using Base::operator-=;
+
+    typedef MappedSparseMatrix<Scalar,Flags> Map;
+    typedef Diagonal<SparseMatrix> DiagonalReturnType;
+    typedef Diagonal<const SparseMatrix> ConstDiagonalReturnType;
+    typedef typename Base::InnerIterator InnerIterator;
+    typedef typename Base::ReverseInnerIterator ReverseInnerIterator;
+    
+
+    using Base::IsRowMajor;
+    typedef internal::CompressedStorage<Scalar,StorageIndex> Storage;
+    enum {
+      Options = _Options
+    };
+
+    typedef typename Base::IndexVector IndexVector;
+    typedef typename Base::ScalarVector ScalarVector;
+  protected:
+    typedef SparseMatrix<Scalar,(Flags&~RowMajorBit)|(IsRowMajor?RowMajorBit:0)> TransposedSparseMatrix;
+
+    Index m_outerSize;
+    Index m_innerSize;
+    StorageIndex* m_outerIndex;
+    StorageIndex* m_innerNonZeros;     // optional, if null then the data is compressed
+    Storage m_data;
+
+  public:
+    
+    /** \returns the number of rows of the matrix */
+    inline Index rows() const { return IsRowMajor ? m_outerSize : m_innerSize; }
+    /** \returns the number of columns of the matrix */
+    inline Index cols() const { return IsRowMajor ? m_innerSize : m_outerSize; }
+
+    /** \returns the number of rows (resp. columns) of the matrix if the storage order column major (resp. row major) */
+    inline Index innerSize() const { return m_innerSize; }
+    /** \returns the number of columns (resp. rows) of the matrix if the storage order column major (resp. row major) */
+    inline Index outerSize() const { return m_outerSize; }
+    
+    /** \returns a const pointer to the array of values.
+      * This function is aimed at interoperability with other libraries.
+      * \sa innerIndexPtr(), outerIndexPtr() */
+    inline const Scalar* valuePtr() const { return m_data.valuePtr(); }
+    /** \returns a non-const pointer to the array of values.
+      * This function is aimed at interoperability with other libraries.
+      * \sa innerIndexPtr(), outerIndexPtr() */
+    inline Scalar* valuePtr() { return m_data.valuePtr(); }
+
+    /** \returns a const pointer to the array of inner indices.
+      * This function is aimed at interoperability with other libraries.
+      * \sa valuePtr(), outerIndexPtr() */
+    inline const StorageIndex* innerIndexPtr() const { return m_data.indexPtr(); }
+    /** \returns a non-const pointer to the array of inner indices.
+      * This function is aimed at interoperability with other libraries.
+      * \sa valuePtr(), outerIndexPtr() */
+    inline StorageIndex* innerIndexPtr() { return m_data.indexPtr(); }
+
+    /** \returns a const pointer to the array of the starting positions of the inner vectors.
+      * This function is aimed at interoperability with other libraries.
+      * \sa valuePtr(), innerIndexPtr() */
+    inline const StorageIndex* outerIndexPtr() const { return m_outerIndex; }
+    /** \returns a non-const pointer to the array of the starting positions of the inner vectors.
+      * This function is aimed at interoperability with other libraries.
+      * \sa valuePtr(), innerIndexPtr() */
+    inline StorageIndex* outerIndexPtr() { return m_outerIndex; }
+
+    /** \returns a const pointer to the array of the number of non zeros of the inner vectors.
+      * This function is aimed at interoperability with other libraries.
+      * \warning it returns the null pointer 0 in compressed mode */
+    inline const StorageIndex* innerNonZeroPtr() const { return m_innerNonZeros; }
+    /** \returns a non-const pointer to the array of the number of non zeros of the inner vectors.
+      * This function is aimed at interoperability with other libraries.
+      * \warning it returns the null pointer 0 in compressed mode */
+    inline StorageIndex* innerNonZeroPtr() { return m_innerNonZeros; }
+
+    /** \internal */
+    inline Storage& data() { return m_data; }
+    /** \internal */
+    inline const Storage& data() const { return m_data; }
+
+    /** \returns the value of the matrix at position \a i, \a j
+      * This function returns Scalar(0) if the element is an explicit \em zero */
+    inline Scalar coeff(Index row, Index col) const
+    {
+      eigen_assert(row>=0 && row<rows() && col>=0 && col<cols());
+      
+      const Index outer = IsRowMajor ? row : col;
+      const Index inner = IsRowMajor ? col : row;
+      Index end = m_innerNonZeros ? m_outerIndex[outer] + m_innerNonZeros[outer] : m_outerIndex[outer+1];
+      return m_data.atInRange(m_outerIndex[outer], end, StorageIndex(inner));
+    }
+
+    /** \returns a non-const reference to the value of the matrix at position \a i, \a j
+      *
+      * If the element does not exist then it is inserted via the insert(Index,Index) function
+      * which itself turns the matrix into a non compressed form if that was not the case.
+      *
+      * This is a O(log(nnz_j)) operation (binary search) plus the cost of insert(Index,Index)
+      * function if the element does not already exist.
+      */
+    inline Scalar& coeffRef(Index row, Index col)
+    {
+      eigen_assert(row>=0 && row<rows() && col>=0 && col<cols());
+      
+      const Index outer = IsRowMajor ? row : col;
+      const Index inner = IsRowMajor ? col : row;
+
+      Index start = m_outerIndex[outer];
+      Index end = m_innerNonZeros ? m_outerIndex[outer] + m_innerNonZeros[outer] : m_outerIndex[outer+1];
+      eigen_assert(end>=start && "you probably called coeffRef on a non finalized matrix");
+      if(end<=start)
+        return insert(row,col);
+      const Index p = m_data.searchLowerIndex(start,end-1,StorageIndex(inner));
+      if((p<end) && (m_data.index(p)==inner))
+        return m_data.value(p);
+      else
+        return insert(row,col);
+    }
+
+    /** \returns a reference to a novel non zero coefficient with coordinates \a row x \a col.
+      * The non zero coefficient must \b not already exist.
+      *
+      * If the matrix \c *this is in compressed mode, then \c *this is turned into uncompressed
+      * mode while reserving room for 2 x this->innerSize() non zeros if reserve(Index) has not been called earlier.
+      * In this case, the insertion procedure is optimized for a \e sequential insertion mode where elements are assumed to be
+      * inserted by increasing outer-indices.
+      * 
+      * If that's not the case, then it is strongly recommended to either use a triplet-list to assemble the matrix, or to first
+      * call reserve(const SizesType &) to reserve the appropriate number of non-zero elements per inner vector.
+      *
+      * Assuming memory has been appropriately reserved, this function performs a sorted insertion in O(1)
+      * if the elements of each inner vector are inserted in increasing inner index order, and in O(nnz_j) for a random insertion.
+      *
+      */
+    Scalar& insert(Index row, Index col);
+
+  public:
+
+    /** Removes all non zeros but keep allocated memory
+      *
+      * This function does not free the currently allocated memory. To release as much as memory as possible,
+      * call \code mat.data().squeeze(); \endcode after resizing it.
+      * 
+      * \sa resize(Index,Index), data()
+      */
+    inline void setZero()
+    {
+      m_data.clear();
+      memset(m_outerIndex, 0, (m_outerSize+1)*sizeof(StorageIndex));
+      if(m_innerNonZeros)
+        memset(m_innerNonZeros, 0, (m_outerSize)*sizeof(StorageIndex));
+    }
+
+    /** Preallocates \a reserveSize non zeros.
+      *
+      * Precondition: the matrix must be in compressed mode. */
+    inline void reserve(Index reserveSize)
+    {
+      eigen_assert(isCompressed() && "This function does not make sense in non compressed mode.");
+      m_data.reserve(reserveSize);
+    }
+    
+    #ifdef EIGEN_PARSED_BY_DOXYGEN
+    /** Preallocates \a reserveSize[\c j] non zeros for each column (resp. row) \c j.
+      *
+      * This function turns the matrix in non-compressed mode.
+      * 
+      * The type \c SizesType must expose the following interface:
+        \code
+        typedef value_type;
+        const value_type& operator[](i) const;
+        \endcode
+      * for \c i in the [0,this->outerSize()[ range.
+      * Typical choices include std::vector<int>, Eigen::VectorXi, Eigen::VectorXi::Constant, etc.
+      */
+    template<class SizesType>
+    inline void reserve(const SizesType& reserveSizes);
+    #else
+    template<class SizesType>
+    inline void reserve(const SizesType& reserveSizes, const typename SizesType::value_type& enableif =
+    #if (!EIGEN_COMP_MSVC) || (EIGEN_COMP_MSVC>=1500) // MSVC 2005 fails to compile with this typename
+        typename
+    #endif
+        SizesType::value_type())
+    {
+      EIGEN_UNUSED_VARIABLE(enableif);
+      reserveInnerVectors(reserveSizes);
+    }
+    #endif // EIGEN_PARSED_BY_DOXYGEN
+  protected:
+    template<class SizesType>
+    inline void reserveInnerVectors(const SizesType& reserveSizes)
+    {
+      if(isCompressed())
+      {
+        Index totalReserveSize = 0;
+        // turn the matrix into non-compressed mode
+        m_innerNonZeros = static_cast<StorageIndex*>(std::malloc(m_outerSize * sizeof(StorageIndex)));
+        if (!m_innerNonZeros) internal::throw_std_bad_alloc();
+        
+        // temporarily use m_innerSizes to hold the new starting points.
+        StorageIndex* newOuterIndex = m_innerNonZeros;
+        
+        StorageIndex count = 0;
+        for(Index j=0; j<m_outerSize; ++j)
+        {
+          newOuterIndex[j] = count;
+          count += reserveSizes[j] + (m_outerIndex[j+1]-m_outerIndex[j]);
+          totalReserveSize += reserveSizes[j];
+        }
+        m_data.reserve(totalReserveSize);
+        StorageIndex previousOuterIndex = m_outerIndex[m_outerSize];
+        for(Index j=m_outerSize-1; j>=0; --j)
+        {
+          StorageIndex innerNNZ = previousOuterIndex - m_outerIndex[j];
+          for(Index i=innerNNZ-1; i>=0; --i)
+          {
+            m_data.index(newOuterIndex[j]+i) = m_data.index(m_outerIndex[j]+i);
+            m_data.value(newOuterIndex[j]+i) = m_data.value(m_outerIndex[j]+i);
+          }
+          previousOuterIndex = m_outerIndex[j];
+          m_outerIndex[j] = newOuterIndex[j];
+          m_innerNonZeros[j] = innerNNZ;
+        }
+        if(m_outerSize>0)
+          m_outerIndex[m_outerSize] = m_outerIndex[m_outerSize-1] + m_innerNonZeros[m_outerSize-1] + reserveSizes[m_outerSize-1];
+        
+        m_data.resize(m_outerIndex[m_outerSize]);
+      }
+      else
+      {
+        StorageIndex* newOuterIndex = static_cast<StorageIndex*>(std::malloc((m_outerSize+1)*sizeof(StorageIndex)));
+        if (!newOuterIndex) internal::throw_std_bad_alloc();
+        
+        StorageIndex count = 0;
+        for(Index j=0; j<m_outerSize; ++j)
+        {
+          newOuterIndex[j] = count;
+          StorageIndex alreadyReserved = (m_outerIndex[j+1]-m_outerIndex[j]) - m_innerNonZeros[j];
+          StorageIndex toReserve = std::max<StorageIndex>(reserveSizes[j], alreadyReserved);
+          count += toReserve + m_innerNonZeros[j];
+        }
+        newOuterIndex[m_outerSize] = count;
+        
+        m_data.resize(count);
+        for(Index j=m_outerSize-1; j>=0; --j)
+        {
+          Index offset = newOuterIndex[j] - m_outerIndex[j];
+          if(offset>0)
+          {
+            StorageIndex innerNNZ = m_innerNonZeros[j];
+            for(Index i=innerNNZ-1; i>=0; --i)
+            {
+              m_data.index(newOuterIndex[j]+i) = m_data.index(m_outerIndex[j]+i);
+              m_data.value(newOuterIndex[j]+i) = m_data.value(m_outerIndex[j]+i);
+            }
+          }
+        }
+        
+        std::swap(m_outerIndex, newOuterIndex);
+        std::free(newOuterIndex);
+      }
+      
+    }
+  public:
+
+    //--- low level purely coherent filling ---
+
+    /** \internal
+      * \returns a reference to the non zero coefficient at position \a row, \a col assuming that:
+      * - the nonzero does not already exist
+      * - the new coefficient is the last one according to the storage order
+      *
+      * Before filling a given inner vector you must call the statVec(Index) function.
+      *
+      * After an insertion session, you should call the finalize() function.
+      *
+      * \sa insert, insertBackByOuterInner, startVec */
+    inline Scalar& insertBack(Index row, Index col)
+    {
+      return insertBackByOuterInner(IsRowMajor?row:col, IsRowMajor?col:row);
+    }
+
+    /** \internal
+      * \sa insertBack, startVec */
+    inline Scalar& insertBackByOuterInner(Index outer, Index inner)
+    {
+      eigen_assert(Index(m_outerIndex[outer+1]) == m_data.size() && "Invalid ordered insertion (invalid outer index)");
+      eigen_assert( (m_outerIndex[outer+1]-m_outerIndex[outer]==0 || m_data.index(m_data.size()-1)<inner) && "Invalid ordered insertion (invalid inner index)");
+      Index p = m_outerIndex[outer+1];
+      ++m_outerIndex[outer+1];
+      m_data.append(Scalar(0), inner);
+      return m_data.value(p);
+    }
+
+    /** \internal
+      * \warning use it only if you know what you are doing */
+    inline Scalar& insertBackByOuterInnerUnordered(Index outer, Index inner)
+    {
+      Index p = m_outerIndex[outer+1];
+      ++m_outerIndex[outer+1];
+      m_data.append(Scalar(0), inner);
+      return m_data.value(p);
+    }
+
+    /** \internal
+      * \sa insertBack, insertBackByOuterInner */
+    inline void startVec(Index outer)
+    {
+      eigen_assert(m_outerIndex[outer]==Index(m_data.size()) && "You must call startVec for each inner vector sequentially");
+      eigen_assert(m_outerIndex[outer+1]==0 && "You must call startVec for each inner vector sequentially");
+      m_outerIndex[outer+1] = m_outerIndex[outer];
+    }
+
+    /** \internal
+      * Must be called after inserting a set of non zero entries using the low level compressed API.
+      */
+    inline void finalize()
+    {
+      if(isCompressed())
+      {
+        StorageIndex size = internal::convert_index<StorageIndex>(m_data.size());
+        Index i = m_outerSize;
+        // find the last filled column
+        while (i>=0 && m_outerIndex[i]==0)
+          --i;
+        ++i;
+        while (i<=m_outerSize)
+        {
+          m_outerIndex[i] = size;
+          ++i;
+        }
+      }
+    }
+
+    //---
+
+    template<typename InputIterators>
+    void setFromTriplets(const InputIterators& begin, const InputIterators& end);
+
+    template<typename InputIterators,typename DupFunctor>
+    void setFromTriplets(const InputIterators& begin, const InputIterators& end, DupFunctor dup_func);
+
+    void sumupDuplicates() { collapseDuplicates(internal::scalar_sum_op<Scalar,Scalar>()); }
+
+    template<typename DupFunctor>
+    void collapseDuplicates(DupFunctor dup_func = DupFunctor());
+
+    //---
+    
+    /** \internal
+      * same as insert(Index,Index) except that the indices are given relative to the storage order */
+    Scalar& insertByOuterInner(Index j, Index i)
+    {
+      return insert(IsRowMajor ? j : i, IsRowMajor ? i : j);
+    }
+
+    /** Turns the matrix into the \em compressed format.
+      */
+    void makeCompressed()
+    {
+      if(isCompressed())
+        return;
+      
+      eigen_internal_assert(m_outerIndex!=0 && m_outerSize>0);
+      
+      Index oldStart = m_outerIndex[1];
+      m_outerIndex[1] = m_innerNonZeros[0];
+      for(Index j=1; j<m_outerSize; ++j)
+      {
+        Index nextOldStart = m_outerIndex[j+1];
+        Index offset = oldStart - m_outerIndex[j];
+        if(offset>0)
+        {
+          for(Index k=0; k<m_innerNonZeros[j]; ++k)
+          {
+            m_data.index(m_outerIndex[j]+k) = m_data.index(oldStart+k);
+            m_data.value(m_outerIndex[j]+k) = m_data.value(oldStart+k);
+          }
+        }
+        m_outerIndex[j+1] = m_outerIndex[j] + m_innerNonZeros[j];
+        oldStart = nextOldStart;
+      }
+      std::free(m_innerNonZeros);
+      m_innerNonZeros = 0;
+      m_data.resize(m_outerIndex[m_outerSize]);
+      m_data.squeeze();
+    }
+
+    /** Turns the matrix into the uncompressed mode */
+    void uncompress()
+    {
+      if(m_innerNonZeros != 0)
+        return; 
+      m_innerNonZeros = static_cast<StorageIndex*>(std::malloc(m_outerSize * sizeof(StorageIndex)));
+      for (Index i = 0; i < m_outerSize; i++)
+      {
+        m_innerNonZeros[i] = m_outerIndex[i+1] - m_outerIndex[i]; 
+      }
+    }
+
+    /** Suppresses all nonzeros which are \b much \b smaller \b than \a reference under the tolerance \a epsilon */
+    void prune(const Scalar& reference, const RealScalar& epsilon = NumTraits<RealScalar>::dummy_precision())
+    {
+      prune(default_prunning_func(reference,epsilon));
+    }
+    
+    /** Turns the matrix into compressed format, and suppresses all nonzeros which do not satisfy the predicate \a keep.
+      * The functor type \a KeepFunc must implement the following function:
+      * \code
+      * bool operator() (const Index& row, const Index& col, const Scalar& value) const;
+      * \endcode
+      * \sa prune(Scalar,RealScalar)
+      */
+    template<typename KeepFunc>
+    void prune(const KeepFunc& keep = KeepFunc())
+    {
+      // TODO optimize the uncompressed mode to avoid moving and allocating the data twice
+      makeCompressed();
+
+      StorageIndex k = 0;
+      for(Index j=0; j<m_outerSize; ++j)
+      {
+        Index previousStart = m_outerIndex[j];
+        m_outerIndex[j] = k;
+        Index end = m_outerIndex[j+1];
+        for(Index i=previousStart; i<end; ++i)
+        {
+          if(keep(IsRowMajor?j:m_data.index(i), IsRowMajor?m_data.index(i):j, m_data.value(i)))
+          {
+            m_data.value(k) = m_data.value(i);
+            m_data.index(k) = m_data.index(i);
+            ++k;
+          }
+        }
+      }
+      m_outerIndex[m_outerSize] = k;
+      m_data.resize(k,0);
+    }
+
+    /** Resizes the matrix to a \a rows x \a cols matrix leaving old values untouched.
+      *
+      * If the sizes of the matrix are decreased, then the matrix is turned to \b uncompressed-mode
+      * and the storage of the out of bounds coefficients is kept and reserved.
+      * Call makeCompressed() to pack the entries and squeeze extra memory.
+      *
+      * \sa reserve(), setZero(), makeCompressed()
+      */
+    void conservativeResize(Index rows, Index cols) 
+    {
+      // No change
+      if (this->rows() == rows && this->cols() == cols) return;
+      
+      // If one dimension is null, then there is nothing to be preserved
+      if(rows==0 || cols==0) return resize(rows,cols);
+
+      Index innerChange = IsRowMajor ? cols - this->cols() : rows - this->rows();
+      Index outerChange = IsRowMajor ? rows - this->rows() : cols - this->cols();
+      StorageIndex newInnerSize = convert_index(IsRowMajor ? cols : rows);
+
+      // Deals with inner non zeros
+      if (m_innerNonZeros)
+      {
+        // Resize m_innerNonZeros
+        StorageIndex *newInnerNonZeros = static_cast<StorageIndex*>(std::realloc(m_innerNonZeros, (m_outerSize + outerChange) * sizeof(StorageIndex)));
+        if (!newInnerNonZeros) internal::throw_std_bad_alloc();
+        m_innerNonZeros = newInnerNonZeros;
+        
+        for(Index i=m_outerSize; i<m_outerSize+outerChange; i++)          
+          m_innerNonZeros[i] = 0;
+      } 
+      else if (innerChange < 0) 
+      {
+        // Inner size decreased: allocate a new m_innerNonZeros
+        m_innerNonZeros = static_cast<StorageIndex*>(std::malloc((m_outerSize + outerChange) * sizeof(StorageIndex)));
+        if (!m_innerNonZeros) internal::throw_std_bad_alloc();
+        for(Index i = 0; i < m_outerSize + (std::min)(outerChange, Index(0)); i++)
+          m_innerNonZeros[i] = m_outerIndex[i+1] - m_outerIndex[i];
+        for(Index i = m_outerSize; i < m_outerSize + outerChange; i++)
+          m_innerNonZeros[i] = 0;
+      }
+      
+      // Change the m_innerNonZeros in case of a decrease of inner size
+      if (m_innerNonZeros && innerChange < 0)
+      {
+        for(Index i = 0; i < m_outerSize + (std::min)(outerChange, Index(0)); i++)
+        {
+          StorageIndex &n = m_innerNonZeros[i];
+          StorageIndex start = m_outerIndex[i];
+          while (n > 0 && m_data.index(start+n-1) >= newInnerSize) --n; 
+        }
+      }
+      
+      m_innerSize = newInnerSize;
+
+      // Re-allocate outer index structure if necessary
+      if (outerChange == 0)
+        return;
+          
+      StorageIndex *newOuterIndex = static_cast<StorageIndex*>(std::realloc(m_outerIndex, (m_outerSize + outerChange + 1) * sizeof(StorageIndex)));
+      if (!newOuterIndex) internal::throw_std_bad_alloc();
+      m_outerIndex = newOuterIndex;
+      if (outerChange > 0)
+      {
+        StorageIndex lastIdx = m_outerSize == 0 ? 0 : m_outerIndex[m_outerSize];
+        for(Index i=m_outerSize; i<m_outerSize+outerChange+1; i++)          
+          m_outerIndex[i] = lastIdx; 
+      }
+      m_outerSize += outerChange;
+    }
+    
+    /** Resizes the matrix to a \a rows x \a cols matrix and initializes it to zero.
+      * 
+      * This function does not free the currently allocated memory. To release as much as memory as possible,
+      * call \code mat.data().squeeze(); \endcode after resizing it.
+      * 
+      * \sa reserve(), setZero()
+      */
+    void resize(Index rows, Index cols)
+    {
+      const Index outerSize = IsRowMajor ? rows : cols;
+      m_innerSize = IsRowMajor ? cols : rows;
+      m_data.clear();
+      if (m_outerSize != outerSize || m_outerSize==0)
+      {
+        std::free(m_outerIndex);
+        m_outerIndex = static_cast<StorageIndex*>(std::malloc((outerSize + 1) * sizeof(StorageIndex)));
+        if (!m_outerIndex) internal::throw_std_bad_alloc();
+        
+        m_outerSize = outerSize;
+      }
+      if(m_innerNonZeros)
+      {
+        std::free(m_innerNonZeros);
+        m_innerNonZeros = 0;
+      }
+      memset(m_outerIndex, 0, (m_outerSize+1)*sizeof(StorageIndex));
+    }
+
+    /** \internal
+      * Resize the nonzero vector to \a size */
+    void resizeNonZeros(Index size)
+    {
+      m_data.resize(size);
+    }
+
+    /** \returns a const expression of the diagonal coefficients. */
+    const ConstDiagonalReturnType diagonal() const { return ConstDiagonalReturnType(*this); }
+    
+    /** \returns a read-write expression of the diagonal coefficients.
+      * \warning If the diagonal entries are written, then all diagonal
+      * entries \b must already exist, otherwise an assertion will be raised.
+      */
+    DiagonalReturnType diagonal() { return DiagonalReturnType(*this); }
+
+    /** Default constructor yielding an empty \c 0 \c x \c 0 matrix */
+    inline SparseMatrix()
+      : m_outerSize(-1), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
+    {
+      check_template_parameters();
+      resize(0, 0);
+    }
+
+    /** Constructs a \a rows \c x \a cols empty matrix */
+    inline SparseMatrix(Index rows, Index cols)
+      : m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
+    {
+      check_template_parameters();
+      resize(rows, cols);
+    }
+
+    /** Constructs a sparse matrix from the sparse expression \a other */
+    template<typename OtherDerived>
+    inline SparseMatrix(const SparseMatrixBase<OtherDerived>& other)
+      : m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
+    {
+      EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
+        YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+      check_template_parameters();
+      const bool needToTranspose = (Flags & RowMajorBit) != (internal::evaluator<OtherDerived>::Flags & RowMajorBit);
+      if (needToTranspose)
+        *this = other.derived();
+      else
+      {
+        #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+          EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+        #endif
+        internal::call_assignment_no_alias(*this, other.derived());
+      }
+    }
+    
+    /** Constructs a sparse matrix from the sparse selfadjoint view \a other */
+    template<typename OtherDerived, unsigned int UpLo>
+    inline SparseMatrix(const SparseSelfAdjointView<OtherDerived, UpLo>& other)
+      : m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
+    {
+      check_template_parameters();
+      Base::operator=(other);
+    }
+
+    /** Copy constructor (it performs a deep copy) */
+    inline SparseMatrix(const SparseMatrix& other)
+      : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
+    {
+      check_template_parameters();
+      *this = other.derived();
+    }
+
+    /** \brief Copy constructor with in-place evaluation */
+    template<typename OtherDerived>
+    SparseMatrix(const ReturnByValue<OtherDerived>& other)
+      : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
+    {
+      check_template_parameters();
+      initAssignment(other);
+      other.evalTo(*this);
+    }
+    
+    /** \brief Copy constructor with in-place evaluation */
+    template<typename OtherDerived>
+    explicit SparseMatrix(const DiagonalBase<OtherDerived>& other)
+      : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0), m_innerNonZeros(0)
+    {
+      check_template_parameters();
+      *this = other.derived();
+    }
+
+    /** Swaps the content of two sparse matrices of the same type.
+      * This is a fast operation that simply swaps the underlying pointers and parameters. */
+    inline void swap(SparseMatrix& other)
+    {
+      //EIGEN_DBG_SPARSE(std::cout << "SparseMatrix:: swap\n");
+      std::swap(m_outerIndex, other.m_outerIndex);
+      std::swap(m_innerSize, other.m_innerSize);
+      std::swap(m_outerSize, other.m_outerSize);
+      std::swap(m_innerNonZeros, other.m_innerNonZeros);
+      m_data.swap(other.m_data);
+    }
+
+    /** Sets *this to the identity matrix.
+      * This function also turns the matrix into compressed mode, and drop any reserved memory. */
+    inline void setIdentity()
+    {
+      eigen_assert(rows() == cols() && "ONLY FOR SQUARED MATRICES");
+      this->m_data.resize(rows());
+      Eigen::Map<IndexVector>(this->m_data.indexPtr(), rows()).setLinSpaced(0, StorageIndex(rows()-1));
+      Eigen::Map<ScalarVector>(this->m_data.valuePtr(), rows()).setOnes();
+      Eigen::Map<IndexVector>(this->m_outerIndex, rows()+1).setLinSpaced(0, StorageIndex(rows()));
+      std::free(m_innerNonZeros);
+      m_innerNonZeros = 0;
+    }
+    inline SparseMatrix& operator=(const SparseMatrix& other)
+    {
+      if (other.isRValue())
+      {
+        swap(other.const_cast_derived());
+      }
+      else if(this!=&other)
+      {
+        #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+          EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+        #endif
+        initAssignment(other);
+        if(other.isCompressed())
+        {
+          internal::smart_copy(other.m_outerIndex, other.m_outerIndex + m_outerSize + 1, m_outerIndex);
+          m_data = other.m_data;
+        }
+        else
+        {
+          Base::operator=(other);
+        }
+      }
+      return *this;
+    }
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    template<typename OtherDerived>
+    inline SparseMatrix& operator=(const EigenBase<OtherDerived>& other)
+    { return Base::operator=(other.derived()); }
+
+    template<typename Lhs, typename Rhs>
+    inline SparseMatrix& operator=(const Product<Lhs,Rhs,AliasFreeProduct>& other);
+#endif // EIGEN_PARSED_BY_DOXYGEN
+
+    template<typename OtherDerived>
+    EIGEN_DONT_INLINE SparseMatrix& operator=(const SparseMatrixBase<OtherDerived>& other);
+
+    friend std::ostream & operator << (std::ostream & s, const SparseMatrix& m)
+    {
+      EIGEN_DBG_SPARSE(
+        s << "Nonzero entries:\n";
+        if(m.isCompressed())
+        {
+          for (Index i=0; i<m.nonZeros(); ++i)
+            s << "(" << m.m_data.value(i) << "," << m.m_data.index(i) << ") ";
+        }
+        else
+        {
+          for (Index i=0; i<m.outerSize(); ++i)
+          {
+            Index p = m.m_outerIndex[i];
+            Index pe = m.m_outerIndex[i]+m.m_innerNonZeros[i];
+            Index k=p;
+            for (; k<pe; ++k) {
+              s << "(" << m.m_data.value(k) << "," << m.m_data.index(k) << ") ";
+            }
+            for (; k<m.m_outerIndex[i+1]; ++k) {
+              s << "(_,_) ";
+            }
+          }
+        }
+        s << std::endl;
+        s << std::endl;
+        s << "Outer pointers:\n";
+        for (Index i=0; i<m.outerSize(); ++i) {
+          s << m.m_outerIndex[i] << " ";
+        }
+        s << " $" << std::endl;
+        if(!m.isCompressed())
+        {
+          s << "Inner non zeros:\n";
+          for (Index i=0; i<m.outerSize(); ++i) {
+            s << m.m_innerNonZeros[i] << " ";
+          }
+          s << " $" << std::endl;
+        }
+        s << std::endl;
+      );
+      s << static_cast<const SparseMatrixBase<SparseMatrix>&>(m);
+      return s;
+    }
+
+    /** Destructor */
+    inline ~SparseMatrix()
+    {
+      std::free(m_outerIndex);
+      std::free(m_innerNonZeros);
+    }
+
+    /** Overloaded for performance */
+    Scalar sum() const;
+    
+#   ifdef EIGEN_SPARSEMATRIX_PLUGIN
+#     include EIGEN_SPARSEMATRIX_PLUGIN
+#   endif
+
+protected:
+
+    template<typename Other>
+    void initAssignment(const Other& other)
+    {
+      resize(other.rows(), other.cols());
+      if(m_innerNonZeros)
+      {
+        std::free(m_innerNonZeros);
+        m_innerNonZeros = 0;
+      }
+    }
+
+    /** \internal
+      * \sa insert(Index,Index) */
+    EIGEN_DONT_INLINE Scalar& insertCompressed(Index row, Index col);
+
+    /** \internal
+      * A vector object that is equal to 0 everywhere but v at the position i */
+    class SingletonVector
+    {
+        StorageIndex m_index;
+        StorageIndex m_value;
+      public:
+        typedef StorageIndex value_type;
+        SingletonVector(Index i, Index v)
+          : m_index(convert_index(i)), m_value(convert_index(v))
+        {}
+
+        StorageIndex operator[](Index i) const { return i==m_index ? m_value : 0; }
+    };
+
+    /** \internal
+      * \sa insert(Index,Index) */
+    EIGEN_DONT_INLINE Scalar& insertUncompressed(Index row, Index col);
+
+public:
+    /** \internal
+      * \sa insert(Index,Index) */
+    EIGEN_STRONG_INLINE Scalar& insertBackUncompressed(Index row, Index col)
+    {
+      const Index outer = IsRowMajor ? row : col;
+      const Index inner = IsRowMajor ? col : row;
+
+      eigen_assert(!isCompressed());
+      eigen_assert(m_innerNonZeros[outer]<=(m_outerIndex[outer+1] - m_outerIndex[outer]));
+
+      Index p = m_outerIndex[outer] + m_innerNonZeros[outer]++;
+      m_data.index(p) = convert_index(inner);
+      return (m_data.value(p) = Scalar(0));
+    }
+protected:
+    struct IndexPosPair {
+      IndexPosPair(Index a_i, Index a_p) : i(a_i), p(a_p) {}
+      Index i;
+      Index p;
+    };
+
+    /** \internal assign \a diagXpr to the diagonal of \c *this
+      * There are different strategies:
+      *   1 - if *this is overwritten (Func==assign_op) or *this is empty, then we can work treat *this as a dense vector expression.
+      *   2 - otherwise, for each diagonal coeff,
+      *     2.a - if it already exists, then we update it,
+      *     2.b - otherwise, if *this is uncompressed and that the current inner-vector has empty room for at least 1 element, then we perform an in-place insertion.
+      *     2.c - otherwise, we'll have to reallocate and copy everything, so instead of doing so for each new element, it is recorded in a std::vector.
+      *   3 - at the end, if some entries failed to be inserted in-place, then we alloc a new buffer, copy each chunk at the right position, and insert the new elements.
+      * 
+      * TODO: some piece of code could be isolated and reused for a general in-place update strategy.
+      * TODO: if we start to defer the insertion of some elements (i.e., case 2.c executed once),
+      *       then it *might* be better to disable case 2.b since they will have to be copied anyway.
+      */
+    template<typename DiagXpr, typename Func>
+    void assignDiagonal(const DiagXpr diagXpr, const Func& assignFunc)
+    {
+      Index n = diagXpr.size();
+
+      const bool overwrite = internal::is_same<Func, internal::assign_op<Scalar,Scalar> >::value;
+      if(overwrite)
+      {
+        if((this->rows()!=n) || (this->cols()!=n))
+          this->resize(n, n);
+      }
+
+      if(m_data.size()==0 || overwrite)
+      {
+        typedef Array<StorageIndex,Dynamic,1> ArrayXI;  
+        this->makeCompressed();
+        this->resizeNonZeros(n);
+        Eigen::Map<ArrayXI>(this->innerIndexPtr(), n).setLinSpaced(0,StorageIndex(n)-1);
+        Eigen::Map<ArrayXI>(this->outerIndexPtr(), n+1).setLinSpaced(0,StorageIndex(n));
+        Eigen::Map<Array<Scalar,Dynamic,1> > values = this->coeffs();
+        values.setZero();
+        internal::call_assignment_no_alias(values, diagXpr, assignFunc);
+      }
+      else
+      {
+        bool isComp = isCompressed();
+        internal::evaluator<DiagXpr> diaEval(diagXpr);
+        std::vector<IndexPosPair> newEntries;
+
+        // 1 - try in-place update and record insertion failures
+        for(Index i = 0; i<n; ++i)
+        {
+          internal::LowerBoundIndex lb = this->lower_bound(i,i);
+          Index p = lb.value;
+          if(lb.found)
+          {
+            // the coeff already exists
+            assignFunc.assignCoeff(m_data.value(p), diaEval.coeff(i));
+          }
+          else if((!isComp) && m_innerNonZeros[i] < (m_outerIndex[i+1]-m_outerIndex[i]))
+          {
+            // non compressed mode with local room for inserting one element
+            m_data.moveChunk(p, p+1, m_outerIndex[i]+m_innerNonZeros[i]-p);
+            m_innerNonZeros[i]++;
+            m_data.value(p) = Scalar(0);
+            m_data.index(p) = StorageIndex(i);
+            assignFunc.assignCoeff(m_data.value(p), diaEval.coeff(i));
+          }
+          else
+          {
+            // defer insertion
+            newEntries.push_back(IndexPosPair(i,p));
+          }
+        }
+        // 2 - insert deferred entries
+        Index n_entries = Index(newEntries.size());
+        if(n_entries>0)
+        {
+          Storage newData(m_data.size()+n_entries);
+          Index prev_p = 0;
+          Index prev_i = 0;
+          for(Index k=0; k<n_entries;++k)
+          {
+            Index i = newEntries[k].i;
+            Index p = newEntries[k].p;
+            internal::smart_copy(m_data.valuePtr()+prev_p, m_data.valuePtr()+p, newData.valuePtr()+prev_p+k);
+            internal::smart_copy(m_data.indexPtr()+prev_p, m_data.indexPtr()+p, newData.indexPtr()+prev_p+k);
+            for(Index j=prev_i;j<i;++j)
+              m_outerIndex[j+1] += k;
+            if(!isComp)
+              m_innerNonZeros[i]++;
+            prev_p = p;
+            prev_i = i;
+            newData.value(p+k) = Scalar(0);
+            newData.index(p+k) = StorageIndex(i);
+            assignFunc.assignCoeff(newData.value(p+k), diaEval.coeff(i));
+          }
+          {
+            internal::smart_copy(m_data.valuePtr()+prev_p, m_data.valuePtr()+m_data.size(), newData.valuePtr()+prev_p+n_entries);
+            internal::smart_copy(m_data.indexPtr()+prev_p, m_data.indexPtr()+m_data.size(), newData.indexPtr()+prev_p+n_entries);
+            for(Index j=prev_i+1;j<=m_outerSize;++j)
+              m_outerIndex[j] += n_entries;
+          }
+          m_data.swap(newData);
+        }
+      }
+    }
+
+private:
+  static void check_template_parameters()
+  {
+    EIGEN_STATIC_ASSERT(NumTraits<StorageIndex>::IsSigned,THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE);
+    EIGEN_STATIC_ASSERT((Options&(ColMajor|RowMajor))==Options,INVALID_MATRIX_TEMPLATE_PARAMETERS);
+  }
+
+  struct default_prunning_func {
+    default_prunning_func(const Scalar& ref, const RealScalar& eps) : reference(ref), epsilon(eps) {}
+    inline bool operator() (const Index&, const Index&, const Scalar& value) const
+    {
+      return !internal::isMuchSmallerThan(value, reference, epsilon);
+    }
+    Scalar reference;
+    RealScalar epsilon;
+  };
+};
+
+namespace internal {
+
+template<typename InputIterator, typename SparseMatrixType, typename DupFunctor>
+void set_from_triplets(const InputIterator& begin, const InputIterator& end, SparseMatrixType& mat, DupFunctor dup_func)
+{
+  enum { IsRowMajor = SparseMatrixType::IsRowMajor };
+  typedef typename SparseMatrixType::Scalar Scalar;
+  typedef typename SparseMatrixType::StorageIndex StorageIndex;
+  SparseMatrix<Scalar,IsRowMajor?ColMajor:RowMajor,StorageIndex> trMat(mat.rows(),mat.cols());
+
+  if(begin!=end)
+  {
+    // pass 1: count the nnz per inner-vector
+    typename SparseMatrixType::IndexVector wi(trMat.outerSize());
+    wi.setZero();
+    for(InputIterator it(begin); it!=end; ++it)
+    {
+      eigen_assert(it->row()>=0 && it->row()<mat.rows() && it->col()>=0 && it->col()<mat.cols());
+      wi(IsRowMajor ? it->col() : it->row())++;
+    }
+
+    // pass 2: insert all the elements into trMat
+    trMat.reserve(wi);
+    for(InputIterator it(begin); it!=end; ++it)
+      trMat.insertBackUncompressed(it->row(),it->col()) = it->value();
+
+    // pass 3:
+    trMat.collapseDuplicates(dup_func);
+  }
+
+  // pass 4: transposed copy -> implicit sorting
+  mat = trMat;
+}
+
+}
+
+
+/** Fill the matrix \c *this with the list of \em triplets defined by the iterator range \a begin - \a end.
+  *
+  * A \em triplet is a tuple (i,j,value) defining a non-zero element.
+  * The input list of triplets does not have to be sorted, and can contains duplicated elements.
+  * In any case, the result is a \b sorted and \b compressed sparse matrix where the duplicates have been summed up.
+  * This is a \em O(n) operation, with \em n the number of triplet elements.
+  * The initial contents of \c *this is destroyed.
+  * The matrix \c *this must be properly resized beforehand using the SparseMatrix(Index,Index) constructor,
+  * or the resize(Index,Index) method. The sizes are not extracted from the triplet list.
+  *
+  * The \a InputIterators value_type must provide the following interface:
+  * \code
+  * Scalar value() const; // the value
+  * Scalar row() const;   // the row index i
+  * Scalar col() const;   // the column index j
+  * \endcode
+  * See for instance the Eigen::Triplet template class.
+  *
+  * Here is a typical usage example:
+  * \code
+    typedef Triplet<double> T;
+    std::vector<T> tripletList;
+    tripletList.reserve(estimation_of_entries);
+    for(...)
+    {
+      // ...
+      tripletList.push_back(T(i,j,v_ij));
+    }
+    SparseMatrixType m(rows,cols);
+    m.setFromTriplets(tripletList.begin(), tripletList.end());
+    // m is ready to go!
+  * \endcode
+  *
+  * \warning The list of triplets is read multiple times (at least twice). Therefore, it is not recommended to define
+  * an abstract iterator over a complex data-structure that would be expensive to evaluate. The triplets should rather
+  * be explicitly stored into a std::vector for instance.
+  */
+template<typename Scalar, int _Options, typename _StorageIndex>
+template<typename InputIterators>
+void SparseMatrix<Scalar,_Options,_StorageIndex>::setFromTriplets(const InputIterators& begin, const InputIterators& end)
+{
+  internal::set_from_triplets<InputIterators, SparseMatrix<Scalar,_Options,_StorageIndex> >(begin, end, *this, internal::scalar_sum_op<Scalar,Scalar>());
+}
+
+/** The same as setFromTriplets but when duplicates are met the functor \a dup_func is applied:
+  * \code
+  * value = dup_func(OldValue, NewValue)
+  * \endcode 
+  * Here is a C++11 example keeping the latest entry only:
+  * \code
+  * mat.setFromTriplets(triplets.begin(), triplets.end(), [] (const Scalar&,const Scalar &b) { return b; });
+  * \endcode
+  */
+template<typename Scalar, int _Options, typename _StorageIndex>
+template<typename InputIterators,typename DupFunctor>
+void SparseMatrix<Scalar,_Options,_StorageIndex>::setFromTriplets(const InputIterators& begin, const InputIterators& end, DupFunctor dup_func)
+{
+  internal::set_from_triplets<InputIterators, SparseMatrix<Scalar,_Options,_StorageIndex>, DupFunctor>(begin, end, *this, dup_func);
+}
+
+/** \internal */
+template<typename Scalar, int _Options, typename _StorageIndex>
+template<typename DupFunctor>
+void SparseMatrix<Scalar,_Options,_StorageIndex>::collapseDuplicates(DupFunctor dup_func)
+{
+  eigen_assert(!isCompressed());
+  // TODO, in practice we should be able to use m_innerNonZeros for that task
+  IndexVector wi(innerSize());
+  wi.fill(-1);
+  StorageIndex count = 0;
+  // for each inner-vector, wi[inner_index] will hold the position of first element into the index/value buffers
+  for(Index j=0; j<outerSize(); ++j)
+  {
+    StorageIndex start   = count;
+    Index oldEnd  = m_outerIndex[j]+m_innerNonZeros[j];
+    for(Index k=m_outerIndex[j]; k<oldEnd; ++k)
+    {
+      Index i = m_data.index(k);
+      if(wi(i)>=start)
+      {
+        // we already meet this entry => accumulate it
+        m_data.value(wi(i)) = dup_func(m_data.value(wi(i)), m_data.value(k));
+      }
+      else
+      {
+        m_data.value(count) = m_data.value(k);
+        m_data.index(count) = m_data.index(k);
+        wi(i) = count;
+        ++count;
+      }
+    }
+    m_outerIndex[j] = start;
+  }
+  m_outerIndex[m_outerSize] = count;
+
+  // turn the matrix into compressed form
+  std::free(m_innerNonZeros);
+  m_innerNonZeros = 0;
+  m_data.resize(m_outerIndex[m_outerSize]);
+}
+
+template<typename Scalar, int _Options, typename _StorageIndex>
+template<typename OtherDerived>
+EIGEN_DONT_INLINE SparseMatrix<Scalar,_Options,_StorageIndex>& SparseMatrix<Scalar,_Options,_StorageIndex>::operator=(const SparseMatrixBase<OtherDerived>& other)
+{
+  EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename OtherDerived::Scalar>::value),
+        YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+
+  #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+    EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+  #endif
+      
+  const bool needToTranspose = (Flags & RowMajorBit) != (internal::evaluator<OtherDerived>::Flags & RowMajorBit);
+  if (needToTranspose)
+  {
+    #ifdef EIGEN_SPARSE_TRANSPOSED_COPY_PLUGIN
+      EIGEN_SPARSE_TRANSPOSED_COPY_PLUGIN
+    #endif
+    // two passes algorithm:
+    //  1 - compute the number of coeffs per dest inner vector
+    //  2 - do the actual copy/eval
+    // Since each coeff of the rhs has to be evaluated twice, let's evaluate it if needed
+    typedef typename internal::nested_eval<OtherDerived,2,typename internal::plain_matrix_type<OtherDerived>::type >::type OtherCopy;
+    typedef typename internal::remove_all<OtherCopy>::type _OtherCopy;
+    typedef internal::evaluator<_OtherCopy> OtherCopyEval;
+    OtherCopy otherCopy(other.derived());
+    OtherCopyEval otherCopyEval(otherCopy);
+
+    SparseMatrix dest(other.rows(),other.cols());
+    Eigen::Map<IndexVector> (dest.m_outerIndex,dest.outerSize()).setZero();
+
+    // pass 1
+    // FIXME the above copy could be merged with that pass
+    for (Index j=0; j<otherCopy.outerSize(); ++j)
+      for (typename OtherCopyEval::InnerIterator it(otherCopyEval, j); it; ++it)
+        ++dest.m_outerIndex[it.index()];
+
+    // prefix sum
+    StorageIndex count = 0;
+    IndexVector positions(dest.outerSize());
+    for (Index j=0; j<dest.outerSize(); ++j)
+    {
+      StorageIndex tmp = dest.m_outerIndex[j];
+      dest.m_outerIndex[j] = count;
+      positions[j] = count;
+      count += tmp;
+    }
+    dest.m_outerIndex[dest.outerSize()] = count;
+    // alloc
+    dest.m_data.resize(count);
+    // pass 2
+    for (StorageIndex j=0; j<otherCopy.outerSize(); ++j)
+    {
+      for (typename OtherCopyEval::InnerIterator it(otherCopyEval, j); it; ++it)
+      {
+        Index pos = positions[it.index()]++;
+        dest.m_data.index(pos) = j;
+        dest.m_data.value(pos) = it.value();
+      }
+    }
+    this->swap(dest);
+    return *this;
+  }
+  else
+  {
+    if(other.isRValue())
+    {
+      initAssignment(other.derived());
+    }
+    // there is no special optimization
+    return Base::operator=(other.derived());
+  }
+}
+
+template<typename _Scalar, int _Options, typename _StorageIndex>
+typename SparseMatrix<_Scalar,_Options,_StorageIndex>::Scalar& SparseMatrix<_Scalar,_Options,_StorageIndex>::insert(Index row, Index col)
+{
+  eigen_assert(row>=0 && row<rows() && col>=0 && col<cols());
+  
+  const Index outer = IsRowMajor ? row : col;
+  const Index inner = IsRowMajor ? col : row;
+  
+  if(isCompressed())
+  {
+    if(nonZeros()==0)
+    {
+      // reserve space if not already done
+      if(m_data.allocatedSize()==0)
+        m_data.reserve(2*m_innerSize);
+      
+      // turn the matrix into non-compressed mode
+      m_innerNonZeros = static_cast<StorageIndex*>(std::malloc(m_outerSize * sizeof(StorageIndex)));
+      if(!m_innerNonZeros) internal::throw_std_bad_alloc();
+      
+      memset(m_innerNonZeros, 0, (m_outerSize)*sizeof(StorageIndex));
+      
+      // pack all inner-vectors to the end of the pre-allocated space
+      // and allocate the entire free-space to the first inner-vector
+      StorageIndex end = convert_index(m_data.allocatedSize());
+      for(Index j=1; j<=m_outerSize; ++j)
+        m_outerIndex[j] = end;
+    }
+    else
+    {
+      // turn the matrix into non-compressed mode
+      m_innerNonZeros = static_cast<StorageIndex*>(std::malloc(m_outerSize * sizeof(StorageIndex)));
+      if(!m_innerNonZeros) internal::throw_std_bad_alloc();
+      for(Index j=0; j<m_outerSize; ++j)
+        m_innerNonZeros[j] = m_outerIndex[j+1]-m_outerIndex[j];
+    }
+  }
+  
+  // check whether we can do a fast "push back" insertion
+  Index data_end = m_data.allocatedSize();
+  
+  // First case: we are filling a new inner vector which is packed at the end.
+  // We assume that all remaining inner-vectors are also empty and packed to the end.
+  if(m_outerIndex[outer]==data_end)
+  {
+    eigen_internal_assert(m_innerNonZeros[outer]==0);
+    
+    // pack previous empty inner-vectors to end of the used-space
+    // and allocate the entire free-space to the current inner-vector.
+    StorageIndex p = convert_index(m_data.size());
+    Index j = outer;
+    while(j>=0 && m_innerNonZeros[j]==0)
+      m_outerIndex[j--] = p;
+    
+    // push back the new element
+    ++m_innerNonZeros[outer];
+    m_data.append(Scalar(0), inner);
+    
+    // check for reallocation
+    if(data_end != m_data.allocatedSize())
+    {
+      // m_data has been reallocated
+      //  -> move remaining inner-vectors back to the end of the free-space
+      //     so that the entire free-space is allocated to the current inner-vector.
+      eigen_internal_assert(data_end < m_data.allocatedSize());
+      StorageIndex new_end = convert_index(m_data.allocatedSize());
+      for(Index k=outer+1; k<=m_outerSize; ++k)
+        if(m_outerIndex[k]==data_end)
+          m_outerIndex[k] = new_end;
+    }
+    return m_data.value(p);
+  }
+  
+  // Second case: the next inner-vector is packed to the end
+  // and the current inner-vector end match the used-space.
+  if(m_outerIndex[outer+1]==data_end && m_outerIndex[outer]+m_innerNonZeros[outer]==m_data.size())
+  {
+    eigen_internal_assert(outer+1==m_outerSize || m_innerNonZeros[outer+1]==0);
+    
+    // add space for the new element
+    ++m_innerNonZeros[outer];
+    m_data.resize(m_data.size()+1);
+    
+    // check for reallocation
+    if(data_end != m_data.allocatedSize())
+    {
+      // m_data has been reallocated
+      //  -> move remaining inner-vectors back to the end of the free-space
+      //     so that the entire free-space is allocated to the current inner-vector.
+      eigen_internal_assert(data_end < m_data.allocatedSize());
+      StorageIndex new_end = convert_index(m_data.allocatedSize());
+      for(Index k=outer+1; k<=m_outerSize; ++k)
+        if(m_outerIndex[k]==data_end)
+          m_outerIndex[k] = new_end;
+    }
+    
+    // and insert it at the right position (sorted insertion)
+    Index startId = m_outerIndex[outer];
+    Index p = m_outerIndex[outer]+m_innerNonZeros[outer]-1;
+    while ( (p > startId) && (m_data.index(p-1) > inner) )
+    {
+      m_data.index(p) = m_data.index(p-1);
+      m_data.value(p) = m_data.value(p-1);
+      --p;
+    }
+    
+    m_data.index(p) = convert_index(inner);
+    return (m_data.value(p) = Scalar(0));
+  }
+  
+  if(m_data.size() != m_data.allocatedSize())
+  {
+    // make sure the matrix is compatible to random un-compressed insertion:
+    m_data.resize(m_data.allocatedSize());
+    this->reserveInnerVectors(Array<StorageIndex,Dynamic,1>::Constant(m_outerSize, 2));
+  }
+  
+  return insertUncompressed(row,col);
+}
+    
+template<typename _Scalar, int _Options, typename _StorageIndex>
+EIGEN_DONT_INLINE typename SparseMatrix<_Scalar,_Options,_StorageIndex>::Scalar& SparseMatrix<_Scalar,_Options,_StorageIndex>::insertUncompressed(Index row, Index col)
+{
+  eigen_assert(!isCompressed());
+
+  const Index outer = IsRowMajor ? row : col;
+  const StorageIndex inner = convert_index(IsRowMajor ? col : row);
+
+  Index room = m_outerIndex[outer+1] - m_outerIndex[outer];
+  StorageIndex innerNNZ = m_innerNonZeros[outer];
+  if(innerNNZ>=room)
+  {
+    // this inner vector is full, we need to reallocate the whole buffer :(
+    reserve(SingletonVector(outer,std::max<StorageIndex>(2,innerNNZ)));
+  }
+
+  Index startId = m_outerIndex[outer];
+  Index p = startId + m_innerNonZeros[outer];
+  while ( (p > startId) && (m_data.index(p-1) > inner) )
+  {
+    m_data.index(p) = m_data.index(p-1);
+    m_data.value(p) = m_data.value(p-1);
+    --p;
+  }
+  eigen_assert((p<=startId || m_data.index(p-1)!=inner) && "you cannot insert an element that already exists, you must call coeffRef to this end");
+
+  m_innerNonZeros[outer]++;
+
+  m_data.index(p) = inner;
+  return (m_data.value(p) = Scalar(0));
+}
+
+template<typename _Scalar, int _Options, typename _StorageIndex>
+EIGEN_DONT_INLINE typename SparseMatrix<_Scalar,_Options,_StorageIndex>::Scalar& SparseMatrix<_Scalar,_Options,_StorageIndex>::insertCompressed(Index row, Index col)
+{
+  eigen_assert(isCompressed());
+
+  const Index outer = IsRowMajor ? row : col;
+  const Index inner = IsRowMajor ? col : row;
+
+  Index previousOuter = outer;
+  if (m_outerIndex[outer+1]==0)
+  {
+    // we start a new inner vector
+    while (previousOuter>=0 && m_outerIndex[previousOuter]==0)
+    {
+      m_outerIndex[previousOuter] = convert_index(m_data.size());
+      --previousOuter;
+    }
+    m_outerIndex[outer+1] = m_outerIndex[outer];
+  }
+
+  // here we have to handle the tricky case where the outerIndex array
+  // starts with: [ 0 0 0 0 0 1 ...] and we are inserted in, e.g.,
+  // the 2nd inner vector...
+  bool isLastVec = (!(previousOuter==-1 && m_data.size()!=0))
+                && (std::size_t(m_outerIndex[outer+1]) == m_data.size());
+
+  std::size_t startId = m_outerIndex[outer];
+  // FIXME let's make sure sizeof(long int) == sizeof(std::size_t)
+  std::size_t p = m_outerIndex[outer+1];
+  ++m_outerIndex[outer+1];
+
+  double reallocRatio = 1;
+  if (m_data.allocatedSize()<=m_data.size())
+  {
+    // if there is no preallocated memory, let's reserve a minimum of 32 elements
+    if (m_data.size()==0)
+    {
+      m_data.reserve(32);
+    }
+    else
+    {
+      // we need to reallocate the data, to reduce multiple reallocations
+      // we use a smart resize algorithm based on the current filling ratio
+      // in addition, we use double to avoid integers overflows
+      double nnzEstimate = double(m_outerIndex[outer])*double(m_outerSize)/double(outer+1);
+      reallocRatio = (nnzEstimate-double(m_data.size()))/double(m_data.size());
+      // furthermore we bound the realloc ratio to:
+      //   1) reduce multiple minor realloc when the matrix is almost filled
+      //   2) avoid to allocate too much memory when the matrix is almost empty
+      reallocRatio = (std::min)((std::max)(reallocRatio,1.5),8.);
+    }
+  }
+  m_data.resize(m_data.size()+1,reallocRatio);
+
+  if (!isLastVec)
+  {
+    if (previousOuter==-1)
+    {
+      // oops wrong guess.
+      // let's correct the outer offsets
+      for (Index k=0; k<=(outer+1); ++k)
+        m_outerIndex[k] = 0;
+      Index k=outer+1;
+      while(m_outerIndex[k]==0)
+        m_outerIndex[k++] = 1;
+      while (k<=m_outerSize && m_outerIndex[k]!=0)
+        m_outerIndex[k++]++;
+      p = 0;
+      --k;
+      k = m_outerIndex[k]-1;
+      while (k>0)
+      {
+        m_data.index(k) = m_data.index(k-1);
+        m_data.value(k) = m_data.value(k-1);
+        k--;
+      }
+    }
+    else
+    {
+      // we are not inserting into the last inner vec
+      // update outer indices:
+      Index j = outer+2;
+      while (j<=m_outerSize && m_outerIndex[j]!=0)
+        m_outerIndex[j++]++;
+      --j;
+      // shift data of last vecs:
+      Index k = m_outerIndex[j]-1;
+      while (k>=Index(p))
+      {
+        m_data.index(k) = m_data.index(k-1);
+        m_data.value(k) = m_data.value(k-1);
+        k--;
+      }
+    }
+  }
+
+  while ( (p > startId) && (m_data.index(p-1) > inner) )
+  {
+    m_data.index(p) = m_data.index(p-1);
+    m_data.value(p) = m_data.value(p-1);
+    --p;
+  }
+
+  m_data.index(p) = inner;
+  return (m_data.value(p) = Scalar(0));
+}
+
+namespace internal {
+
+template<typename _Scalar, int _Options, typename _StorageIndex>
+struct evaluator<SparseMatrix<_Scalar,_Options,_StorageIndex> >
+  : evaluator<SparseCompressedBase<SparseMatrix<_Scalar,_Options,_StorageIndex> > >
+{
+  typedef evaluator<SparseCompressedBase<SparseMatrix<_Scalar,_Options,_StorageIndex> > > Base;
+  typedef SparseMatrix<_Scalar,_Options,_StorageIndex> SparseMatrixType;
+  evaluator() : Base() {}
+  explicit evaluator(const SparseMatrixType &mat) : Base(mat) {}
+};
+
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSEMATRIX_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h
new file mode 100644
index 0000000..229449f
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h
@@ -0,0 +1,398 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSEMATRIXBASE_H
+#define EIGEN_SPARSEMATRIXBASE_H
+
+namespace Eigen { 
+
+/** \ingroup SparseCore_Module
+  *
+  * \class SparseMatrixBase
+  *
+  * \brief Base class of any sparse matrices or sparse expressions
+  *
+  * \tparam Derived is the derived type, e.g. a sparse matrix type, or an expression, etc.
+  *
+  * This class can be extended with the help of the plugin mechanism described on the page
+  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_SPARSEMATRIXBASE_PLUGIN.
+  */
+template<typename Derived> class SparseMatrixBase
+  : public EigenBase<Derived>
+{
+  public:
+
+    typedef typename internal::traits<Derived>::Scalar Scalar;
+    
+    /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex<float>, etc.
+      *
+      * It is an alias for the Scalar type */
+    typedef Scalar value_type;
+    
+    typedef typename internal::packet_traits<Scalar>::type PacketScalar;
+    typedef typename internal::traits<Derived>::StorageKind StorageKind;
+
+    /** The integer type used to \b store indices within a SparseMatrix.
+      * For a \c SparseMatrix<Scalar,Options,IndexType> it an alias of the third template parameter \c IndexType. */
+    typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
+
+    typedef typename internal::add_const_on_value_type_if_arithmetic<
+                         typename internal::packet_traits<Scalar>::type
+                     >::type PacketReturnType;
+
+    typedef SparseMatrixBase StorageBaseType;
+
+    typedef Matrix<StorageIndex,Dynamic,1> IndexVector;
+    typedef Matrix<Scalar,Dynamic,1> ScalarVector;
+    
+    template<typename OtherDerived>
+    Derived& operator=(const EigenBase<OtherDerived> &other);
+
+    enum {
+
+      RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
+        /**< The number of rows at compile-time. This is just a copy of the value provided
+          * by the \a Derived type. If a value is not known at compile-time,
+          * it is set to the \a Dynamic constant.
+          * \sa MatrixBase::rows(), MatrixBase::cols(), ColsAtCompileTime, SizeAtCompileTime */
+
+      ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
+        /**< The number of columns at compile-time. This is just a copy of the value provided
+          * by the \a Derived type. If a value is not known at compile-time,
+          * it is set to the \a Dynamic constant.
+          * \sa MatrixBase::rows(), MatrixBase::cols(), RowsAtCompileTime, SizeAtCompileTime */
+
+
+      SizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::RowsAtCompileTime,
+                                                   internal::traits<Derived>::ColsAtCompileTime>::ret),
+        /**< This is equal to the number of coefficients, i.e. the number of
+          * rows times the number of columns, or to \a Dynamic if this is not
+          * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */
+
+      MaxRowsAtCompileTime = RowsAtCompileTime,
+      MaxColsAtCompileTime = ColsAtCompileTime,
+
+      MaxSizeAtCompileTime = (internal::size_at_compile_time<MaxRowsAtCompileTime,
+                                                      MaxColsAtCompileTime>::ret),
+
+      IsVectorAtCompileTime = RowsAtCompileTime == 1 || ColsAtCompileTime == 1,
+        /**< This is set to true if either the number of rows or the number of
+          * columns is known at compile-time to be equal to 1. Indeed, in that case,
+          * we are dealing with a column-vector (if there is only one column) or with
+          * a row-vector (if there is only one row). */
+
+      NumDimensions = int(MaxSizeAtCompileTime) == 1 ? 0 : bool(IsVectorAtCompileTime) ? 1 : 2,
+        /**< This value is equal to Tensor::NumDimensions, i.e. 0 for scalars, 1 for vectors,
+         * and 2 for matrices.
+         */
+
+      Flags = internal::traits<Derived>::Flags,
+        /**< This stores expression \ref flags flags which may or may not be inherited by new expressions
+          * constructed from this one. See the \ref flags "list of flags".
+          */
+
+      IsRowMajor = Flags&RowMajorBit ? 1 : 0,
+      
+      InnerSizeAtCompileTime = int(IsVectorAtCompileTime) ? int(SizeAtCompileTime)
+                             : int(IsRowMajor) ? int(ColsAtCompileTime) : int(RowsAtCompileTime),
+
+      #ifndef EIGEN_PARSED_BY_DOXYGEN
+      _HasDirectAccess = (int(Flags)&DirectAccessBit) ? 1 : 0 // workaround sunCC
+      #endif
+    };
+
+    /** \internal the return type of MatrixBase::adjoint() */
+    typedef typename internal::conditional<NumTraits<Scalar>::IsComplex,
+                        CwiseUnaryOp<internal::scalar_conjugate_op<Scalar>, Eigen::Transpose<const Derived> >,
+                        Transpose<const Derived>
+                     >::type AdjointReturnType;
+    typedef Transpose<Derived> TransposeReturnType;
+    typedef typename internal::add_const<Transpose<const Derived> >::type ConstTransposeReturnType;
+
+    // FIXME storage order do not match evaluator storage order
+    typedef SparseMatrix<Scalar, Flags&RowMajorBit ? RowMajor : ColMajor, StorageIndex> PlainObject;
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** This is the "real scalar" type; if the \a Scalar type is already real numbers
+      * (e.g. int, float or double) then \a RealScalar is just the same as \a Scalar. If
+      * \a Scalar is \a std::complex<T> then RealScalar is \a T.
+      *
+      * \sa class NumTraits
+      */
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+
+    /** \internal the return type of coeff()
+      */
+    typedef typename internal::conditional<_HasDirectAccess, const Scalar&, Scalar>::type CoeffReturnType;
+
+    /** \internal Represents a matrix with all coefficients equal to one another*/
+    typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>,Matrix<Scalar,Dynamic,Dynamic> > ConstantReturnType;
+
+    /** type of the equivalent dense matrix */
+    typedef Matrix<Scalar,RowsAtCompileTime,ColsAtCompileTime> DenseMatrixType;
+    /** type of the equivalent square matrix */
+    typedef Matrix<Scalar,EIGEN_SIZE_MAX(RowsAtCompileTime,ColsAtCompileTime),
+                          EIGEN_SIZE_MAX(RowsAtCompileTime,ColsAtCompileTime)> SquareMatrixType;
+
+    inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
+    inline Derived& derived() { return *static_cast<Derived*>(this); }
+    inline Derived& const_cast_derived() const
+    { return *static_cast<Derived*>(const_cast<SparseMatrixBase*>(this)); }
+
+    typedef EigenBase<Derived> Base;
+
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+#define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::SparseMatrixBase
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+#define EIGEN_DOC_UNARY_ADDONS(METHOD,OP)           /** <p>This method does not change the sparsity of \c *this: the OP is applied to explicitly stored coefficients only. \sa SparseCompressedBase::coeffs() </p> */
+#define EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL      /** <p> \warning This method returns a read-only expression for any sparse matrices. \sa \ref TutorialSparse_SubMatrices "Sparse block operations" </p> */
+#define EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(COND) /** <p> \warning This method returns a read-write expression for COND sparse matrices only. Otherwise, the returned expression is read-only. \sa \ref TutorialSparse_SubMatrices "Sparse block operations" </p> */
+#else
+#define EIGEN_DOC_UNARY_ADDONS(X,Y)
+#define EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+#define EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(COND)
+#endif
+#   include "../plugins/CommonCwiseUnaryOps.h"
+#   include "../plugins/CommonCwiseBinaryOps.h"
+#   include "../plugins/MatrixCwiseUnaryOps.h"
+#   include "../plugins/MatrixCwiseBinaryOps.h"
+#   include "../plugins/BlockMethods.h"
+#   ifdef EIGEN_SPARSEMATRIXBASE_PLUGIN
+#     include EIGEN_SPARSEMATRIXBASE_PLUGIN
+#   endif
+#undef EIGEN_CURRENT_STORAGE_BASE_CLASS
+#undef EIGEN_DOC_UNARY_ADDONS
+#undef EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+#undef EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF
+
+    /** \returns the number of rows. \sa cols() */
+    inline Index rows() const { return derived().rows(); }
+    /** \returns the number of columns. \sa rows() */
+    inline Index cols() const { return derived().cols(); }
+    /** \returns the number of coefficients, which is \a rows()*cols().
+      * \sa rows(), cols(). */
+    inline Index size() const { return rows() * cols(); }
+    /** \returns true if either the number of rows or the number of columns is equal to 1.
+      * In other words, this function returns
+      * \code rows()==1 || cols()==1 \endcode
+      * \sa rows(), cols(), IsVectorAtCompileTime. */
+    inline bool isVector() const { return rows()==1 || cols()==1; }
+    /** \returns the size of the storage major dimension,
+      * i.e., the number of columns for a columns major matrix, and the number of rows otherwise */
+    Index outerSize() const { return (int(Flags)&RowMajorBit) ? this->rows() : this->cols(); }
+    /** \returns the size of the inner dimension according to the storage order,
+      * i.e., the number of rows for a columns major matrix, and the number of cols otherwise */
+    Index innerSize() const { return (int(Flags)&RowMajorBit) ? this->cols() : this->rows(); }
+
+    bool isRValue() const { return m_isRValue; }
+    Derived& markAsRValue() { m_isRValue = true; return derived(); }
+
+    SparseMatrixBase() : m_isRValue(false) { /* TODO check flags */ }
+
+    
+    template<typename OtherDerived>
+    Derived& operator=(const ReturnByValue<OtherDerived>& other);
+
+    template<typename OtherDerived>
+    inline Derived& operator=(const SparseMatrixBase<OtherDerived>& other);
+
+    inline Derived& operator=(const Derived& other);
+
+  protected:
+
+    template<typename OtherDerived>
+    inline Derived& assign(const OtherDerived& other);
+
+    template<typename OtherDerived>
+    inline void assignGeneric(const OtherDerived& other);
+
+  public:
+
+    friend std::ostream & operator << (std::ostream & s, const SparseMatrixBase& m)
+    {
+      typedef typename Derived::Nested Nested;
+      typedef typename internal::remove_all<Nested>::type NestedCleaned;
+
+      if (Flags&RowMajorBit)
+      {
+        Nested nm(m.derived());
+        internal::evaluator<NestedCleaned> thisEval(nm);
+        for (Index row=0; row<nm.outerSize(); ++row)
+        {
+          Index col = 0;
+          for (typename internal::evaluator<NestedCleaned>::InnerIterator it(thisEval, row); it; ++it)
+          {
+            for ( ; col<it.index(); ++col)
+              s << "0 ";
+            s << it.value() << " ";
+            ++col;
+          }
+          for ( ; col<m.cols(); ++col)
+            s << "0 ";
+          s << std::endl;
+        }
+      }
+      else
+      {
+        Nested nm(m.derived());
+        internal::evaluator<NestedCleaned> thisEval(nm);
+        if (m.cols() == 1) {
+          Index row = 0;
+          for (typename internal::evaluator<NestedCleaned>::InnerIterator it(thisEval, 0); it; ++it)
+          {
+            for ( ; row<it.index(); ++row)
+              s << "0" << std::endl;
+            s << it.value() << std::endl;
+            ++row;
+          }
+          for ( ; row<m.rows(); ++row)
+            s << "0" << std::endl;
+        }
+        else
+        {
+          SparseMatrix<Scalar, RowMajorBit, StorageIndex> trans = m;
+          s << static_cast<const SparseMatrixBase<SparseMatrix<Scalar, RowMajorBit, StorageIndex> >&>(trans);
+        }
+      }
+      return s;
+    }
+
+    template<typename OtherDerived>
+    Derived& operator+=(const SparseMatrixBase<OtherDerived>& other);
+    template<typename OtherDerived>
+    Derived& operator-=(const SparseMatrixBase<OtherDerived>& other);
+    
+    template<typename OtherDerived>
+    Derived& operator+=(const DiagonalBase<OtherDerived>& other);
+    template<typename OtherDerived>
+    Derived& operator-=(const DiagonalBase<OtherDerived>& other);
+
+    template<typename OtherDerived>
+    Derived& operator+=(const EigenBase<OtherDerived> &other);
+    template<typename OtherDerived>
+    Derived& operator-=(const EigenBase<OtherDerived> &other);
+
+    Derived& operator*=(const Scalar& other);
+    Derived& operator/=(const Scalar& other);
+
+    template<typename OtherDerived> struct CwiseProductDenseReturnType {
+      typedef CwiseBinaryOp<internal::scalar_product_op<typename ScalarBinaryOpTraits<
+                                                          typename internal::traits<Derived>::Scalar,
+                                                          typename internal::traits<OtherDerived>::Scalar
+                                                        >::ReturnType>,
+                            const Derived,
+                            const OtherDerived
+                          > Type;
+    };
+
+    template<typename OtherDerived>
+    EIGEN_STRONG_INLINE const typename CwiseProductDenseReturnType<OtherDerived>::Type
+    cwiseProduct(const MatrixBase<OtherDerived> &other) const;
+
+    // sparse * diagonal
+    template<typename OtherDerived>
+    const Product<Derived,OtherDerived>
+    operator*(const DiagonalBase<OtherDerived> &other) const
+    { return Product<Derived,OtherDerived>(derived(), other.derived()); }
+
+    // diagonal * sparse
+    template<typename OtherDerived> friend
+    const Product<OtherDerived,Derived>
+    operator*(const DiagonalBase<OtherDerived> &lhs, const SparseMatrixBase& rhs)
+    { return Product<OtherDerived,Derived>(lhs.derived(), rhs.derived()); }
+    
+    // sparse * sparse
+    template<typename OtherDerived>
+    const Product<Derived,OtherDerived,AliasFreeProduct>
+    operator*(const SparseMatrixBase<OtherDerived> &other) const;
+    
+    // sparse * dense
+    template<typename OtherDerived>
+    const Product<Derived,OtherDerived>
+    operator*(const MatrixBase<OtherDerived> &other) const
+    { return Product<Derived,OtherDerived>(derived(), other.derived()); }
+    
+    // dense * sparse
+    template<typename OtherDerived> friend
+    const Product<OtherDerived,Derived>
+    operator*(const MatrixBase<OtherDerived> &lhs, const SparseMatrixBase& rhs)
+    { return Product<OtherDerived,Derived>(lhs.derived(), rhs.derived()); }
+    
+     /** \returns an expression of P H P^-1 where H is the matrix represented by \c *this */
+    SparseSymmetricPermutationProduct<Derived,Upper|Lower> twistedBy(const PermutationMatrix<Dynamic,Dynamic,StorageIndex>& perm) const
+    {
+      return SparseSymmetricPermutationProduct<Derived,Upper|Lower>(derived(), perm);
+    }
+
+    template<typename OtherDerived>
+    Derived& operator*=(const SparseMatrixBase<OtherDerived>& other);
+
+    template<int Mode>
+    inline const TriangularView<const Derived, Mode> triangularView() const;
+    
+    template<unsigned int UpLo> struct SelfAdjointViewReturnType { typedef SparseSelfAdjointView<Derived, UpLo> Type; };
+    template<unsigned int UpLo> struct ConstSelfAdjointViewReturnType { typedef const SparseSelfAdjointView<const Derived, UpLo> Type; };
+
+    template<unsigned int UpLo> inline 
+    typename ConstSelfAdjointViewReturnType<UpLo>::Type selfadjointView() const;
+    template<unsigned int UpLo> inline
+    typename SelfAdjointViewReturnType<UpLo>::Type selfadjointView();
+
+    template<typename OtherDerived> Scalar dot(const MatrixBase<OtherDerived>& other) const;
+    template<typename OtherDerived> Scalar dot(const SparseMatrixBase<OtherDerived>& other) const;
+    RealScalar squaredNorm() const;
+    RealScalar norm()  const;
+    RealScalar blueNorm() const;
+
+    TransposeReturnType transpose() { return TransposeReturnType(derived()); }
+    const ConstTransposeReturnType transpose() const { return ConstTransposeReturnType(derived()); }
+    const AdjointReturnType adjoint() const { return AdjointReturnType(transpose()); }
+
+    DenseMatrixType toDense() const
+    {
+      return DenseMatrixType(derived());
+    }
+
+    template<typename OtherDerived>
+    bool isApprox(const SparseMatrixBase<OtherDerived>& other,
+                  const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+
+    template<typename OtherDerived>
+    bool isApprox(const MatrixBase<OtherDerived>& other,
+                  const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
+    { return toDense().isApprox(other,prec); }
+
+    /** \returns the matrix or vector obtained by evaluating this expression.
+      *
+      * Notice that in the case of a plain matrix or vector (not an expression) this function just returns
+      * a const reference, in order to avoid a useless copy.
+      */
+    inline const typename internal::eval<Derived>::type eval() const
+    { return typename internal::eval<Derived>::type(derived()); }
+
+    Scalar sum() const;
+    
+    inline const SparseView<Derived>
+    pruned(const Scalar& reference = Scalar(0), const RealScalar& epsilon = NumTraits<Scalar>::dummy_precision()) const;
+
+  protected:
+
+    bool m_isRValue;
+
+    static inline StorageIndex convert_index(const Index idx) {
+      return internal::convert_index<StorageIndex>(idx);
+    }
+  private:
+    template<typename Dest> void evalTo(Dest &) const;
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSEMATRIXBASE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparsePermutation.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparsePermutation.h
new file mode 100644
index 0000000..ef38357
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparsePermutation.h
@@ -0,0 +1,178 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_PERMUTATION_H
+#define EIGEN_SPARSE_PERMUTATION_H
+
+// This file implements sparse * permutation products
+
+namespace Eigen { 
+
+namespace internal {
+
+template<typename ExpressionType, int Side, bool Transposed>
+struct permutation_matrix_product<ExpressionType, Side, Transposed, SparseShape>
+{
+    typedef typename nested_eval<ExpressionType, 1>::type MatrixType;
+    typedef typename remove_all<MatrixType>::type MatrixTypeCleaned;
+
+    typedef typename MatrixTypeCleaned::Scalar Scalar;
+    typedef typename MatrixTypeCleaned::StorageIndex StorageIndex;
+
+    enum {
+      SrcStorageOrder = MatrixTypeCleaned::Flags&RowMajorBit ? RowMajor : ColMajor,
+      MoveOuter = SrcStorageOrder==RowMajor ? Side==OnTheLeft : Side==OnTheRight
+    };
+    
+    typedef typename internal::conditional<MoveOuter,
+        SparseMatrix<Scalar,SrcStorageOrder,StorageIndex>,
+        SparseMatrix<Scalar,int(SrcStorageOrder)==RowMajor?ColMajor:RowMajor,StorageIndex> >::type ReturnType;
+
+    template<typename Dest,typename PermutationType>
+    static inline void run(Dest& dst, const PermutationType& perm, const ExpressionType& xpr)
+    {
+      MatrixType mat(xpr);
+      if(MoveOuter)
+      {
+        SparseMatrix<Scalar,SrcStorageOrder,StorageIndex> tmp(mat.rows(), mat.cols());
+        Matrix<StorageIndex,Dynamic,1> sizes(mat.outerSize());
+        for(Index j=0; j<mat.outerSize(); ++j)
+        {
+          Index jp = perm.indices().coeff(j);
+          sizes[((Side==OnTheLeft) ^ Transposed) ? jp : j] = StorageIndex(mat.innerVector(((Side==OnTheRight) ^ Transposed) ? jp : j).nonZeros());
+        }
+        tmp.reserve(sizes);
+        for(Index j=0; j<mat.outerSize(); ++j)
+        {
+          Index jp = perm.indices().coeff(j);
+          Index jsrc = ((Side==OnTheRight) ^ Transposed) ? jp : j;
+          Index jdst = ((Side==OnTheLeft) ^ Transposed) ? jp : j;
+          for(typename MatrixTypeCleaned::InnerIterator it(mat,jsrc); it; ++it)
+            tmp.insertByOuterInner(jdst,it.index()) = it.value();
+        }
+        dst = tmp;
+      }
+      else
+      {
+        SparseMatrix<Scalar,int(SrcStorageOrder)==RowMajor?ColMajor:RowMajor,StorageIndex> tmp(mat.rows(), mat.cols());
+        Matrix<StorageIndex,Dynamic,1> sizes(tmp.outerSize());
+        sizes.setZero();
+        PermutationMatrix<Dynamic,Dynamic,StorageIndex> perm_cpy;
+        if((Side==OnTheLeft) ^ Transposed)
+          perm_cpy = perm;
+        else
+          perm_cpy = perm.transpose();
+
+        for(Index j=0; j<mat.outerSize(); ++j)
+          for(typename MatrixTypeCleaned::InnerIterator it(mat,j); it; ++it)
+            sizes[perm_cpy.indices().coeff(it.index())]++;
+        tmp.reserve(sizes);
+        for(Index j=0; j<mat.outerSize(); ++j)
+          for(typename MatrixTypeCleaned::InnerIterator it(mat,j); it; ++it)
+            tmp.insertByOuterInner(perm_cpy.indices().coeff(it.index()),j) = it.value();
+        dst = tmp;
+      }
+    }
+};
+
+}
+
+namespace internal {
+
+template <int ProductTag> struct product_promote_storage_type<Sparse,             PermutationStorage, ProductTag> { typedef Sparse ret; };
+template <int ProductTag> struct product_promote_storage_type<PermutationStorage, Sparse,             ProductTag> { typedef Sparse ret; };
+
+// TODO, the following two overloads are only needed to define the right temporary type through 
+// typename traits<permutation_sparse_matrix_product<Rhs,Lhs,OnTheRight,false> >::ReturnType
+// whereas it should be correctly handled by traits<Product<> >::PlainObject
+
+template<typename Lhs, typename Rhs, int ProductTag>
+struct product_evaluator<Product<Lhs, Rhs, AliasFreeProduct>, ProductTag, PermutationShape, SparseShape>
+  : public evaluator<typename permutation_matrix_product<Rhs,OnTheLeft,false,SparseShape>::ReturnType>
+{
+  typedef Product<Lhs, Rhs, AliasFreeProduct> XprType;
+  typedef typename permutation_matrix_product<Rhs,OnTheLeft,false,SparseShape>::ReturnType PlainObject;
+  typedef evaluator<PlainObject> Base;
+
+  enum {
+    Flags = Base::Flags | EvalBeforeNestingBit
+  };
+
+  explicit product_evaluator(const XprType& xpr)
+    : m_result(xpr.rows(), xpr.cols())
+  {
+    ::new (static_cast<Base*>(this)) Base(m_result);
+    generic_product_impl<Lhs, Rhs, PermutationShape, SparseShape, ProductTag>::evalTo(m_result, xpr.lhs(), xpr.rhs());
+  }
+
+protected:
+  PlainObject m_result;
+};
+
+template<typename Lhs, typename Rhs, int ProductTag>
+struct product_evaluator<Product<Lhs, Rhs, AliasFreeProduct>, ProductTag, SparseShape, PermutationShape >
+  : public evaluator<typename permutation_matrix_product<Lhs,OnTheRight,false,SparseShape>::ReturnType>
+{
+  typedef Product<Lhs, Rhs, AliasFreeProduct> XprType;
+  typedef typename permutation_matrix_product<Lhs,OnTheRight,false,SparseShape>::ReturnType PlainObject;
+  typedef evaluator<PlainObject> Base;
+
+  enum {
+    Flags = Base::Flags | EvalBeforeNestingBit
+  };
+
+  explicit product_evaluator(const XprType& xpr)
+    : m_result(xpr.rows(), xpr.cols())
+  {
+    ::new (static_cast<Base*>(this)) Base(m_result);
+    generic_product_impl<Lhs, Rhs, SparseShape, PermutationShape, ProductTag>::evalTo(m_result, xpr.lhs(), xpr.rhs());
+  }
+
+protected:
+  PlainObject m_result;
+};
+
+} // end namespace internal
+
+/** \returns the matrix with the permutation applied to the columns
+  */
+template<typename SparseDerived, typename PermDerived>
+inline const Product<SparseDerived, PermDerived, AliasFreeProduct>
+operator*(const SparseMatrixBase<SparseDerived>& matrix, const PermutationBase<PermDerived>& perm)
+{ return Product<SparseDerived, PermDerived, AliasFreeProduct>(matrix.derived(), perm.derived()); }
+
+/** \returns the matrix with the permutation applied to the rows
+  */
+template<typename SparseDerived, typename PermDerived>
+inline const Product<PermDerived, SparseDerived, AliasFreeProduct>
+operator*( const PermutationBase<PermDerived>& perm, const SparseMatrixBase<SparseDerived>& matrix)
+{ return  Product<PermDerived, SparseDerived, AliasFreeProduct>(perm.derived(), matrix.derived()); }
+
+
+/** \returns the matrix with the inverse permutation applied to the columns.
+  */
+template<typename SparseDerived, typename PermutationType>
+inline const Product<SparseDerived, Inverse<PermutationType>, AliasFreeProduct>
+operator*(const SparseMatrixBase<SparseDerived>& matrix, const InverseImpl<PermutationType, PermutationStorage>& tperm)
+{
+  return Product<SparseDerived, Inverse<PermutationType>, AliasFreeProduct>(matrix.derived(), tperm.derived());
+}
+
+/** \returns the matrix with the inverse permutation applied to the rows.
+  */
+template<typename SparseDerived, typename PermutationType>
+inline const Product<Inverse<PermutationType>, SparseDerived, AliasFreeProduct>
+operator*(const InverseImpl<PermutationType,PermutationStorage>& tperm, const SparseMatrixBase<SparseDerived>& matrix)
+{
+  return Product<Inverse<PermutationType>, SparseDerived, AliasFreeProduct>(tperm.derived(), matrix.derived());
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_SELFADJOINTVIEW_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseProduct.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseProduct.h
new file mode 100644
index 0000000..af8a774
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseProduct.h
@@ -0,0 +1,181 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSEPRODUCT_H
+#define EIGEN_SPARSEPRODUCT_H
+
+namespace Eigen { 
+
+/** \returns an expression of the product of two sparse matrices.
+  * By default a conservative product preserving the symbolic non zeros is performed.
+  * The automatic pruning of the small values can be achieved by calling the pruned() function
+  * in which case a totally different product algorithm is employed:
+  * \code
+  * C = (A*B).pruned();             // suppress numerical zeros (exact)
+  * C = (A*B).pruned(ref);
+  * C = (A*B).pruned(ref,epsilon);
+  * \endcode
+  * where \c ref is a meaningful non zero reference value.
+  * */
+template<typename Derived>
+template<typename OtherDerived>
+inline const Product<Derived,OtherDerived,AliasFreeProduct>
+SparseMatrixBase<Derived>::operator*(const SparseMatrixBase<OtherDerived> &other) const
+{
+  return Product<Derived,OtherDerived,AliasFreeProduct>(derived(), other.derived());
+}
+
+namespace internal {
+
+// sparse * sparse
+template<typename Lhs, typename Rhs, int ProductType>
+struct generic_product_impl<Lhs, Rhs, SparseShape, SparseShape, ProductType>
+{
+  template<typename Dest>
+  static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
+  {
+    evalTo(dst, lhs, rhs, typename evaluator_traits<Dest>::Shape());
+  }
+
+  // dense += sparse * sparse
+  template<typename Dest,typename ActualLhs>
+  static void addTo(Dest& dst, const ActualLhs& lhs, const Rhs& rhs, typename enable_if<is_same<typename evaluator_traits<Dest>::Shape,DenseShape>::value,int*>::type* = 0)
+  {
+    typedef typename nested_eval<ActualLhs,Dynamic>::type LhsNested;
+    typedef typename nested_eval<Rhs,Dynamic>::type RhsNested;
+    LhsNested lhsNested(lhs);
+    RhsNested rhsNested(rhs);
+    internal::sparse_sparse_to_dense_product_selector<typename remove_all<LhsNested>::type,
+                                                      typename remove_all<RhsNested>::type, Dest>::run(lhsNested,rhsNested,dst);
+  }
+
+  // dense -= sparse * sparse
+  template<typename Dest>
+  static void subTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, typename enable_if<is_same<typename evaluator_traits<Dest>::Shape,DenseShape>::value,int*>::type* = 0)
+  {
+    addTo(dst, -lhs, rhs);
+  }
+
+protected:
+
+  // sparse = sparse * sparse
+  template<typename Dest>
+  static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, SparseShape)
+  {
+    typedef typename nested_eval<Lhs,Dynamic>::type LhsNested;
+    typedef typename nested_eval<Rhs,Dynamic>::type RhsNested;
+    LhsNested lhsNested(lhs);
+    RhsNested rhsNested(rhs);
+    internal::conservative_sparse_sparse_product_selector<typename remove_all<LhsNested>::type,
+                                                          typename remove_all<RhsNested>::type, Dest>::run(lhsNested,rhsNested,dst);
+  }
+
+  // dense = sparse * sparse
+  template<typename Dest>
+  static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, DenseShape)
+  {
+    dst.setZero();
+    addTo(dst, lhs, rhs);
+  }
+};
+
+// sparse * sparse-triangular
+template<typename Lhs, typename Rhs, int ProductType>
+struct generic_product_impl<Lhs, Rhs, SparseShape, SparseTriangularShape, ProductType>
+ : public generic_product_impl<Lhs, Rhs, SparseShape, SparseShape, ProductType>
+{};
+
+// sparse-triangular * sparse
+template<typename Lhs, typename Rhs, int ProductType>
+struct generic_product_impl<Lhs, Rhs, SparseTriangularShape, SparseShape, ProductType>
+ : public generic_product_impl<Lhs, Rhs, SparseShape, SparseShape, ProductType>
+{};
+
+// dense = sparse-product (can be sparse*sparse, sparse*perm, etc.)
+template< typename DstXprType, typename Lhs, typename Rhs>
+struct Assignment<DstXprType, Product<Lhs,Rhs,AliasFreeProduct>, internal::assign_op<typename DstXprType::Scalar,typename Product<Lhs,Rhs,AliasFreeProduct>::Scalar>, Sparse2Dense>
+{
+  typedef Product<Lhs,Rhs,AliasFreeProduct> SrcXprType;
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &)
+  {
+    Index dstRows = src.rows();
+    Index dstCols = src.cols();
+    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
+      dst.resize(dstRows, dstCols);
+    
+    generic_product_impl<Lhs, Rhs>::evalTo(dst,src.lhs(),src.rhs());
+  }
+};
+
+// dense += sparse-product (can be sparse*sparse, sparse*perm, etc.)
+template< typename DstXprType, typename Lhs, typename Rhs>
+struct Assignment<DstXprType, Product<Lhs,Rhs,AliasFreeProduct>, internal::add_assign_op<typename DstXprType::Scalar,typename Product<Lhs,Rhs,AliasFreeProduct>::Scalar>, Sparse2Dense>
+{
+  typedef Product<Lhs,Rhs,AliasFreeProduct> SrcXprType;
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &)
+  {
+    generic_product_impl<Lhs, Rhs>::addTo(dst,src.lhs(),src.rhs());
+  }
+};
+
+// dense -= sparse-product (can be sparse*sparse, sparse*perm, etc.)
+template< typename DstXprType, typename Lhs, typename Rhs>
+struct Assignment<DstXprType, Product<Lhs,Rhs,AliasFreeProduct>, internal::sub_assign_op<typename DstXprType::Scalar,typename Product<Lhs,Rhs,AliasFreeProduct>::Scalar>, Sparse2Dense>
+{
+  typedef Product<Lhs,Rhs,AliasFreeProduct> SrcXprType;
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &)
+  {
+    generic_product_impl<Lhs, Rhs>::subTo(dst,src.lhs(),src.rhs());
+  }
+};
+
+template<typename Lhs, typename Rhs, int Options>
+struct unary_evaluator<SparseView<Product<Lhs, Rhs, Options> >, IteratorBased>
+ : public evaluator<typename Product<Lhs, Rhs, DefaultProduct>::PlainObject>
+{
+  typedef SparseView<Product<Lhs, Rhs, Options> > XprType;
+  typedef typename XprType::PlainObject PlainObject;
+  typedef evaluator<PlainObject> Base;
+
+  explicit unary_evaluator(const XprType& xpr)
+    : m_result(xpr.rows(), xpr.cols())
+  {
+    using std::abs;
+    ::new (static_cast<Base*>(this)) Base(m_result);
+    typedef typename nested_eval<Lhs,Dynamic>::type LhsNested;
+    typedef typename nested_eval<Rhs,Dynamic>::type RhsNested;
+    LhsNested lhsNested(xpr.nestedExpression().lhs());
+    RhsNested rhsNested(xpr.nestedExpression().rhs());
+
+    internal::sparse_sparse_product_with_pruning_selector<typename remove_all<LhsNested>::type,
+                                                          typename remove_all<RhsNested>::type, PlainObject>::run(lhsNested,rhsNested,m_result,
+                                                                                                                  abs(xpr.reference())*xpr.epsilon());
+  }
+
+protected:
+  PlainObject m_result;
+};
+
+} // end namespace internal
+
+// sparse matrix = sparse-product (can be sparse*sparse, sparse*perm, etc.)
+template<typename Scalar, int _Options, typename _StorageIndex>
+template<typename Lhs, typename Rhs>
+SparseMatrix<Scalar,_Options,_StorageIndex>& SparseMatrix<Scalar,_Options,_StorageIndex>::operator=(const Product<Lhs,Rhs,AliasFreeProduct>& src)
+{
+  // std::cout << "in Assignment : " << DstOptions << "\n";
+  SparseMatrix dst(src.rows(),src.cols());
+  internal::generic_product_impl<Lhs, Rhs>::evalTo(dst,src.lhs(),src.rhs());
+  this->swap(dst);
+  return *this;
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSEPRODUCT_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRedux.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRedux.h
new file mode 100644
index 0000000..4587749
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRedux.h
@@ -0,0 +1,49 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSEREDUX_H
+#define EIGEN_SPARSEREDUX_H
+
+namespace Eigen { 
+
+template<typename Derived>
+typename internal::traits<Derived>::Scalar
+SparseMatrixBase<Derived>::sum() const
+{
+  eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix");
+  Scalar res(0);
+  internal::evaluator<Derived> thisEval(derived());
+  for (Index j=0; j<outerSize(); ++j)
+    for (typename internal::evaluator<Derived>::InnerIterator iter(thisEval,j); iter; ++iter)
+      res += iter.value();
+  return res;
+}
+
+template<typename _Scalar, int _Options, typename _Index>
+typename internal::traits<SparseMatrix<_Scalar,_Options,_Index> >::Scalar
+SparseMatrix<_Scalar,_Options,_Index>::sum() const
+{
+  eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix");
+  if(this->isCompressed())
+    return Matrix<Scalar,1,Dynamic>::Map(m_data.valuePtr(), m_data.size()).sum();
+  else
+    return Base::sum();
+}
+
+template<typename _Scalar, int _Options, typename _Index>
+typename internal::traits<SparseVector<_Scalar,_Options, _Index> >::Scalar
+SparseVector<_Scalar,_Options,_Index>::sum() const
+{
+  eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix");
+  return Matrix<Scalar,1,Dynamic>::Map(m_data.valuePtr(), m_data.size()).sum();
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSEREDUX_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRef.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRef.h
new file mode 100644
index 0000000..748f87d
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseRef.h
@@ -0,0 +1,397 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_REF_H
+#define EIGEN_SPARSE_REF_H
+
+namespace Eigen {
+
+enum {
+  StandardCompressedFormat = 2 /**< used by Ref<SparseMatrix> to specify whether the input storage must be in standard compressed form */
+};
+  
+namespace internal {
+
+template<typename Derived> class SparseRefBase;
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int _Options, typename _StrideType>
+struct traits<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
+  : public traits<SparseMatrix<MatScalar,MatOptions,MatIndex> >
+{
+  typedef SparseMatrix<MatScalar,MatOptions,MatIndex> PlainObjectType;
+  enum {
+    Options = _Options,
+    Flags = traits<PlainObjectType>::Flags | CompressedAccessBit | NestByRefBit
+  };
+
+  template<typename Derived> struct match {
+    enum {
+      StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)),
+      MatchAtCompileTime = (Derived::Flags&CompressedAccessBit) && StorageOrderMatch
+    };
+    typedef typename internal::conditional<MatchAtCompileTime,internal::true_type,internal::false_type>::type type;
+  };
+  
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int _Options, typename _StrideType>
+struct traits<Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
+  : public traits<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
+{
+  enum {
+    Flags = (traits<SparseMatrix<MatScalar,MatOptions,MatIndex> >::Flags | CompressedAccessBit | NestByRefBit) & ~LvalueBit
+  };
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int _Options, typename _StrideType>
+struct traits<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
+  : public traits<SparseVector<MatScalar,MatOptions,MatIndex> >
+{
+  typedef SparseVector<MatScalar,MatOptions,MatIndex> PlainObjectType;
+  enum {
+    Options = _Options,
+    Flags = traits<PlainObjectType>::Flags | CompressedAccessBit | NestByRefBit
+  };
+
+  template<typename Derived> struct match {
+    enum {
+      MatchAtCompileTime = (Derived::Flags&CompressedAccessBit) && Derived::IsVectorAtCompileTime
+    };
+    typedef typename internal::conditional<MatchAtCompileTime,internal::true_type,internal::false_type>::type type;
+  };
+
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int _Options, typename _StrideType>
+struct traits<Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
+  : public traits<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, _Options, _StrideType> >
+{
+  enum {
+    Flags = (traits<SparseVector<MatScalar,MatOptions,MatIndex> >::Flags | CompressedAccessBit | NestByRefBit) & ~LvalueBit
+  };
+};
+
+template<typename Derived>
+struct traits<SparseRefBase<Derived> > : public traits<Derived> {};
+
+template<typename Derived> class SparseRefBase
+  : public SparseMapBase<Derived>
+{
+public:
+
+  typedef SparseMapBase<Derived> Base;
+  EIGEN_SPARSE_PUBLIC_INTERFACE(SparseRefBase)
+
+  SparseRefBase()
+    : Base(RowsAtCompileTime==Dynamic?0:RowsAtCompileTime,ColsAtCompileTime==Dynamic?0:ColsAtCompileTime, 0, 0, 0, 0, 0)
+  {}
+  
+protected:
+
+  template<typename Expression>
+  void construct(Expression& expr)
+  {
+    if(expr.outerIndexPtr()==0)
+      ::new (static_cast<Base*>(this)) Base(expr.size(), expr.nonZeros(), expr.innerIndexPtr(), expr.valuePtr());
+    else
+      ::new (static_cast<Base*>(this)) Base(expr.rows(), expr.cols(), expr.nonZeros(), expr.outerIndexPtr(), expr.innerIndexPtr(), expr.valuePtr(), expr.innerNonZeroPtr());
+  }
+};
+
+} // namespace internal
+
+
+/** 
+  * \ingroup SparseCore_Module
+  *
+  * \brief A sparse matrix expression referencing an existing sparse expression
+  *
+  * \tparam SparseMatrixType the equivalent sparse matrix type of the referenced data, it must be a template instance of class SparseMatrix.
+  * \tparam Options specifies whether the a standard compressed format is required \c Options is  \c #StandardCompressedFormat, or \c 0.
+  *                The default is \c 0.
+  *
+  * \sa class Ref
+  */
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType >
+  : public internal::SparseRefBase<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType > >
+#else
+template<typename SparseMatrixType, int Options>
+class Ref<SparseMatrixType, Options>
+  : public SparseMapBase<Derived,WriteAccessors> // yes, that's weird to use Derived here, but that works!
+#endif
+{
+    typedef SparseMatrix<MatScalar,MatOptions,MatIndex> PlainObjectType;
+    typedef internal::traits<Ref> Traits;
+    template<int OtherOptions>
+    inline Ref(const SparseMatrix<MatScalar,OtherOptions,MatIndex>& expr);
+    template<int OtherOptions>
+    inline Ref(const MappedSparseMatrix<MatScalar,OtherOptions,MatIndex>& expr);
+  public:
+
+    typedef internal::SparseRefBase<Ref> Base;
+    EIGEN_SPARSE_PUBLIC_INTERFACE(Ref)
+
+
+    #ifndef EIGEN_PARSED_BY_DOXYGEN
+    template<int OtherOptions>
+    inline Ref(SparseMatrix<MatScalar,OtherOptions,MatIndex>& expr)
+    {
+      EIGEN_STATIC_ASSERT(bool(Traits::template match<SparseMatrix<MatScalar,OtherOptions,MatIndex> >::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
+      eigen_assert( ((Options & int(StandardCompressedFormat))==0) || (expr.isCompressed()) );
+      Base::construct(expr.derived());
+    }
+    
+    template<int OtherOptions>
+    inline Ref(MappedSparseMatrix<MatScalar,OtherOptions,MatIndex>& expr)
+    {
+      EIGEN_STATIC_ASSERT(bool(Traits::template match<SparseMatrix<MatScalar,OtherOptions,MatIndex> >::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
+      eigen_assert( ((Options & int(StandardCompressedFormat))==0) || (expr.isCompressed()) );
+      Base::construct(expr.derived());
+    }
+    
+    template<typename Derived>
+    inline Ref(const SparseCompressedBase<Derived>& expr)
+    #else
+    /** Implicit constructor from any sparse expression (2D matrix or 1D vector) */
+    template<typename Derived>
+    inline Ref(SparseCompressedBase<Derived>& expr)
+    #endif
+    {
+      EIGEN_STATIC_ASSERT(bool(internal::is_lvalue<Derived>::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
+      EIGEN_STATIC_ASSERT(bool(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
+      eigen_assert( ((Options & int(StandardCompressedFormat))==0) || (expr.isCompressed()) );
+      Base::construct(expr.const_cast_derived());
+    }
+};
+
+// this is the const ref version
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType>
+  : public internal::SparseRefBase<Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+{
+    typedef SparseMatrix<MatScalar,MatOptions,MatIndex> TPlainObjectType;
+    typedef internal::traits<Ref> Traits;
+  public:
+
+    typedef internal::SparseRefBase<Ref> Base;
+    EIGEN_SPARSE_PUBLIC_INTERFACE(Ref)
+
+    template<typename Derived>
+    inline Ref(const SparseMatrixBase<Derived>& expr) : m_hasCopy(false)
+    {
+      construct(expr.derived(), typename Traits::template match<Derived>::type());
+    }
+
+    inline Ref(const Ref& other) : Base(other), m_hasCopy(false) {
+      // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy
+    }
+
+    template<typename OtherRef>
+    inline Ref(const RefBase<OtherRef>& other) : m_hasCopy(false) {
+      construct(other.derived(), typename Traits::template match<OtherRef>::type());
+    }
+
+    ~Ref() {
+      if(m_hasCopy) {
+        TPlainObjectType* obj = reinterpret_cast<TPlainObjectType*>(&m_storage);
+        obj->~TPlainObjectType();
+      }
+    }
+
+  protected:
+
+    template<typename Expression>
+    void construct(const Expression& expr,internal::true_type)
+    {
+      if((Options & int(StandardCompressedFormat)) && (!expr.isCompressed()))
+      {
+        TPlainObjectType* obj = reinterpret_cast<TPlainObjectType*>(&m_storage);
+        ::new (obj) TPlainObjectType(expr);
+        m_hasCopy = true;
+        Base::construct(*obj);
+      }
+      else
+      {
+        Base::construct(expr);
+      }
+    }
+
+    template<typename Expression>
+    void construct(const Expression& expr, internal::false_type)
+    {
+      TPlainObjectType* obj = reinterpret_cast<TPlainObjectType*>(&m_storage);
+      ::new (obj) TPlainObjectType(expr);
+      m_hasCopy = true;
+      Base::construct(*obj);
+    }
+
+  protected:
+    typename internal::aligned_storage<sizeof(TPlainObjectType), EIGEN_ALIGNOF(TPlainObjectType)>::type m_storage;
+    bool m_hasCopy;
+};
+
+
+
+/**
+  * \ingroup SparseCore_Module
+  *
+  * \brief A sparse vector expression referencing an existing sparse vector expression
+  *
+  * \tparam SparseVectorType the equivalent sparse vector type of the referenced data, it must be a template instance of class SparseVector.
+  *
+  * \sa class Ref
+  */
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType >
+  : public internal::SparseRefBase<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType > >
+#else
+template<typename SparseVectorType>
+class Ref<SparseVectorType>
+  : public SparseMapBase<Derived,WriteAccessors>
+#endif
+{
+    typedef SparseVector<MatScalar,MatOptions,MatIndex> PlainObjectType;
+    typedef internal::traits<Ref> Traits;
+    template<int OtherOptions>
+    inline Ref(const SparseVector<MatScalar,OtherOptions,MatIndex>& expr);
+  public:
+
+    typedef internal::SparseRefBase<Ref> Base;
+    EIGEN_SPARSE_PUBLIC_INTERFACE(Ref)
+
+    #ifndef EIGEN_PARSED_BY_DOXYGEN
+    template<int OtherOptions>
+    inline Ref(SparseVector<MatScalar,OtherOptions,MatIndex>& expr)
+    {
+      EIGEN_STATIC_ASSERT(bool(Traits::template match<SparseVector<MatScalar,OtherOptions,MatIndex> >::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
+      Base::construct(expr.derived());
+    }
+
+    template<typename Derived>
+    inline Ref(const SparseCompressedBase<Derived>& expr)
+    #else
+    /** Implicit constructor from any 1D sparse vector expression */
+    template<typename Derived>
+    inline Ref(SparseCompressedBase<Derived>& expr)
+    #endif
+    {
+      EIGEN_STATIC_ASSERT(bool(internal::is_lvalue<Derived>::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
+      EIGEN_STATIC_ASSERT(bool(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
+      Base::construct(expr.const_cast_derived());
+    }
+};
+
+// this is the const ref version
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+class Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType>
+  : public internal::SparseRefBase<Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+{
+    typedef SparseVector<MatScalar,MatOptions,MatIndex> TPlainObjectType;
+    typedef internal::traits<Ref> Traits;
+  public:
+
+    typedef internal::SparseRefBase<Ref> Base;
+    EIGEN_SPARSE_PUBLIC_INTERFACE(Ref)
+
+    template<typename Derived>
+    inline Ref(const SparseMatrixBase<Derived>& expr) : m_hasCopy(false)
+    {
+      construct(expr.derived(), typename Traits::template match<Derived>::type());
+    }
+
+    inline Ref(const Ref& other) : Base(other), m_hasCopy(false) {
+      // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy
+    }
+
+    template<typename OtherRef>
+    inline Ref(const RefBase<OtherRef>& other) : m_hasCopy(false) {
+      construct(other.derived(), typename Traits::template match<OtherRef>::type());
+    }
+
+    ~Ref() {
+      if(m_hasCopy) {
+        TPlainObjectType* obj = reinterpret_cast<TPlainObjectType*>(&m_storage);
+        obj->~TPlainObjectType();
+      }
+    }
+
+  protected:
+
+    template<typename Expression>
+    void construct(const Expression& expr,internal::true_type)
+    {
+      Base::construct(expr);
+    }
+
+    template<typename Expression>
+    void construct(const Expression& expr, internal::false_type)
+    {
+      TPlainObjectType* obj = reinterpret_cast<TPlainObjectType*>(&m_storage);
+      ::new (obj) TPlainObjectType(expr);
+      m_hasCopy = true;
+      Base::construct(*obj);
+    }
+
+  protected:
+    typename internal::aligned_storage<sizeof(TPlainObjectType), EIGEN_ALIGNOF(TPlainObjectType)>::type m_storage;
+    bool m_hasCopy;
+};
+
+namespace internal {
+
+// FIXME shall we introduce a general evaluatior_ref that we can specialize for any sparse object once, and thus remove this copy-pasta thing...
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+  : evaluator<SparseCompressedBase<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
+{
+  typedef evaluator<SparseCompressedBase<Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
+  typedef Ref<SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;  
+  evaluator() : Base() {}
+  explicit evaluator(const XprType &mat) : Base(mat) {}
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+  : evaluator<SparseCompressedBase<Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
+{
+  typedef evaluator<SparseCompressedBase<Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
+  typedef Ref<const SparseMatrix<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;  
+  evaluator() : Base() {}
+  explicit evaluator(const XprType &mat) : Base(mat) {}
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+  : evaluator<SparseCompressedBase<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
+{
+  typedef evaluator<SparseCompressedBase<Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
+  typedef Ref<SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;
+  evaluator() : Base() {}
+  explicit evaluator(const XprType &mat) : Base(mat) {}
+};
+
+template<typename MatScalar, int MatOptions, typename MatIndex, int Options, typename StrideType>
+struct evaluator<Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> >
+  : evaluator<SparseCompressedBase<Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> > >
+{
+  typedef evaluator<SparseCompressedBase<Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> > > Base;
+  typedef Ref<const SparseVector<MatScalar,MatOptions,MatIndex>, Options, StrideType> XprType;
+  evaluator() : Base() {}
+  explicit evaluator(const XprType &mat) : Base(mat) {}
+};
+
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_REF_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSelfAdjointView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSelfAdjointView.h
new file mode 100644
index 0000000..85b00e1
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSelfAdjointView.h
@@ -0,0 +1,659 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_SELFADJOINTVIEW_H
+#define EIGEN_SPARSE_SELFADJOINTVIEW_H
+
+namespace Eigen { 
+  
+/** \ingroup SparseCore_Module
+  * \class SparseSelfAdjointView
+  *
+  * \brief Pseudo expression to manipulate a triangular sparse matrix as a selfadjoint matrix.
+  *
+  * \param MatrixType the type of the dense matrix storing the coefficients
+  * \param Mode can be either \c #Lower or \c #Upper
+  *
+  * This class is an expression of a sefladjoint matrix from a triangular part of a matrix
+  * with given dense storage of the coefficients. It is the return type of MatrixBase::selfadjointView()
+  * and most of the time this is the only way that it is used.
+  *
+  * \sa SparseMatrixBase::selfadjointView()
+  */
+namespace internal {
+  
+template<typename MatrixType, unsigned int Mode>
+struct traits<SparseSelfAdjointView<MatrixType,Mode> > : traits<MatrixType> {
+};
+
+template<int SrcMode,int DstMode,typename MatrixType,int DestOrder>
+void permute_symm_to_symm(const MatrixType& mat, SparseMatrix<typename MatrixType::Scalar,DestOrder,typename MatrixType::StorageIndex>& _dest, const typename MatrixType::StorageIndex* perm = 0);
+
+template<int Mode,typename MatrixType,int DestOrder>
+void permute_symm_to_fullsymm(const MatrixType& mat, SparseMatrix<typename MatrixType::Scalar,DestOrder,typename MatrixType::StorageIndex>& _dest, const typename MatrixType::StorageIndex* perm = 0);
+
+}
+
+template<typename MatrixType, unsigned int _Mode> class SparseSelfAdjointView
+  : public EigenBase<SparseSelfAdjointView<MatrixType,_Mode> >
+{
+  public:
+    
+    enum {
+      Mode = _Mode,
+      TransposeMode = ((Mode & Upper) ? Lower : 0) | ((Mode & Lower) ? Upper : 0),
+      RowsAtCompileTime = internal::traits<SparseSelfAdjointView>::RowsAtCompileTime,
+      ColsAtCompileTime = internal::traits<SparseSelfAdjointView>::ColsAtCompileTime
+    };
+
+    typedef EigenBase<SparseSelfAdjointView> Base;
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename MatrixType::StorageIndex StorageIndex;
+    typedef Matrix<StorageIndex,Dynamic,1> VectorI;
+    typedef typename internal::ref_selector<MatrixType>::non_const_type MatrixTypeNested;
+    typedef typename internal::remove_all<MatrixTypeNested>::type _MatrixTypeNested;
+    
+    explicit inline SparseSelfAdjointView(MatrixType& matrix) : m_matrix(matrix)
+    {
+      eigen_assert(rows()==cols() && "SelfAdjointView is only for squared matrices");
+    }
+
+    inline Index rows() const { return m_matrix.rows(); }
+    inline Index cols() const { return m_matrix.cols(); }
+
+    /** \internal \returns a reference to the nested matrix */
+    const _MatrixTypeNested& matrix() const { return m_matrix; }
+    typename internal::remove_reference<MatrixTypeNested>::type& matrix() { return m_matrix; }
+
+    /** \returns an expression of the matrix product between a sparse self-adjoint matrix \c *this and a sparse matrix \a rhs.
+      *
+      * Note that there is no algorithmic advantage of performing such a product compared to a general sparse-sparse matrix product.
+      * Indeed, the SparseSelfadjointView operand is first copied into a temporary SparseMatrix before computing the product.
+      */
+    template<typename OtherDerived>
+    Product<SparseSelfAdjointView, OtherDerived>
+    operator*(const SparseMatrixBase<OtherDerived>& rhs) const
+    {
+      return Product<SparseSelfAdjointView, OtherDerived>(*this, rhs.derived());
+    }
+
+    /** \returns an expression of the matrix product between a sparse matrix \a lhs and a sparse self-adjoint matrix \a rhs.
+      *
+      * Note that there is no algorithmic advantage of performing such a product compared to a general sparse-sparse matrix product.
+      * Indeed, the SparseSelfadjointView operand is first copied into a temporary SparseMatrix before computing the product.
+      */
+    template<typename OtherDerived> friend
+    Product<OtherDerived, SparseSelfAdjointView>
+    operator*(const SparseMatrixBase<OtherDerived>& lhs, const SparseSelfAdjointView& rhs)
+    {
+      return Product<OtherDerived, SparseSelfAdjointView>(lhs.derived(), rhs);
+    }
+    
+    /** Efficient sparse self-adjoint matrix times dense vector/matrix product */
+    template<typename OtherDerived>
+    Product<SparseSelfAdjointView,OtherDerived>
+    operator*(const MatrixBase<OtherDerived>& rhs) const
+    {
+      return Product<SparseSelfAdjointView,OtherDerived>(*this, rhs.derived());
+    }
+
+    /** Efficient dense vector/matrix times sparse self-adjoint matrix product */
+    template<typename OtherDerived> friend
+    Product<OtherDerived,SparseSelfAdjointView>
+    operator*(const MatrixBase<OtherDerived>& lhs, const SparseSelfAdjointView& rhs)
+    {
+      return Product<OtherDerived,SparseSelfAdjointView>(lhs.derived(), rhs);
+    }
+
+    /** Perform a symmetric rank K update of the selfadjoint matrix \c *this:
+      * \f$ this = this + \alpha ( u u^* ) \f$ where \a u is a vector or matrix.
+      *
+      * \returns a reference to \c *this
+      *
+      * To perform \f$ this = this + \alpha ( u^* u ) \f$ you can simply
+      * call this function with u.adjoint().
+      */
+    template<typename DerivedU>
+    SparseSelfAdjointView& rankUpdate(const SparseMatrixBase<DerivedU>& u, const Scalar& alpha = Scalar(1));
+    
+    /** \returns an expression of P H P^-1 */
+    // TODO implement twists in a more evaluator friendly fashion
+    SparseSymmetricPermutationProduct<_MatrixTypeNested,Mode> twistedBy(const PermutationMatrix<Dynamic,Dynamic,StorageIndex>& perm) const
+    {
+      return SparseSymmetricPermutationProduct<_MatrixTypeNested,Mode>(m_matrix, perm);
+    }
+
+    template<typename SrcMatrixType,int SrcMode>
+    SparseSelfAdjointView& operator=(const SparseSymmetricPermutationProduct<SrcMatrixType,SrcMode>& permutedMatrix)
+    {
+      internal::call_assignment_no_alias_no_transpose(*this, permutedMatrix);
+      return *this;
+    }
+
+    SparseSelfAdjointView& operator=(const SparseSelfAdjointView& src)
+    {
+      PermutationMatrix<Dynamic,Dynamic,StorageIndex> pnull;
+      return *this = src.twistedBy(pnull);
+    }
+
+    // Since we override the copy-assignment operator, we need to explicitly re-declare the copy-constructor
+    EIGEN_DEFAULT_COPY_CONSTRUCTOR(SparseSelfAdjointView)
+
+    template<typename SrcMatrixType,unsigned int SrcMode>
+    SparseSelfAdjointView& operator=(const SparseSelfAdjointView<SrcMatrixType,SrcMode>& src)
+    {
+      PermutationMatrix<Dynamic,Dynamic,StorageIndex> pnull;
+      return *this = src.twistedBy(pnull);
+    }
+    
+    void resize(Index rows, Index cols)
+    {
+      EIGEN_ONLY_USED_FOR_DEBUG(rows);
+      EIGEN_ONLY_USED_FOR_DEBUG(cols);
+      eigen_assert(rows == this->rows() && cols == this->cols()
+                && "SparseSelfadjointView::resize() does not actually allow to resize.");
+    }
+    
+  protected:
+
+    MatrixTypeNested m_matrix;
+    //mutable VectorI m_countPerRow;
+    //mutable VectorI m_countPerCol;
+  private:
+    template<typename Dest> void evalTo(Dest &) const;
+};
+
+/***************************************************************************
+* Implementation of SparseMatrixBase methods
+***************************************************************************/
+
+template<typename Derived>
+template<unsigned int UpLo>
+typename SparseMatrixBase<Derived>::template ConstSelfAdjointViewReturnType<UpLo>::Type SparseMatrixBase<Derived>::selfadjointView() const
+{
+  return SparseSelfAdjointView<const Derived, UpLo>(derived());
+}
+
+template<typename Derived>
+template<unsigned int UpLo>
+typename SparseMatrixBase<Derived>::template SelfAdjointViewReturnType<UpLo>::Type SparseMatrixBase<Derived>::selfadjointView()
+{
+  return SparseSelfAdjointView<Derived, UpLo>(derived());
+}
+
+/***************************************************************************
+* Implementation of SparseSelfAdjointView methods
+***************************************************************************/
+
+template<typename MatrixType, unsigned int Mode>
+template<typename DerivedU>
+SparseSelfAdjointView<MatrixType,Mode>&
+SparseSelfAdjointView<MatrixType,Mode>::rankUpdate(const SparseMatrixBase<DerivedU>& u, const Scalar& alpha)
+{
+  SparseMatrix<Scalar,(MatrixType::Flags&RowMajorBit)?RowMajor:ColMajor> tmp = u * u.adjoint();
+  if(alpha==Scalar(0))
+    m_matrix = tmp.template triangularView<Mode>();
+  else
+    m_matrix += alpha * tmp.template triangularView<Mode>();
+
+  return *this;
+}
+
+namespace internal {
+  
+// TODO currently a selfadjoint expression has the form SelfAdjointView<.,.>
+//      in the future selfadjoint-ness should be defined by the expression traits
+//      such that Transpose<SelfAdjointView<.,.> > is valid. (currently TriangularBase::transpose() is overloaded to make it work)
+template<typename MatrixType, unsigned int Mode>
+struct evaluator_traits<SparseSelfAdjointView<MatrixType,Mode> >
+{
+  typedef typename storage_kind_to_evaluator_kind<typename MatrixType::StorageKind>::Kind Kind;
+  typedef SparseSelfAdjointShape Shape;
+};
+
+struct SparseSelfAdjoint2Sparse {};
+
+template<> struct AssignmentKind<SparseShape,SparseSelfAdjointShape> { typedef SparseSelfAdjoint2Sparse Kind; };
+template<> struct AssignmentKind<SparseSelfAdjointShape,SparseShape> { typedef Sparse2Sparse Kind; };
+
+template< typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, SparseSelfAdjoint2Sparse>
+{
+  typedef typename DstXprType::StorageIndex StorageIndex;
+  typedef internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> AssignOpType;
+
+  template<typename DestScalar,int StorageOrder>
+  static void run(SparseMatrix<DestScalar,StorageOrder,StorageIndex> &dst, const SrcXprType &src, const AssignOpType&/*func*/)
+  {
+    internal::permute_symm_to_fullsymm<SrcXprType::Mode>(src.matrix(), dst);
+  }
+
+  // FIXME: the handling of += and -= in sparse matrices should be cleanup so that next two overloads could be reduced to:
+  template<typename DestScalar,int StorageOrder,typename AssignFunc>
+  static void run(SparseMatrix<DestScalar,StorageOrder,StorageIndex> &dst, const SrcXprType &src, const AssignFunc& func)
+  {
+    SparseMatrix<DestScalar,StorageOrder,StorageIndex> tmp(src.rows(),src.cols());
+    run(tmp, src, AssignOpType());
+    call_assignment_no_alias_no_transpose(dst, tmp, func);
+  }
+
+  template<typename DestScalar,int StorageOrder>
+  static void run(SparseMatrix<DestScalar,StorageOrder,StorageIndex> &dst, const SrcXprType &src,
+                  const internal::add_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar>& /* func */)
+  {
+    SparseMatrix<DestScalar,StorageOrder,StorageIndex> tmp(src.rows(),src.cols());
+    run(tmp, src, AssignOpType());
+    dst += tmp;
+  }
+
+  template<typename DestScalar,int StorageOrder>
+  static void run(SparseMatrix<DestScalar,StorageOrder,StorageIndex> &dst, const SrcXprType &src,
+                  const internal::sub_assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar>& /* func */)
+  {
+    SparseMatrix<DestScalar,StorageOrder,StorageIndex> tmp(src.rows(),src.cols());
+    run(tmp, src, AssignOpType());
+    dst -= tmp;
+  }
+  
+  template<typename DestScalar>
+  static void run(DynamicSparseMatrix<DestScalar,ColMajor,StorageIndex>& dst, const SrcXprType &src, const AssignOpType&/*func*/)
+  {
+    // TODO directly evaluate into dst;
+    SparseMatrix<DestScalar,ColMajor,StorageIndex> tmp(dst.rows(),dst.cols());
+    internal::permute_symm_to_fullsymm<SrcXprType::Mode>(src.matrix(), tmp);
+    dst = tmp;
+  }
+};
+
+} // end namespace internal
+
+/***************************************************************************
+* Implementation of sparse self-adjoint time dense matrix
+***************************************************************************/
+
+namespace internal {
+
+template<int Mode, typename SparseLhsType, typename DenseRhsType, typename DenseResType, typename AlphaType>
+inline void sparse_selfadjoint_time_dense_product(const SparseLhsType& lhs, const DenseRhsType& rhs, DenseResType& res, const AlphaType& alpha)
+{
+  EIGEN_ONLY_USED_FOR_DEBUG(alpha);
+  
+  typedef typename internal::nested_eval<SparseLhsType,DenseRhsType::MaxColsAtCompileTime>::type SparseLhsTypeNested;
+  typedef typename internal::remove_all<SparseLhsTypeNested>::type SparseLhsTypeNestedCleaned;
+  typedef evaluator<SparseLhsTypeNestedCleaned> LhsEval;
+  typedef typename LhsEval::InnerIterator LhsIterator;
+  typedef typename SparseLhsType::Scalar LhsScalar;
+  
+  enum {
+    LhsIsRowMajor = (LhsEval::Flags&RowMajorBit)==RowMajorBit,
+    ProcessFirstHalf =
+              ((Mode&(Upper|Lower))==(Upper|Lower))
+          || ( (Mode&Upper) && !LhsIsRowMajor)
+          || ( (Mode&Lower) && LhsIsRowMajor),
+    ProcessSecondHalf = !ProcessFirstHalf
+  };
+  
+  SparseLhsTypeNested lhs_nested(lhs);
+  LhsEval lhsEval(lhs_nested);
+
+  // work on one column at once
+  for (Index k=0; k<rhs.cols(); ++k)
+  {
+    for (Index j=0; j<lhs.outerSize(); ++j)
+    {
+      LhsIterator i(lhsEval,j);
+      // handle diagonal coeff
+      if (ProcessSecondHalf)
+      {
+        while (i && i.index()<j) ++i;
+        if(i && i.index()==j)
+        {
+          res.coeffRef(j,k) += alpha * i.value() * rhs.coeff(j,k);
+          ++i;
+        }
+      }
+
+      // premultiplied rhs for scatters
+      typename ScalarBinaryOpTraits<AlphaType, typename DenseRhsType::Scalar>::ReturnType rhs_j(alpha*rhs(j,k));
+      // accumulator for partial scalar product
+      typename DenseResType::Scalar res_j(0);
+      for(; (ProcessFirstHalf ? i && i.index() < j : i) ; ++i)
+      {
+        LhsScalar lhs_ij = i.value();
+        if(!LhsIsRowMajor) lhs_ij = numext::conj(lhs_ij);
+        res_j += lhs_ij * rhs.coeff(i.index(),k);
+        res(i.index(),k) += numext::conj(lhs_ij) * rhs_j;
+      }
+      res.coeffRef(j,k) += alpha * res_j;
+
+      // handle diagonal coeff
+      if (ProcessFirstHalf && i && (i.index()==j))
+        res.coeffRef(j,k) += alpha * i.value() * rhs.coeff(j,k);
+    }
+  }
+}
+
+
+template<typename LhsView, typename Rhs, int ProductType>
+struct generic_product_impl<LhsView, Rhs, SparseSelfAdjointShape, DenseShape, ProductType>
+: generic_product_impl_base<LhsView, Rhs, generic_product_impl<LhsView, Rhs, SparseSelfAdjointShape, DenseShape, ProductType> >
+{
+  template<typename Dest>
+  static void scaleAndAddTo(Dest& dst, const LhsView& lhsView, const Rhs& rhs, const typename Dest::Scalar& alpha)
+  {
+    typedef typename LhsView::_MatrixTypeNested Lhs;
+    typedef typename nested_eval<Lhs,Dynamic>::type LhsNested;
+    typedef typename nested_eval<Rhs,Dynamic>::type RhsNested;
+    LhsNested lhsNested(lhsView.matrix());
+    RhsNested rhsNested(rhs);
+    
+    internal::sparse_selfadjoint_time_dense_product<LhsView::Mode>(lhsNested, rhsNested, dst, alpha);
+  }
+};
+
+template<typename Lhs, typename RhsView, int ProductType>
+struct generic_product_impl<Lhs, RhsView, DenseShape, SparseSelfAdjointShape, ProductType>
+: generic_product_impl_base<Lhs, RhsView, generic_product_impl<Lhs, RhsView, DenseShape, SparseSelfAdjointShape, ProductType> >
+{
+  template<typename Dest>
+  static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const RhsView& rhsView, const typename Dest::Scalar& alpha)
+  {
+    typedef typename RhsView::_MatrixTypeNested Rhs;
+    typedef typename nested_eval<Lhs,Dynamic>::type LhsNested;
+    typedef typename nested_eval<Rhs,Dynamic>::type RhsNested;
+    LhsNested lhsNested(lhs);
+    RhsNested rhsNested(rhsView.matrix());
+    
+    // transpose everything
+    Transpose<Dest> dstT(dst);
+    internal::sparse_selfadjoint_time_dense_product<RhsView::TransposeMode>(rhsNested.transpose(), lhsNested.transpose(), dstT, alpha);
+  }
+};
+
+// NOTE: these two overloads are needed to evaluate the sparse selfadjoint view into a full sparse matrix
+// TODO: maybe the copy could be handled by generic_product_impl so that these overloads would not be needed anymore
+
+template<typename LhsView, typename Rhs, int ProductTag>
+struct product_evaluator<Product<LhsView, Rhs, DefaultProduct>, ProductTag, SparseSelfAdjointShape, SparseShape>
+  : public evaluator<typename Product<typename Rhs::PlainObject, Rhs, DefaultProduct>::PlainObject>
+{
+  typedef Product<LhsView, Rhs, DefaultProduct> XprType;
+  typedef typename XprType::PlainObject PlainObject;
+  typedef evaluator<PlainObject> Base;
+
+  product_evaluator(const XprType& xpr)
+    : m_lhs(xpr.lhs()), m_result(xpr.rows(), xpr.cols())
+  {
+    ::new (static_cast<Base*>(this)) Base(m_result);
+    generic_product_impl<typename Rhs::PlainObject, Rhs, SparseShape, SparseShape, ProductTag>::evalTo(m_result, m_lhs, xpr.rhs());
+  }
+  
+protected:
+  typename Rhs::PlainObject m_lhs;
+  PlainObject m_result;
+};
+
+template<typename Lhs, typename RhsView, int ProductTag>
+struct product_evaluator<Product<Lhs, RhsView, DefaultProduct>, ProductTag, SparseShape, SparseSelfAdjointShape>
+  : public evaluator<typename Product<Lhs, typename Lhs::PlainObject, DefaultProduct>::PlainObject>
+{
+  typedef Product<Lhs, RhsView, DefaultProduct> XprType;
+  typedef typename XprType::PlainObject PlainObject;
+  typedef evaluator<PlainObject> Base;
+
+  product_evaluator(const XprType& xpr)
+    : m_rhs(xpr.rhs()), m_result(xpr.rows(), xpr.cols())
+  {
+    ::new (static_cast<Base*>(this)) Base(m_result);
+    generic_product_impl<Lhs, typename Lhs::PlainObject, SparseShape, SparseShape, ProductTag>::evalTo(m_result, xpr.lhs(), m_rhs);
+  }
+  
+protected:
+  typename Lhs::PlainObject m_rhs;
+  PlainObject m_result;
+};
+
+} // namespace internal
+
+/***************************************************************************
+* Implementation of symmetric copies and permutations
+***************************************************************************/
+namespace internal {
+
+template<int Mode,typename MatrixType,int DestOrder>
+void permute_symm_to_fullsymm(const MatrixType& mat, SparseMatrix<typename MatrixType::Scalar,DestOrder,typename MatrixType::StorageIndex>& _dest, const typename MatrixType::StorageIndex* perm)
+{
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  typedef typename MatrixType::Scalar Scalar;
+  typedef SparseMatrix<Scalar,DestOrder,StorageIndex> Dest;
+  typedef Matrix<StorageIndex,Dynamic,1> VectorI;
+  typedef evaluator<MatrixType> MatEval;
+  typedef typename evaluator<MatrixType>::InnerIterator MatIterator;
+  
+  MatEval matEval(mat);
+  Dest& dest(_dest.derived());
+  enum {
+    StorageOrderMatch = int(Dest::IsRowMajor) == int(MatrixType::IsRowMajor)
+  };
+  
+  Index size = mat.rows();
+  VectorI count;
+  count.resize(size);
+  count.setZero();
+  dest.resize(size,size);
+  for(Index j = 0; j<size; ++j)
+  {
+    Index jp = perm ? perm[j] : j;
+    for(MatIterator it(matEval,j); it; ++it)
+    {
+      Index i = it.index();
+      Index r = it.row();
+      Index c = it.col();
+      Index ip = perm ? perm[i] : i;
+      if(Mode==int(Upper|Lower))
+        count[StorageOrderMatch ? jp : ip]++;
+      else if(r==c)
+        count[ip]++;
+      else if(( Mode==Lower && r>c) || ( Mode==Upper && r<c))
+      {
+        count[ip]++;
+        count[jp]++;
+      }
+    }
+  }
+  Index nnz = count.sum();
+  
+  // reserve space
+  dest.resizeNonZeros(nnz);
+  dest.outerIndexPtr()[0] = 0;
+  for(Index j=0; j<size; ++j)
+    dest.outerIndexPtr()[j+1] = dest.outerIndexPtr()[j] + count[j];
+  for(Index j=0; j<size; ++j)
+    count[j] = dest.outerIndexPtr()[j];
+  
+  // copy data
+  for(StorageIndex j = 0; j<size; ++j)
+  {
+    for(MatIterator it(matEval,j); it; ++it)
+    {
+      StorageIndex i = internal::convert_index<StorageIndex>(it.index());
+      Index r = it.row();
+      Index c = it.col();
+      
+      StorageIndex jp = perm ? perm[j] : j;
+      StorageIndex ip = perm ? perm[i] : i;
+      
+      if(Mode==int(Upper|Lower))
+      {
+        Index k = count[StorageOrderMatch ? jp : ip]++;
+        dest.innerIndexPtr()[k] = StorageOrderMatch ? ip : jp;
+        dest.valuePtr()[k] = it.value();
+      }
+      else if(r==c)
+      {
+        Index k = count[ip]++;
+        dest.innerIndexPtr()[k] = ip;
+        dest.valuePtr()[k] = it.value();
+      }
+      else if(( (Mode&Lower)==Lower && r>c) || ( (Mode&Upper)==Upper && r<c))
+      {
+        if(!StorageOrderMatch)
+          std::swap(ip,jp);
+        Index k = count[jp]++;
+        dest.innerIndexPtr()[k] = ip;
+        dest.valuePtr()[k] = it.value();
+        k = count[ip]++;
+        dest.innerIndexPtr()[k] = jp;
+        dest.valuePtr()[k] = numext::conj(it.value());
+      }
+    }
+  }
+}
+
+template<int _SrcMode,int _DstMode,typename MatrixType,int DstOrder>
+void permute_symm_to_symm(const MatrixType& mat, SparseMatrix<typename MatrixType::Scalar,DstOrder,typename MatrixType::StorageIndex>& _dest, const typename MatrixType::StorageIndex* perm)
+{
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  typedef typename MatrixType::Scalar Scalar;
+  SparseMatrix<Scalar,DstOrder,StorageIndex>& dest(_dest.derived());
+  typedef Matrix<StorageIndex,Dynamic,1> VectorI;
+  typedef evaluator<MatrixType> MatEval;
+  typedef typename evaluator<MatrixType>::InnerIterator MatIterator;
+
+  enum {
+    SrcOrder = MatrixType::IsRowMajor ? RowMajor : ColMajor,
+    StorageOrderMatch = int(SrcOrder) == int(DstOrder),
+    DstMode = DstOrder==RowMajor ? (_DstMode==Upper ? Lower : Upper) : _DstMode,
+    SrcMode = SrcOrder==RowMajor ? (_SrcMode==Upper ? Lower : Upper) : _SrcMode
+  };
+
+  MatEval matEval(mat);
+  
+  Index size = mat.rows();
+  VectorI count(size);
+  count.setZero();
+  dest.resize(size,size);
+  for(StorageIndex j = 0; j<size; ++j)
+  {
+    StorageIndex jp = perm ? perm[j] : j;
+    for(MatIterator it(matEval,j); it; ++it)
+    {
+      StorageIndex i = it.index();
+      if((int(SrcMode)==int(Lower) && i<j) || (int(SrcMode)==int(Upper) && i>j))
+        continue;
+                  
+      StorageIndex ip = perm ? perm[i] : i;
+      count[int(DstMode)==int(Lower) ? (std::min)(ip,jp) : (std::max)(ip,jp)]++;
+    }
+  }
+  dest.outerIndexPtr()[0] = 0;
+  for(Index j=0; j<size; ++j)
+    dest.outerIndexPtr()[j+1] = dest.outerIndexPtr()[j] + count[j];
+  dest.resizeNonZeros(dest.outerIndexPtr()[size]);
+  for(Index j=0; j<size; ++j)
+    count[j] = dest.outerIndexPtr()[j];
+  
+  for(StorageIndex j = 0; j<size; ++j)
+  {
+    
+    for(MatIterator it(matEval,j); it; ++it)
+    {
+      StorageIndex i = it.index();
+      if((int(SrcMode)==int(Lower) && i<j) || (int(SrcMode)==int(Upper) && i>j))
+        continue;
+                  
+      StorageIndex jp = perm ? perm[j] : j;
+      StorageIndex ip = perm? perm[i] : i;
+      
+      Index k = count[int(DstMode)==int(Lower) ? (std::min)(ip,jp) : (std::max)(ip,jp)]++;
+      dest.innerIndexPtr()[k] = int(DstMode)==int(Lower) ? (std::max)(ip,jp) : (std::min)(ip,jp);
+      
+      if(!StorageOrderMatch) std::swap(ip,jp);
+      if( ((int(DstMode)==int(Lower) && ip<jp) || (int(DstMode)==int(Upper) && ip>jp)))
+        dest.valuePtr()[k] = numext::conj(it.value());
+      else
+        dest.valuePtr()[k] = it.value();
+    }
+  }
+}
+
+}
+
+// TODO implement twists in a more evaluator friendly fashion
+
+namespace internal {
+
+template<typename MatrixType, int Mode>
+struct traits<SparseSymmetricPermutationProduct<MatrixType,Mode> > : traits<MatrixType> {
+};
+
+}
+
+template<typename MatrixType,int Mode>
+class SparseSymmetricPermutationProduct
+  : public EigenBase<SparseSymmetricPermutationProduct<MatrixType,Mode> >
+{
+  public:
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename MatrixType::StorageIndex StorageIndex;
+    enum {
+      RowsAtCompileTime = internal::traits<SparseSymmetricPermutationProduct>::RowsAtCompileTime,
+      ColsAtCompileTime = internal::traits<SparseSymmetricPermutationProduct>::ColsAtCompileTime
+    };
+  protected:
+    typedef PermutationMatrix<Dynamic,Dynamic,StorageIndex> Perm;
+  public:
+    typedef Matrix<StorageIndex,Dynamic,1> VectorI;
+    typedef typename MatrixType::Nested MatrixTypeNested;
+    typedef typename internal::remove_all<MatrixTypeNested>::type NestedExpression;
+    
+    SparseSymmetricPermutationProduct(const MatrixType& mat, const Perm& perm)
+      : m_matrix(mat), m_perm(perm)
+    {}
+    
+    inline Index rows() const { return m_matrix.rows(); }
+    inline Index cols() const { return m_matrix.cols(); }
+        
+    const NestedExpression& matrix() const { return m_matrix; }
+    const Perm& perm() const { return m_perm; }
+    
+  protected:
+    MatrixTypeNested m_matrix;
+    const Perm& m_perm;
+
+};
+
+namespace internal {
+  
+template<typename DstXprType, typename MatrixType, int Mode, typename Scalar>
+struct Assignment<DstXprType, SparseSymmetricPermutationProduct<MatrixType,Mode>, internal::assign_op<Scalar,typename MatrixType::Scalar>, Sparse2Sparse>
+{
+  typedef SparseSymmetricPermutationProduct<MatrixType,Mode> SrcXprType;
+  typedef typename DstXprType::StorageIndex DstIndex;
+  template<int Options>
+  static void run(SparseMatrix<Scalar,Options,DstIndex> &dst, const SrcXprType &src, const internal::assign_op<Scalar,typename MatrixType::Scalar> &)
+  {
+    // internal::permute_symm_to_fullsymm<Mode>(m_matrix,_dest,m_perm.indices().data());
+    SparseMatrix<Scalar,(Options&RowMajor)==RowMajor ? ColMajor : RowMajor, DstIndex> tmp;
+    internal::permute_symm_to_fullsymm<Mode>(src.matrix(),tmp,src.perm().indices().data());
+    dst = tmp;
+  }
+  
+  template<typename DestType,unsigned int DestMode>
+  static void run(SparseSelfAdjointView<DestType,DestMode>& dst, const SrcXprType &src, const internal::assign_op<Scalar,typename MatrixType::Scalar> &)
+  {
+    internal::permute_symm_to_symm<Mode,DestMode>(src.matrix(),dst.matrix(),src.perm().indices().data());
+  }
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_SELFADJOINTVIEW_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSolverBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSolverBase.h
new file mode 100644
index 0000000..b4c9a42
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSolverBase.h
@@ -0,0 +1,124 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSESOLVERBASE_H
+#define EIGEN_SPARSESOLVERBASE_H
+
+namespace Eigen { 
+
+namespace internal {
+
+  /** \internal
+  * Helper functions to solve with a sparse right-hand-side and result.
+  * The rhs is decomposed into small vertical panels which are solved through dense temporaries.
+  */
+template<typename Decomposition, typename Rhs, typename Dest>
+typename enable_if<Rhs::ColsAtCompileTime!=1 && Dest::ColsAtCompileTime!=1>::type
+solve_sparse_through_dense_panels(const Decomposition &dec, const Rhs& rhs, Dest &dest)
+{
+  EIGEN_STATIC_ASSERT((Dest::Flags&RowMajorBit)==0,THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+  typedef typename Dest::Scalar DestScalar;
+  // we process the sparse rhs per block of NbColsAtOnce columns temporarily stored into a dense matrix.
+  static const Index NbColsAtOnce = 4;
+  Index rhsCols = rhs.cols();
+  Index size = rhs.rows();
+  // the temporary matrices do not need more columns than NbColsAtOnce:
+  Index tmpCols = (std::min)(rhsCols, NbColsAtOnce); 
+  Eigen::Matrix<DestScalar,Dynamic,Dynamic> tmp(size,tmpCols);
+  Eigen::Matrix<DestScalar,Dynamic,Dynamic> tmpX(size,tmpCols);
+  for(Index k=0; k<rhsCols; k+=NbColsAtOnce)
+  {
+    Index actualCols = std::min<Index>(rhsCols-k, NbColsAtOnce);
+    tmp.leftCols(actualCols) = rhs.middleCols(k,actualCols);
+    tmpX.leftCols(actualCols) = dec.solve(tmp.leftCols(actualCols));
+    dest.middleCols(k,actualCols) = tmpX.leftCols(actualCols).sparseView();
+  }
+}
+
+// Overload for vector as rhs
+template<typename Decomposition, typename Rhs, typename Dest>
+typename enable_if<Rhs::ColsAtCompileTime==1 || Dest::ColsAtCompileTime==1>::type
+solve_sparse_through_dense_panels(const Decomposition &dec, const Rhs& rhs, Dest &dest)
+{
+  typedef typename Dest::Scalar DestScalar;
+  Index size = rhs.rows();
+  Eigen::Matrix<DestScalar,Dynamic,1> rhs_dense(rhs);
+  Eigen::Matrix<DestScalar,Dynamic,1> dest_dense(size);
+  dest_dense = dec.solve(rhs_dense);
+  dest = dest_dense.sparseView();
+}
+
+} // end namespace internal
+
+/** \class SparseSolverBase
+  * \ingroup SparseCore_Module
+  * \brief A base class for sparse solvers
+  *
+  * \tparam Derived the actual type of the solver.
+  *
+  */
+template<typename Derived>
+class SparseSolverBase : internal::noncopyable
+{
+  public:
+
+    /** Default constructor */
+    SparseSolverBase()
+      : m_isInitialized(false)
+    {}
+
+    ~SparseSolverBase()
+    {}
+
+    Derived& derived() { return *static_cast<Derived*>(this); }
+    const Derived& derived() const { return *static_cast<const Derived*>(this); }
+    
+    /** \returns an expression of the solution x of \f$ A x = b \f$ using the current decomposition of A.
+      *
+      * \sa compute()
+      */
+    template<typename Rhs>
+    inline const Solve<Derived, Rhs>
+    solve(const MatrixBase<Rhs>& b) const
+    {
+      eigen_assert(m_isInitialized && "Solver is not initialized.");
+      eigen_assert(derived().rows()==b.rows() && "solve(): invalid number of rows of the right hand side matrix b");
+      return Solve<Derived, Rhs>(derived(), b.derived());
+    }
+    
+    /** \returns an expression of the solution x of \f$ A x = b \f$ using the current decomposition of A.
+      *
+      * \sa compute()
+      */
+    template<typename Rhs>
+    inline const Solve<Derived, Rhs>
+    solve(const SparseMatrixBase<Rhs>& b) const
+    {
+      eigen_assert(m_isInitialized && "Solver is not initialized.");
+      eigen_assert(derived().rows()==b.rows() && "solve(): invalid number of rows of the right hand side matrix b");
+      return Solve<Derived, Rhs>(derived(), b.derived());
+    }
+    
+    #ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** \internal default implementation of solving with a sparse rhs */
+    template<typename Rhs,typename Dest>
+    void _solve_impl(const SparseMatrixBase<Rhs> &b, SparseMatrixBase<Dest> &dest) const
+    {
+      internal::solve_sparse_through_dense_panels(derived(), b.derived(), dest.derived());
+    }
+    #endif // EIGEN_PARSED_BY_DOXYGEN
+
+  protected:
+    
+    mutable bool m_isInitialized;
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSESOLVERBASE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSparseProductWithPruning.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSparseProductWithPruning.h
new file mode 100644
index 0000000..88820a4
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseSparseProductWithPruning.h
@@ -0,0 +1,198 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSESPARSEPRODUCTWITHPRUNING_H
+#define EIGEN_SPARSESPARSEPRODUCTWITHPRUNING_H
+
+namespace Eigen { 
+
+namespace internal {
+
+
+// perform a pseudo in-place sparse * sparse product assuming all matrices are col major
+template<typename Lhs, typename Rhs, typename ResultType>
+static void sparse_sparse_product_with_pruning_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res, const typename ResultType::RealScalar& tolerance)
+{
+  // return sparse_sparse_product_with_pruning_impl2(lhs,rhs,res);
+
+  typedef typename remove_all<Rhs>::type::Scalar RhsScalar;
+  typedef typename remove_all<ResultType>::type::Scalar ResScalar;
+  typedef typename remove_all<Lhs>::type::StorageIndex StorageIndex;
+
+  // make sure to call innerSize/outerSize since we fake the storage order.
+  Index rows = lhs.innerSize();
+  Index cols = rhs.outerSize();
+  //Index size = lhs.outerSize();
+  eigen_assert(lhs.outerSize() == rhs.innerSize());
+
+  // allocate a temporary buffer
+  AmbiVector<ResScalar,StorageIndex> tempVector(rows);
+
+  // mimics a resizeByInnerOuter:
+  if(ResultType::IsRowMajor)
+    res.resize(cols, rows);
+  else
+    res.resize(rows, cols);
+  
+  evaluator<Lhs> lhsEval(lhs);
+  evaluator<Rhs> rhsEval(rhs);
+  
+  // estimate the number of non zero entries
+  // given a rhs column containing Y non zeros, we assume that the respective Y columns
+  // of the lhs differs in average of one non zeros, thus the number of non zeros for
+  // the product of a rhs column with the lhs is X+Y where X is the average number of non zero
+  // per column of the lhs.
+  // Therefore, we have nnz(lhs*rhs) = nnz(lhs) + nnz(rhs)
+  Index estimated_nnz_prod = lhsEval.nonZerosEstimate() + rhsEval.nonZerosEstimate();
+
+  res.reserve(estimated_nnz_prod);
+  double ratioColRes = double(estimated_nnz_prod)/(double(lhs.rows())*double(rhs.cols()));
+  for (Index j=0; j<cols; ++j)
+  {
+    // FIXME:
+    //double ratioColRes = (double(rhs.innerVector(j).nonZeros()) + double(lhs.nonZeros())/double(lhs.cols()))/double(lhs.rows());
+    // let's do a more accurate determination of the nnz ratio for the current column j of res
+    tempVector.init(ratioColRes);
+    tempVector.setZero();
+    for (typename evaluator<Rhs>::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt)
+    {
+      // FIXME should be written like this: tmp += rhsIt.value() * lhs.col(rhsIt.index())
+      tempVector.restart();
+      RhsScalar x = rhsIt.value();
+      for (typename evaluator<Lhs>::InnerIterator lhsIt(lhsEval, rhsIt.index()); lhsIt; ++lhsIt)
+      {
+        tempVector.coeffRef(lhsIt.index()) += lhsIt.value() * x;
+      }
+    }
+    res.startVec(j);
+    for (typename AmbiVector<ResScalar,StorageIndex>::Iterator it(tempVector,tolerance); it; ++it)
+      res.insertBackByOuterInner(j,it.index()) = it.value();
+  }
+  res.finalize();
+}
+
+template<typename Lhs, typename Rhs, typename ResultType,
+  int LhsStorageOrder = traits<Lhs>::Flags&RowMajorBit,
+  int RhsStorageOrder = traits<Rhs>::Flags&RowMajorBit,
+  int ResStorageOrder = traits<ResultType>::Flags&RowMajorBit>
+struct sparse_sparse_product_with_pruning_selector;
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor,ColMajor>
+{
+  typedef typename ResultType::RealScalar RealScalar;
+
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
+  {
+    typename remove_all<ResultType>::type _res(res.rows(), res.cols());
+    internal::sparse_sparse_product_with_pruning_impl<Lhs,Rhs,ResultType>(lhs, rhs, _res, tolerance);
+    res.swap(_res);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor,RowMajor>
+{
+  typedef typename ResultType::RealScalar RealScalar;
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
+  {
+    // we need a col-major matrix to hold the result
+    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> SparseTemporaryType;
+    SparseTemporaryType _res(res.rows(), res.cols());
+    internal::sparse_sparse_product_with_pruning_impl<Lhs,Rhs,SparseTemporaryType>(lhs, rhs, _res, tolerance);
+    res = _res;
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor,RowMajor>
+{
+  typedef typename ResultType::RealScalar RealScalar;
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
+  {
+    // let's transpose the product to get a column x column product
+    typename remove_all<ResultType>::type _res(res.rows(), res.cols());
+    internal::sparse_sparse_product_with_pruning_impl<Rhs,Lhs,ResultType>(rhs, lhs, _res, tolerance);
+    res.swap(_res);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor,ColMajor>
+{
+  typedef typename ResultType::RealScalar RealScalar;
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
+  {
+    typedef SparseMatrix<typename Lhs::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixLhs;
+    typedef SparseMatrix<typename Rhs::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixRhs;
+    ColMajorMatrixLhs colLhs(lhs);
+    ColMajorMatrixRhs colRhs(rhs);
+    internal::sparse_sparse_product_with_pruning_impl<ColMajorMatrixLhs,ColMajorMatrixRhs,ResultType>(colLhs, colRhs, res, tolerance);
+
+    // let's transpose the product to get a column x column product
+//     typedef SparseMatrix<typename ResultType::Scalar> SparseTemporaryType;
+//     SparseTemporaryType _res(res.cols(), res.rows());
+//     sparse_sparse_product_with_pruning_impl<Rhs,Lhs,SparseTemporaryType>(rhs, lhs, _res);
+//     res = _res.transpose();
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,ColMajor,RowMajor,RowMajor>
+{
+  typedef typename ResultType::RealScalar RealScalar;
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
+  {
+    typedef SparseMatrix<typename Lhs::Scalar,RowMajor,typename Lhs::StorageIndex> RowMajorMatrixLhs;
+    RowMajorMatrixLhs rowLhs(lhs);
+    sparse_sparse_product_with_pruning_selector<RowMajorMatrixLhs,Rhs,ResultType,RowMajor,RowMajor>(rowLhs,rhs,res,tolerance);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,RowMajor,ColMajor,RowMajor>
+{
+  typedef typename ResultType::RealScalar RealScalar;
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
+  {
+    typedef SparseMatrix<typename Rhs::Scalar,RowMajor,typename Lhs::StorageIndex> RowMajorMatrixRhs;
+    RowMajorMatrixRhs rowRhs(rhs);
+    sparse_sparse_product_with_pruning_selector<Lhs,RowMajorMatrixRhs,ResultType,RowMajor,RowMajor,RowMajor>(lhs,rowRhs,res,tolerance);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,ColMajor,RowMajor,ColMajor>
+{
+  typedef typename ResultType::RealScalar RealScalar;
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
+  {
+    typedef SparseMatrix<typename Rhs::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixRhs;
+    ColMajorMatrixRhs colRhs(rhs);
+    internal::sparse_sparse_product_with_pruning_impl<Lhs,ColMajorMatrixRhs,ResultType>(lhs, colRhs, res, tolerance);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,RowMajor,ColMajor,ColMajor>
+{
+  typedef typename ResultType::RealScalar RealScalar;
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
+  {
+    typedef SparseMatrix<typename Lhs::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixLhs;
+    ColMajorMatrixLhs colLhs(lhs);
+    internal::sparse_sparse_product_with_pruning_impl<ColMajorMatrixLhs,Rhs,ResultType>(colLhs, rhs, res, tolerance);
+  }
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSESPARSEPRODUCTWITHPRUNING_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTranspose.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTranspose.h
new file mode 100644
index 0000000..3757d4c
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTranspose.h
@@ -0,0 +1,92 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSETRANSPOSE_H
+#define EIGEN_SPARSETRANSPOSE_H
+
+namespace Eigen { 
+
+namespace internal {
+  template<typename MatrixType,int CompressedAccess=int(MatrixType::Flags&CompressedAccessBit)>
+  class SparseTransposeImpl
+    : public SparseMatrixBase<Transpose<MatrixType> >
+  {};
+  
+  template<typename MatrixType>
+  class SparseTransposeImpl<MatrixType,CompressedAccessBit>
+    : public SparseCompressedBase<Transpose<MatrixType> >
+  {
+    typedef SparseCompressedBase<Transpose<MatrixType> > Base;
+  public:
+    using Base::derived;
+    typedef typename Base::Scalar Scalar;
+    typedef typename Base::StorageIndex StorageIndex;
+
+    inline Index nonZeros() const { return derived().nestedExpression().nonZeros(); }
+    
+    inline const Scalar* valuePtr() const { return derived().nestedExpression().valuePtr(); }
+    inline const StorageIndex* innerIndexPtr() const { return derived().nestedExpression().innerIndexPtr(); }
+    inline const StorageIndex* outerIndexPtr() const { return derived().nestedExpression().outerIndexPtr(); }
+    inline const StorageIndex* innerNonZeroPtr() const { return derived().nestedExpression().innerNonZeroPtr(); }
+
+    inline Scalar* valuePtr() { return derived().nestedExpression().valuePtr(); }
+    inline StorageIndex* innerIndexPtr() { return derived().nestedExpression().innerIndexPtr(); }
+    inline StorageIndex* outerIndexPtr() { return derived().nestedExpression().outerIndexPtr(); }
+    inline StorageIndex* innerNonZeroPtr() { return derived().nestedExpression().innerNonZeroPtr(); }
+  };
+}
+  
+template<typename MatrixType> class TransposeImpl<MatrixType,Sparse>
+  : public internal::SparseTransposeImpl<MatrixType>
+{
+  protected:
+    typedef internal::SparseTransposeImpl<MatrixType> Base;
+};
+
+namespace internal {
+  
+template<typename ArgType>
+struct unary_evaluator<Transpose<ArgType>, IteratorBased>
+  : public evaluator_base<Transpose<ArgType> >
+{
+    typedef typename evaluator<ArgType>::InnerIterator        EvalIterator;
+  public:
+    typedef Transpose<ArgType> XprType;
+    
+    inline Index nonZerosEstimate() const {
+      return m_argImpl.nonZerosEstimate();
+    }
+
+    class InnerIterator : public EvalIterator
+    {
+    public:
+      EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& unaryOp, Index outer)
+        : EvalIterator(unaryOp.m_argImpl,outer)
+      {}
+      
+      Index row() const { return EvalIterator::col(); }
+      Index col() const { return EvalIterator::row(); }
+    };
+    
+    enum {
+      CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
+      Flags = XprType::Flags
+    };
+    
+    explicit unary_evaluator(const XprType& op) :m_argImpl(op.nestedExpression()) {}
+
+  protected:
+    evaluator<ArgType> m_argImpl;
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSETRANSPOSE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTriangularView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTriangularView.h
new file mode 100644
index 0000000..9ac1202
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseTriangularView.h
@@ -0,0 +1,189 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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_SPARSE_TRIANGULARVIEW_H
+#define EIGEN_SPARSE_TRIANGULARVIEW_H
+
+namespace Eigen {
+
+/** \ingroup SparseCore_Module
+  *
+  * \brief Base class for a triangular part in a \b sparse matrix
+  *
+  * This class is an abstract base class of class TriangularView, and objects of type TriangularViewImpl cannot be instantiated.
+  * It extends class TriangularView with additional methods which are available for sparse expressions only.
+  *
+  * \sa class TriangularView, SparseMatrixBase::triangularView()
+  */
+template<typename MatrixType, unsigned int Mode> class TriangularViewImpl<MatrixType,Mode,Sparse>
+  : public SparseMatrixBase<TriangularView<MatrixType,Mode> >
+{
+    enum { SkipFirst = ((Mode&Lower) && !(MatrixType::Flags&RowMajorBit))
+                    || ((Mode&Upper) &&  (MatrixType::Flags&RowMajorBit)),
+           SkipLast = !SkipFirst,
+           SkipDiag = (Mode&ZeroDiag) ? 1 : 0,
+           HasUnitDiag = (Mode&UnitDiag) ? 1 : 0
+    };
+    
+    typedef TriangularView<MatrixType,Mode> TriangularViewType;
+    
+  protected:
+    // dummy solve function to make TriangularView happy.
+    void solve() const;
+
+    typedef SparseMatrixBase<TriangularViewType> Base;
+  public:
+    
+    EIGEN_SPARSE_PUBLIC_INTERFACE(TriangularViewType)
+    
+    typedef typename MatrixType::Nested MatrixTypeNested;
+    typedef typename internal::remove_reference<MatrixTypeNested>::type MatrixTypeNestedNonRef;
+    typedef typename internal::remove_all<MatrixTypeNested>::type MatrixTypeNestedCleaned;
+
+    template<typename RhsType, typename DstType>
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE void _solve_impl(const RhsType &rhs, DstType &dst) const {
+      if(!(internal::is_same<RhsType,DstType>::value && internal::extract_data(dst) == internal::extract_data(rhs)))
+        dst = rhs;
+      this->solveInPlace(dst);
+    }
+
+    /** Applies the inverse of \c *this to the dense vector or matrix \a other, "in-place" */
+    template<typename OtherDerived> void solveInPlace(MatrixBase<OtherDerived>& other) const;
+
+    /** Applies the inverse of \c *this to the sparse vector or matrix \a other, "in-place" */
+    template<typename OtherDerived> void solveInPlace(SparseMatrixBase<OtherDerived>& other) const;
+  
+};
+
+namespace internal {
+
+template<typename ArgType, unsigned int Mode>
+struct unary_evaluator<TriangularView<ArgType,Mode>, IteratorBased>
+ : evaluator_base<TriangularView<ArgType,Mode> >
+{
+  typedef TriangularView<ArgType,Mode> XprType;
+  
+protected:
+  
+  typedef typename XprType::Scalar Scalar;
+  typedef typename XprType::StorageIndex StorageIndex;
+  typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
+  
+  enum { SkipFirst = ((Mode&Lower) && !(ArgType::Flags&RowMajorBit))
+                    || ((Mode&Upper) &&  (ArgType::Flags&RowMajorBit)),
+         SkipLast = !SkipFirst,
+         SkipDiag = (Mode&ZeroDiag) ? 1 : 0,
+         HasUnitDiag = (Mode&UnitDiag) ? 1 : 0
+  };
+  
+public:
+  
+  enum {
+    CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
+    Flags = XprType::Flags
+  };
+    
+  explicit unary_evaluator(const XprType &xpr) : m_argImpl(xpr.nestedExpression()), m_arg(xpr.nestedExpression()) {}
+  
+  inline Index nonZerosEstimate() const {
+    return m_argImpl.nonZerosEstimate();
+  }
+  
+  class InnerIterator : public EvalIterator
+  {
+      typedef EvalIterator Base;
+    public:
+
+      EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& xprEval, Index outer)
+        : Base(xprEval.m_argImpl,outer), m_returnOne(false), m_containsDiag(Base::outer()<xprEval.m_arg.innerSize())
+      {
+        if(SkipFirst)
+        {
+          while((*this) && ((HasUnitDiag||SkipDiag)  ? this->index()<=outer : this->index()<outer))
+            Base::operator++();
+          if(HasUnitDiag)
+            m_returnOne = m_containsDiag;
+        }
+        else if(HasUnitDiag && ((!Base::operator bool()) || Base::index()>=Base::outer()))
+        {
+          if((!SkipFirst) && Base::operator bool())
+            Base::operator++();
+          m_returnOne = m_containsDiag;
+        }
+      }
+
+      EIGEN_STRONG_INLINE InnerIterator& operator++()
+      {
+        if(HasUnitDiag && m_returnOne)
+          m_returnOne = false;
+        else
+        {
+          Base::operator++();
+          if(HasUnitDiag && (!SkipFirst) && ((!Base::operator bool()) || Base::index()>=Base::outer()))
+          {
+            if((!SkipFirst) && Base::operator bool())
+              Base::operator++();
+            m_returnOne = m_containsDiag;
+          }
+        }
+        return *this;
+      }
+      
+      EIGEN_STRONG_INLINE operator bool() const
+      {
+        if(HasUnitDiag && m_returnOne)
+          return true;
+        if(SkipFirst) return  Base::operator bool();
+        else
+        {
+          if (SkipDiag) return (Base::operator bool() && this->index() < this->outer());
+          else return (Base::operator bool() && this->index() <= this->outer());
+        }
+      }
+
+//       inline Index row() const { return (ArgType::Flags&RowMajorBit ? Base::outer() : this->index()); }
+//       inline Index col() const { return (ArgType::Flags&RowMajorBit ? this->index() : Base::outer()); }
+      inline StorageIndex index() const
+      {
+        if(HasUnitDiag && m_returnOne)  return internal::convert_index<StorageIndex>(Base::outer());
+        else                            return Base::index();
+      }
+      inline Scalar value() const
+      {
+        if(HasUnitDiag && m_returnOne)  return Scalar(1);
+        else                            return Base::value();
+      }
+
+    protected:
+      bool m_returnOne;
+      bool m_containsDiag;
+    private:
+      Scalar& valueRef();
+  };
+  
+protected:
+  evaluator<ArgType> m_argImpl;
+  const ArgType& m_arg;
+};
+
+} // end namespace internal
+
+template<typename Derived>
+template<int Mode>
+inline const TriangularView<const Derived, Mode>
+SparseMatrixBase<Derived>::triangularView() const
+{
+  return TriangularView<const Derived, Mode>(derived());
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSE_TRIANGULARVIEW_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseUtil.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseUtil.h
new file mode 100644
index 0000000..ceb9368
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseUtil.h
@@ -0,0 +1,186 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSEUTIL_H
+#define EIGEN_SPARSEUTIL_H
+
+namespace Eigen { 
+
+#ifdef NDEBUG
+#define EIGEN_DBG_SPARSE(X)
+#else
+#define EIGEN_DBG_SPARSE(X) X
+#endif
+
+#define EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(Derived, Op) \
+template<typename OtherDerived> \
+EIGEN_STRONG_INLINE Derived& operator Op(const Eigen::SparseMatrixBase<OtherDerived>& other) \
+{ \
+  return Base::operator Op(other.derived()); \
+} \
+EIGEN_STRONG_INLINE Derived& operator Op(const Derived& other) \
+{ \
+  return Base::operator Op(other); \
+}
+
+#define EIGEN_SPARSE_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, Op) \
+template<typename Other> \
+EIGEN_STRONG_INLINE Derived& operator Op(const Other& scalar) \
+{ \
+  return Base::operator Op(scalar); \
+}
+
+#define EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATORS(Derived) \
+EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(Derived, =)
+
+
+#define EIGEN_SPARSE_PUBLIC_INTERFACE(Derived) \
+  EIGEN_GENERIC_PUBLIC_INTERFACE(Derived)
+
+  
+const int CoherentAccessPattern     = 0x1;
+const int InnerRandomAccessPattern  = 0x2 | CoherentAccessPattern;
+const int OuterRandomAccessPattern  = 0x4 | CoherentAccessPattern;
+const int RandomAccessPattern       = 0x8 | OuterRandomAccessPattern | InnerRandomAccessPattern;
+
+template<typename _Scalar, int _Flags = 0, typename _StorageIndex = int>  class SparseMatrix;
+template<typename _Scalar, int _Flags = 0, typename _StorageIndex = int>  class DynamicSparseMatrix;
+template<typename _Scalar, int _Flags = 0, typename _StorageIndex = int>  class SparseVector;
+template<typename _Scalar, int _Flags = 0, typename _StorageIndex = int>  class MappedSparseMatrix;
+
+template<typename MatrixType, unsigned int UpLo>  class SparseSelfAdjointView;
+template<typename Lhs, typename Rhs>              class SparseDiagonalProduct;
+template<typename MatrixType> class SparseView;
+
+template<typename Lhs, typename Rhs>        class SparseSparseProduct;
+template<typename Lhs, typename Rhs>        class SparseTimeDenseProduct;
+template<typename Lhs, typename Rhs>        class DenseTimeSparseProduct;
+template<typename Lhs, typename Rhs, bool Transpose> class SparseDenseOuterProduct;
+
+template<typename Lhs, typename Rhs> struct SparseSparseProductReturnType;
+template<typename Lhs, typename Rhs,
+         int InnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(internal::traits<Lhs>::ColsAtCompileTime,internal::traits<Rhs>::RowsAtCompileTime)> struct DenseSparseProductReturnType;
+         
+template<typename Lhs, typename Rhs,
+         int InnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(internal::traits<Lhs>::ColsAtCompileTime,internal::traits<Rhs>::RowsAtCompileTime)> struct SparseDenseProductReturnType;
+template<typename MatrixType,int UpLo> class SparseSymmetricPermutationProduct;
+
+namespace internal {
+
+template<typename T,int Rows,int Cols,int Flags> struct sparse_eval;
+
+template<typename T> struct eval<T,Sparse>
+  : sparse_eval<T, traits<T>::RowsAtCompileTime,traits<T>::ColsAtCompileTime,traits<T>::Flags>
+{};
+
+template<typename T,int Cols,int Flags> struct sparse_eval<T,1,Cols,Flags> {
+    typedef typename traits<T>::Scalar _Scalar;
+    typedef typename traits<T>::StorageIndex _StorageIndex;
+  public:
+    typedef SparseVector<_Scalar, RowMajor, _StorageIndex> type;
+};
+
+template<typename T,int Rows,int Flags> struct sparse_eval<T,Rows,1,Flags> {
+    typedef typename traits<T>::Scalar _Scalar;
+    typedef typename traits<T>::StorageIndex _StorageIndex;
+  public:
+    typedef SparseVector<_Scalar, ColMajor, _StorageIndex> type;
+};
+
+// TODO this seems almost identical to plain_matrix_type<T, Sparse>
+template<typename T,int Rows,int Cols,int Flags> struct sparse_eval {
+    typedef typename traits<T>::Scalar _Scalar;
+    typedef typename traits<T>::StorageIndex _StorageIndex;
+    enum { _Options = ((Flags&RowMajorBit)==RowMajorBit) ? RowMajor : ColMajor };
+  public:
+    typedef SparseMatrix<_Scalar, _Options, _StorageIndex> type;
+};
+
+template<typename T,int Flags> struct sparse_eval<T,1,1,Flags> {
+    typedef typename traits<T>::Scalar _Scalar;
+  public:
+    typedef Matrix<_Scalar, 1, 1> type;
+};
+
+template<typename T> struct plain_matrix_type<T,Sparse>
+{
+  typedef typename traits<T>::Scalar _Scalar;
+  typedef typename traits<T>::StorageIndex _StorageIndex;
+  enum { _Options = ((evaluator<T>::Flags&RowMajorBit)==RowMajorBit) ? RowMajor : ColMajor };
+  public:
+    typedef SparseMatrix<_Scalar, _Options, _StorageIndex> type;
+};
+
+template<typename T>
+struct plain_object_eval<T,Sparse>
+  : sparse_eval<T, traits<T>::RowsAtCompileTime,traits<T>::ColsAtCompileTime, evaluator<T>::Flags>
+{};
+
+template<typename Decomposition, typename RhsType>
+struct solve_traits<Decomposition,RhsType,Sparse>
+{
+  typedef typename sparse_eval<RhsType, RhsType::RowsAtCompileTime, RhsType::ColsAtCompileTime,traits<RhsType>::Flags>::type PlainObject;
+};
+
+template<typename Derived>
+struct generic_xpr_base<Derived, MatrixXpr, Sparse>
+{
+  typedef SparseMatrixBase<Derived> type;
+};
+
+struct SparseTriangularShape  { static std::string debugName() { return "SparseTriangularShape"; } };
+struct SparseSelfAdjointShape { static std::string debugName() { return "SparseSelfAdjointShape"; } };
+
+template<> struct glue_shapes<SparseShape,SelfAdjointShape> { typedef SparseSelfAdjointShape type;  };
+template<> struct glue_shapes<SparseShape,TriangularShape > { typedef SparseTriangularShape  type;  };
+
+// return type of SparseCompressedBase::lower_bound;
+struct LowerBoundIndex {
+  LowerBoundIndex() : value(-1), found(false) {}
+  LowerBoundIndex(Index val, bool ok) : value(val), found(ok) {}
+  Index value;
+  bool found;
+};
+
+} // end namespace internal
+
+/** \ingroup SparseCore_Module
+  *
+  * \class Triplet
+  *
+  * \brief A small structure to hold a non zero as a triplet (i,j,value).
+  *
+  * \sa SparseMatrix::setFromTriplets()
+  */
+template<typename Scalar, typename StorageIndex=typename SparseMatrix<Scalar>::StorageIndex >
+class Triplet
+{
+public:
+  Triplet() : m_row(0), m_col(0), m_value(0) {}
+
+  Triplet(const StorageIndex& i, const StorageIndex& j, const Scalar& v = Scalar(0))
+    : m_row(i), m_col(j), m_value(v)
+  {}
+
+  /** \returns the row index of the element */
+  const StorageIndex& row() const { return m_row; }
+
+  /** \returns the column index of the element */
+  const StorageIndex& col() const { return m_col; }
+
+  /** \returns the value of the element */
+  const Scalar& value() const { return m_value; }
+protected:
+  StorageIndex m_row, m_col;
+  Scalar m_value;
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSEUTIL_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h
new file mode 100644
index 0000000..05779be
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h
@@ -0,0 +1,478 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSEVECTOR_H
+#define EIGEN_SPARSEVECTOR_H
+
+namespace Eigen { 
+
+/** \ingroup SparseCore_Module
+  * \class SparseVector
+  *
+  * \brief a sparse vector class
+  *
+  * \tparam _Scalar the scalar type, i.e. the type of the coefficients
+  *
+  * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme.
+  *
+  * This class can be extended with the help of the plugin mechanism described on the page
+  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_SPARSEVECTOR_PLUGIN.
+  */
+
+namespace internal {
+template<typename _Scalar, int _Options, typename _StorageIndex>
+struct traits<SparseVector<_Scalar, _Options, _StorageIndex> >
+{
+  typedef _Scalar Scalar;
+  typedef _StorageIndex StorageIndex;
+  typedef Sparse StorageKind;
+  typedef MatrixXpr XprKind;
+  enum {
+    IsColVector = (_Options & RowMajorBit) ? 0 : 1,
+
+    RowsAtCompileTime = IsColVector ? Dynamic : 1,
+    ColsAtCompileTime = IsColVector ? 1 : Dynamic,
+    MaxRowsAtCompileTime = RowsAtCompileTime,
+    MaxColsAtCompileTime = ColsAtCompileTime,
+    Flags = _Options | NestByRefBit | LvalueBit | (IsColVector ? 0 : RowMajorBit) | CompressedAccessBit,
+    SupportedAccessPatterns = InnerRandomAccessPattern
+  };
+};
+
+// Sparse-Vector-Assignment kinds:
+enum {
+  SVA_RuntimeSwitch,
+  SVA_Inner,
+  SVA_Outer
+};
+
+template< typename Dest, typename Src,
+          int AssignmentKind = !bool(Src::IsVectorAtCompileTime) ? SVA_RuntimeSwitch
+                             : Src::InnerSizeAtCompileTime==1 ? SVA_Outer
+                             : SVA_Inner>
+struct sparse_vector_assign_selector;
+
+}
+
+template<typename _Scalar, int _Options, typename _StorageIndex>
+class SparseVector
+  : public SparseCompressedBase<SparseVector<_Scalar, _Options, _StorageIndex> >
+{
+    typedef SparseCompressedBase<SparseVector> Base;
+    using Base::convert_index;
+  public:
+    EIGEN_SPARSE_PUBLIC_INTERFACE(SparseVector)
+    EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseVector, +=)
+    EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseVector, -=)
+    
+    typedef internal::CompressedStorage<Scalar,StorageIndex> Storage;
+    enum { IsColVector = internal::traits<SparseVector>::IsColVector };
+    
+    enum {
+      Options = _Options
+    };
+    
+    EIGEN_STRONG_INLINE Index rows() const { return IsColVector ? m_size : 1; }
+    EIGEN_STRONG_INLINE Index cols() const { return IsColVector ? 1 : m_size; }
+    EIGEN_STRONG_INLINE Index innerSize() const { return m_size; }
+    EIGEN_STRONG_INLINE Index outerSize() const { return 1; }
+
+    EIGEN_STRONG_INLINE const Scalar* valuePtr() const { return m_data.valuePtr(); }
+    EIGEN_STRONG_INLINE Scalar* valuePtr() { return m_data.valuePtr(); }
+
+    EIGEN_STRONG_INLINE const StorageIndex* innerIndexPtr() const { return m_data.indexPtr(); }
+    EIGEN_STRONG_INLINE StorageIndex* innerIndexPtr() { return m_data.indexPtr(); }
+
+    inline const StorageIndex* outerIndexPtr() const { return 0; }
+    inline StorageIndex* outerIndexPtr() { return 0; }
+    inline const StorageIndex* innerNonZeroPtr() const { return 0; }
+    inline StorageIndex* innerNonZeroPtr() { return 0; }
+    
+    /** \internal */
+    inline Storage& data() { return m_data; }
+    /** \internal */
+    inline const Storage& data() const { return m_data; }
+
+    inline Scalar coeff(Index row, Index col) const
+    {
+      eigen_assert(IsColVector ? (col==0 && row>=0 && row<m_size) : (row==0 && col>=0 && col<m_size));
+      return coeff(IsColVector ? row : col);
+    }
+    inline Scalar coeff(Index i) const
+    {
+      eigen_assert(i>=0 && i<m_size);
+      return m_data.at(StorageIndex(i));
+    }
+
+    inline Scalar& coeffRef(Index row, Index col)
+    {
+      eigen_assert(IsColVector ? (col==0 && row>=0 && row<m_size) : (row==0 && col>=0 && col<m_size));
+      return coeffRef(IsColVector ? row : col);
+    }
+
+    /** \returns a reference to the coefficient value at given index \a i
+      * This operation involes a log(rho*size) binary search. If the coefficient does not
+      * exist yet, then a sorted insertion into a sequential buffer is performed.
+      *
+      * This insertion might be very costly if the number of nonzeros above \a i is large.
+      */
+    inline Scalar& coeffRef(Index i)
+    {
+      eigen_assert(i>=0 && i<m_size);
+
+      return m_data.atWithInsertion(StorageIndex(i));
+    }
+
+  public:
+
+    typedef typename Base::InnerIterator InnerIterator;
+    typedef typename Base::ReverseInnerIterator ReverseInnerIterator;
+
+    inline void setZero() { m_data.clear(); }
+
+    /** \returns the number of non zero coefficients */
+    inline Index nonZeros() const  { return m_data.size(); }
+
+    inline void startVec(Index outer)
+    {
+      EIGEN_UNUSED_VARIABLE(outer);
+      eigen_assert(outer==0);
+    }
+
+    inline Scalar& insertBackByOuterInner(Index outer, Index inner)
+    {
+      EIGEN_UNUSED_VARIABLE(outer);
+      eigen_assert(outer==0);
+      return insertBack(inner);
+    }
+    inline Scalar& insertBack(Index i)
+    {
+      m_data.append(0, i);
+      return m_data.value(m_data.size()-1);
+    }
+    
+    Scalar& insertBackByOuterInnerUnordered(Index outer, Index inner)
+    {
+      EIGEN_UNUSED_VARIABLE(outer);
+      eigen_assert(outer==0);
+      return insertBackUnordered(inner);
+    }
+    inline Scalar& insertBackUnordered(Index i)
+    {
+      m_data.append(0, i);
+      return m_data.value(m_data.size()-1);
+    }
+
+    inline Scalar& insert(Index row, Index col)
+    {
+      eigen_assert(IsColVector ? (col==0 && row>=0 && row<m_size) : (row==0 && col>=0 && col<m_size));
+      
+      Index inner = IsColVector ? row : col;
+      Index outer = IsColVector ? col : row;
+      EIGEN_ONLY_USED_FOR_DEBUG(outer);
+      eigen_assert(outer==0);
+      return insert(inner);
+    }
+    Scalar& insert(Index i)
+    {
+      eigen_assert(i>=0 && i<m_size);
+      
+      Index startId = 0;
+      Index p = Index(m_data.size()) - 1;
+      // TODO smart realloc
+      m_data.resize(p+2,1);
+
+      while ( (p >= startId) && (m_data.index(p) > i) )
+      {
+        m_data.index(p+1) = m_data.index(p);
+        m_data.value(p+1) = m_data.value(p);
+        --p;
+      }
+      m_data.index(p+1) = convert_index(i);
+      m_data.value(p+1) = 0;
+      return m_data.value(p+1);
+    }
+
+    /**
+      */
+    inline void reserve(Index reserveSize) { m_data.reserve(reserveSize); }
+
+
+    inline void finalize() {}
+
+    /** \copydoc SparseMatrix::prune(const Scalar&,const RealScalar&) */
+    void prune(const Scalar& reference, const RealScalar& epsilon = NumTraits<RealScalar>::dummy_precision())
+    {
+      m_data.prune(reference,epsilon);
+    }
+
+    /** Resizes the sparse vector to \a rows x \a cols
+      *
+      * This method is provided for compatibility with matrices.
+      * For a column vector, \a cols must be equal to 1.
+      * For a row vector, \a rows must be equal to 1.
+      *
+      * \sa resize(Index)
+      */
+    void resize(Index rows, Index cols)
+    {
+      eigen_assert((IsColVector ? cols : rows)==1 && "Outer dimension must equal 1");
+      resize(IsColVector ? rows : cols);
+    }
+
+    /** Resizes the sparse vector to \a newSize
+      * This method deletes all entries, thus leaving an empty sparse vector
+      *
+      * \sa  conservativeResize(), setZero() */
+    void resize(Index newSize)
+    {
+      m_size = newSize;
+      m_data.clear();
+    }
+
+    /** Resizes the sparse vector to \a newSize, while leaving old values untouched.
+      *
+      * If the size of the vector is decreased, then the storage of the out-of bounds coefficients is kept and reserved.
+      * Call .data().squeeze() to free extra memory.
+      *
+      * \sa reserve(), setZero()
+      */
+    void conservativeResize(Index newSize)
+    {
+      if (newSize < m_size)
+      {
+        Index i = 0;
+        while (i<m_data.size() && m_data.index(i)<newSize) ++i;
+        m_data.resize(i);
+      }
+      m_size = newSize;
+    }
+
+    void resizeNonZeros(Index size) { m_data.resize(size); }
+
+    inline SparseVector() : m_size(0) { check_template_parameters(); resize(0); }
+
+    explicit inline SparseVector(Index size) : m_size(0) { check_template_parameters(); resize(size); }
+
+    inline SparseVector(Index rows, Index cols) : m_size(0) { check_template_parameters(); resize(rows,cols); }
+
+    template<typename OtherDerived>
+    inline SparseVector(const SparseMatrixBase<OtherDerived>& other)
+      : m_size(0)
+    {
+      #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+        EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+      #endif
+      check_template_parameters();
+      *this = other.derived();
+    }
+
+    inline SparseVector(const SparseVector& other)
+      : Base(other), m_size(0)
+    {
+      check_template_parameters();
+      *this = other.derived();
+    }
+
+    /** Swaps the values of \c *this and \a other.
+      * Overloaded for performance: this version performs a \em shallow swap by swapping pointers and attributes only.
+      * \sa SparseMatrixBase::swap()
+      */
+    inline void swap(SparseVector& other)
+    {
+      std::swap(m_size, other.m_size);
+      m_data.swap(other.m_data);
+    }
+
+    template<int OtherOptions>
+    inline void swap(SparseMatrix<Scalar,OtherOptions,StorageIndex>& other)
+    {
+      eigen_assert(other.outerSize()==1);
+      std::swap(m_size, other.m_innerSize);
+      m_data.swap(other.m_data);
+    }
+
+    inline SparseVector& operator=(const SparseVector& other)
+    {
+      if (other.isRValue())
+      {
+        swap(other.const_cast_derived());
+      }
+      else
+      {
+        resize(other.size());
+        m_data = other.m_data;
+      }
+      return *this;
+    }
+
+    template<typename OtherDerived>
+    inline SparseVector& operator=(const SparseMatrixBase<OtherDerived>& other)
+    {
+      SparseVector tmp(other.size());
+      internal::sparse_vector_assign_selector<SparseVector,OtherDerived>::run(tmp,other.derived());
+      this->swap(tmp);
+      return *this;
+    }
+
+    #ifndef EIGEN_PARSED_BY_DOXYGEN
+    template<typename Lhs, typename Rhs>
+    inline SparseVector& operator=(const SparseSparseProduct<Lhs,Rhs>& product)
+    {
+      return Base::operator=(product);
+    }
+    #endif
+
+    friend std::ostream & operator << (std::ostream & s, const SparseVector& m)
+    {
+      for (Index i=0; i<m.nonZeros(); ++i)
+        s << "(" << m.m_data.value(i) << "," << m.m_data.index(i) << ") ";
+      s << std::endl;
+      return s;
+    }
+
+    /** Destructor */
+    inline ~SparseVector() {}
+
+    /** Overloaded for performance */
+    Scalar sum() const;
+
+  public:
+
+    /** \internal \deprecated use setZero() and reserve() */
+    EIGEN_DEPRECATED void startFill(Index reserve)
+    {
+      setZero();
+      m_data.reserve(reserve);
+    }
+
+    /** \internal \deprecated use insertBack(Index,Index) */
+    EIGEN_DEPRECATED Scalar& fill(Index r, Index c)
+    {
+      eigen_assert(r==0 || c==0);
+      return fill(IsColVector ? r : c);
+    }
+
+    /** \internal \deprecated use insertBack(Index) */
+    EIGEN_DEPRECATED Scalar& fill(Index i)
+    {
+      m_data.append(0, i);
+      return m_data.value(m_data.size()-1);
+    }
+
+    /** \internal \deprecated use insert(Index,Index) */
+    EIGEN_DEPRECATED Scalar& fillrand(Index r, Index c)
+    {
+      eigen_assert(r==0 || c==0);
+      return fillrand(IsColVector ? r : c);
+    }
+
+    /** \internal \deprecated use insert(Index) */
+    EIGEN_DEPRECATED Scalar& fillrand(Index i)
+    {
+      return insert(i);
+    }
+
+    /** \internal \deprecated use finalize() */
+    EIGEN_DEPRECATED void endFill() {}
+    
+    // These two functions were here in the 3.1 release, so let's keep them in case some code rely on them.
+    /** \internal \deprecated use data() */
+    EIGEN_DEPRECATED Storage& _data() { return m_data; }
+    /** \internal \deprecated use data() */
+    EIGEN_DEPRECATED const Storage& _data() const { return m_data; }
+    
+#   ifdef EIGEN_SPARSEVECTOR_PLUGIN
+#     include EIGEN_SPARSEVECTOR_PLUGIN
+#   endif
+
+protected:
+  
+    static void check_template_parameters()
+    {
+      EIGEN_STATIC_ASSERT(NumTraits<StorageIndex>::IsSigned,THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE);
+      EIGEN_STATIC_ASSERT((_Options&(ColMajor|RowMajor))==Options,INVALID_MATRIX_TEMPLATE_PARAMETERS);
+    }
+    
+    Storage m_data;
+    Index m_size;
+};
+
+namespace internal {
+
+template<typename _Scalar, int _Options, typename _Index>
+struct evaluator<SparseVector<_Scalar,_Options,_Index> >
+  : evaluator_base<SparseVector<_Scalar,_Options,_Index> >
+{
+  typedef SparseVector<_Scalar,_Options,_Index> SparseVectorType;
+  typedef evaluator_base<SparseVectorType> Base;
+  typedef typename SparseVectorType::InnerIterator InnerIterator;
+  typedef typename SparseVectorType::ReverseInnerIterator ReverseInnerIterator;
+  
+  enum {
+    CoeffReadCost = NumTraits<_Scalar>::ReadCost,
+    Flags = SparseVectorType::Flags
+  };
+
+  evaluator() : Base() {}
+  
+  explicit evaluator(const SparseVectorType &mat) : m_matrix(&mat)
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+  
+  inline Index nonZerosEstimate() const {
+    return m_matrix->nonZeros();
+  }
+  
+  operator SparseVectorType&() { return m_matrix->const_cast_derived(); }
+  operator const SparseVectorType&() const { return *m_matrix; }
+  
+  const SparseVectorType *m_matrix;
+};
+
+template< typename Dest, typename Src>
+struct sparse_vector_assign_selector<Dest,Src,SVA_Inner> {
+  static void run(Dest& dst, const Src& src) {
+    eigen_internal_assert(src.innerSize()==src.size());
+    typedef internal::evaluator<Src> SrcEvaluatorType;
+    SrcEvaluatorType srcEval(src);
+    for(typename SrcEvaluatorType::InnerIterator it(srcEval, 0); it; ++it)
+      dst.insert(it.index()) = it.value();
+  }
+};
+
+template< typename Dest, typename Src>
+struct sparse_vector_assign_selector<Dest,Src,SVA_Outer> {
+  static void run(Dest& dst, const Src& src) {
+    eigen_internal_assert(src.outerSize()==src.size());
+    typedef internal::evaluator<Src> SrcEvaluatorType;
+    SrcEvaluatorType srcEval(src);
+    for(Index i=0; i<src.size(); ++i)
+    {
+      typename SrcEvaluatorType::InnerIterator it(srcEval, i);
+      if(it)
+        dst.insert(i) = it.value();
+    }
+  }
+};
+
+template< typename Dest, typename Src>
+struct sparse_vector_assign_selector<Dest,Src,SVA_RuntimeSwitch> {
+  static void run(Dest& dst, const Src& src) {
+    if(src.outerSize()==1)  sparse_vector_assign_selector<Dest,Src,SVA_Inner>::run(dst, src);
+    else                    sparse_vector_assign_selector<Dest,Src,SVA_Outer>::run(dst, src);
+  }
+};
+
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSEVECTOR_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseView.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseView.h
new file mode 100644
index 0000000..92b3d1f
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseView.h
@@ -0,0 +1,254 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2011-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2010 Daniel Lowengrub <lowdanie@gmail.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_SPARSEVIEW_H
+#define EIGEN_SPARSEVIEW_H
+
+namespace Eigen { 
+
+namespace internal {
+
+template<typename MatrixType>
+struct traits<SparseView<MatrixType> > : traits<MatrixType>
+{
+  typedef typename MatrixType::StorageIndex StorageIndex;
+  typedef Sparse StorageKind;
+  enum {
+    Flags = int(traits<MatrixType>::Flags) & (RowMajorBit)
+  };
+};
+
+} // end namespace internal
+
+/** \ingroup SparseCore_Module
+  * \class SparseView
+  *
+  * \brief Expression of a dense or sparse matrix with zero or too small values removed
+  *
+  * \tparam MatrixType the type of the object of which we are removing the small entries
+  *
+  * This class represents an expression of a given dense or sparse matrix with
+  * entries smaller than \c reference * \c epsilon are removed.
+  * It is the return type of MatrixBase::sparseView() and SparseMatrixBase::pruned()
+  * and most of the time this is the only way it is used.
+  *
+  * \sa MatrixBase::sparseView(), SparseMatrixBase::pruned()
+  */
+template<typename MatrixType>
+class SparseView : public SparseMatrixBase<SparseView<MatrixType> >
+{
+  typedef typename MatrixType::Nested MatrixTypeNested;
+  typedef typename internal::remove_all<MatrixTypeNested>::type _MatrixTypeNested;
+  typedef SparseMatrixBase<SparseView > Base;
+public:
+  EIGEN_SPARSE_PUBLIC_INTERFACE(SparseView)
+  typedef typename internal::remove_all<MatrixType>::type NestedExpression;
+
+  explicit SparseView(const MatrixType& mat, const Scalar& reference = Scalar(0),
+                      const RealScalar &epsilon = NumTraits<Scalar>::dummy_precision())
+    : m_matrix(mat), m_reference(reference), m_epsilon(epsilon) {}
+
+  inline Index rows() const { return m_matrix.rows(); }
+  inline Index cols() const { return m_matrix.cols(); }
+
+  inline Index innerSize() const { return m_matrix.innerSize(); }
+  inline Index outerSize() const { return m_matrix.outerSize(); }
+  
+  /** \returns the nested expression */
+  const typename internal::remove_all<MatrixTypeNested>::type&
+  nestedExpression() const { return m_matrix; }
+  
+  Scalar reference() const { return m_reference; }
+  RealScalar epsilon() const { return m_epsilon; }
+  
+protected:
+  MatrixTypeNested m_matrix;
+  Scalar m_reference;
+  RealScalar m_epsilon;
+};
+
+namespace internal {
+
+// TODO find a way to unify the two following variants
+// This is tricky because implementing an inner iterator on top of an IndexBased evaluator is
+// not easy because the evaluators do not expose the sizes of the underlying expression.
+  
+template<typename ArgType>
+struct unary_evaluator<SparseView<ArgType>, IteratorBased>
+  : public evaluator_base<SparseView<ArgType> >
+{
+    typedef typename evaluator<ArgType>::InnerIterator EvalIterator;
+  public:
+    typedef SparseView<ArgType> XprType;
+    
+    class InnerIterator : public EvalIterator
+    {
+      protected:
+        typedef typename XprType::Scalar Scalar;
+      public:
+
+        EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& sve, Index outer)
+          : EvalIterator(sve.m_argImpl,outer), m_view(sve.m_view)
+        {
+          incrementToNonZero();
+        }
+
+        EIGEN_STRONG_INLINE InnerIterator& operator++()
+        {
+          EvalIterator::operator++();
+          incrementToNonZero();
+          return *this;
+        }
+
+        using EvalIterator::value;
+
+      protected:
+        const XprType &m_view;
+
+      private:
+        void incrementToNonZero()
+        {
+          while((bool(*this)) && internal::isMuchSmallerThan(value(), m_view.reference(), m_view.epsilon()))
+          {
+            EvalIterator::operator++();
+          }
+        }
+    };
+    
+    enum {
+      CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
+      Flags = XprType::Flags
+    };
+    
+    explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_view(xpr) {}
+
+  protected:
+    evaluator<ArgType> m_argImpl;
+    const XprType &m_view;
+};
+
+template<typename ArgType>
+struct unary_evaluator<SparseView<ArgType>, IndexBased>
+  : public evaluator_base<SparseView<ArgType> >
+{
+  public:
+    typedef SparseView<ArgType> XprType;
+  protected:
+    enum { IsRowMajor = (XprType::Flags&RowMajorBit)==RowMajorBit };
+    typedef typename XprType::Scalar Scalar;
+    typedef typename XprType::StorageIndex StorageIndex;
+  public:
+    
+    class InnerIterator
+    {
+      public:
+
+        EIGEN_STRONG_INLINE InnerIterator(const unary_evaluator& sve, Index outer)
+          : m_sve(sve), m_inner(0), m_outer(outer), m_end(sve.m_view.innerSize())
+        {
+          incrementToNonZero();
+        }
+
+        EIGEN_STRONG_INLINE InnerIterator& operator++()
+        {
+          m_inner++;
+          incrementToNonZero();
+          return *this;
+        }
+
+        EIGEN_STRONG_INLINE Scalar value() const
+        {
+          return (IsRowMajor) ? m_sve.m_argImpl.coeff(m_outer, m_inner)
+                              : m_sve.m_argImpl.coeff(m_inner, m_outer);
+        }
+
+        EIGEN_STRONG_INLINE StorageIndex index() const { return m_inner; }
+        inline Index row() const { return IsRowMajor ? m_outer : index(); }
+        inline Index col() const { return IsRowMajor ? index() : m_outer; }
+
+        EIGEN_STRONG_INLINE operator bool() const { return m_inner < m_end && m_inner>=0; }
+
+      protected:
+        const unary_evaluator &m_sve;
+        Index m_inner;
+        const Index m_outer;
+        const Index m_end;
+
+      private:
+        void incrementToNonZero()
+        {
+          while((bool(*this)) && internal::isMuchSmallerThan(value(), m_sve.m_view.reference(), m_sve.m_view.epsilon()))
+          {
+            m_inner++;
+          }
+        }
+    };
+    
+    enum {
+      CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
+      Flags = XprType::Flags
+    };
+    
+    explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_view(xpr) {}
+
+  protected:
+    evaluator<ArgType> m_argImpl;
+    const XprType &m_view;
+};
+
+} // end namespace internal
+
+/** \ingroup SparseCore_Module
+  *
+  * \returns a sparse expression of the dense expression \c *this with values smaller than
+  * \a reference * \a epsilon removed.
+  *
+  * This method is typically used when prototyping to convert a quickly assembled dense Matrix \c D to a SparseMatrix \c S:
+  * \code
+  * MatrixXd D(n,m);
+  * SparseMatrix<double> S;
+  * S = D.sparseView();             // suppress numerical zeros (exact)
+  * S = D.sparseView(reference);
+  * S = D.sparseView(reference,epsilon);
+  * \endcode
+  * where \a reference is a meaningful non zero reference value,
+  * and \a epsilon is a tolerance factor defaulting to NumTraits<Scalar>::dummy_precision().
+  *
+  * \sa SparseMatrixBase::pruned(), class SparseView */
+template<typename Derived>
+const SparseView<Derived> MatrixBase<Derived>::sparseView(const Scalar& reference,
+                                                          const typename NumTraits<Scalar>::Real& epsilon) const
+{
+  return SparseView<Derived>(derived(), reference, epsilon);
+}
+
+/** \returns an expression of \c *this with values smaller than
+  * \a reference * \a epsilon removed.
+  *
+  * This method is typically used in conjunction with the product of two sparse matrices
+  * to automatically prune the smallest values as follows:
+  * \code
+  * C = (A*B).pruned();             // suppress numerical zeros (exact)
+  * C = (A*B).pruned(ref);
+  * C = (A*B).pruned(ref,epsilon);
+  * \endcode
+  * where \c ref is a meaningful non zero reference value.
+  * */
+template<typename Derived>
+const SparseView<Derived>
+SparseMatrixBase<Derived>::pruned(const Scalar& reference,
+                                  const RealScalar& epsilon) const
+{
+  return SparseView<Derived>(derived(), reference, epsilon);
+}
+
+} // end namespace Eigen
+
+#endif
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
new file mode 100644
index 0000000..f9c56ba
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/TriangularSolver.h
@@ -0,0 +1,315 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSETRIANGULARSOLVER_H
+#define EIGEN_SPARSETRIANGULARSOLVER_H
+
+namespace Eigen { 
+
+namespace internal {
+
+template<typename Lhs, typename Rhs, int Mode,
+  int UpLo = (Mode & Lower)
+           ? Lower
+           : (Mode & Upper)
+           ? Upper
+           : -1,
+  int StorageOrder = int(traits<Lhs>::Flags) & RowMajorBit>
+struct sparse_solve_triangular_selector;
+
+// forward substitution, row-major
+template<typename Lhs, typename Rhs, int Mode>
+struct sparse_solve_triangular_selector<Lhs,Rhs,Mode,Lower,RowMajor>
+{
+  typedef typename Rhs::Scalar Scalar;
+  typedef evaluator<Lhs> LhsEval;
+  typedef typename evaluator<Lhs>::InnerIterator LhsIterator;
+  static void run(const Lhs& lhs, Rhs& other)
+  {
+    LhsEval lhsEval(lhs);
+    for(Index col=0 ; col<other.cols() ; ++col)
+    {
+      for(Index i=0; i<lhs.rows(); ++i)
+      {
+        Scalar tmp = other.coeff(i,col);
+        Scalar lastVal(0);
+        Index lastIndex = 0;
+        for(LhsIterator it(lhsEval, i); it; ++it)
+        {
+          lastVal = it.value();
+          lastIndex = it.index();
+          if(lastIndex==i)
+            break;
+          tmp -= lastVal * other.coeff(lastIndex,col);
+        }
+        if (Mode & UnitDiag)
+          other.coeffRef(i,col) = tmp;
+        else
+        {
+          eigen_assert(lastIndex==i);
+          other.coeffRef(i,col) = tmp/lastVal;
+        }
+      }
+    }
+  }
+};
+
+// backward substitution, row-major
+template<typename Lhs, typename Rhs, int Mode>
+struct sparse_solve_triangular_selector<Lhs,Rhs,Mode,Upper,RowMajor>
+{
+  typedef typename Rhs::Scalar Scalar;
+  typedef evaluator<Lhs> LhsEval;
+  typedef typename evaluator<Lhs>::InnerIterator LhsIterator;
+  static void run(const Lhs& lhs, Rhs& other)
+  {
+    LhsEval lhsEval(lhs);
+    for(Index col=0 ; col<other.cols() ; ++col)
+    {
+      for(Index i=lhs.rows()-1 ; i>=0 ; --i)
+      {
+        Scalar tmp = other.coeff(i,col);
+        Scalar l_ii(0);
+        LhsIterator it(lhsEval, i);
+        while(it && it.index()<i)
+          ++it;
+        if(!(Mode & UnitDiag))
+        {
+          eigen_assert(it && it.index()==i);
+          l_ii = it.value();
+          ++it;
+        }
+        else if (it && it.index() == i)
+          ++it;
+        for(; it; ++it)
+        {
+          tmp -= it.value() * other.coeff(it.index(),col);
+        }
+
+        if (Mode & UnitDiag)  other.coeffRef(i,col) = tmp;
+        else                  other.coeffRef(i,col) = tmp/l_ii;
+      }
+    }
+  }
+};
+
+// forward substitution, col-major
+template<typename Lhs, typename Rhs, int Mode>
+struct sparse_solve_triangular_selector<Lhs,Rhs,Mode,Lower,ColMajor>
+{
+  typedef typename Rhs::Scalar Scalar;
+  typedef evaluator<Lhs> LhsEval;
+  typedef typename evaluator<Lhs>::InnerIterator LhsIterator;
+  static void run(const Lhs& lhs, Rhs& other)
+  {
+    LhsEval lhsEval(lhs);
+    for(Index col=0 ; col<other.cols() ; ++col)
+    {
+      for(Index i=0; i<lhs.cols(); ++i)
+      {
+        Scalar& tmp = other.coeffRef(i,col);
+        if (tmp!=Scalar(0)) // optimization when other is actually sparse
+        {
+          LhsIterator it(lhsEval, i);
+          while(it && it.index()<i)
+            ++it;
+          if(!(Mode & UnitDiag))
+          {
+            eigen_assert(it && it.index()==i);
+            tmp /= it.value();
+          }
+          if (it && it.index()==i)
+            ++it;
+          for(; it; ++it)
+            other.coeffRef(it.index(), col) -= tmp * it.value();
+        }
+      }
+    }
+  }
+};
+
+// backward substitution, col-major
+template<typename Lhs, typename Rhs, int Mode>
+struct sparse_solve_triangular_selector<Lhs,Rhs,Mode,Upper,ColMajor>
+{
+  typedef typename Rhs::Scalar Scalar;
+  typedef evaluator<Lhs> LhsEval;
+  typedef typename evaluator<Lhs>::InnerIterator LhsIterator;
+  static void run(const Lhs& lhs, Rhs& other)
+  {
+    LhsEval lhsEval(lhs);
+    for(Index col=0 ; col<other.cols() ; ++col)
+    {
+      for(Index i=lhs.cols()-1; i>=0; --i)
+      {
+        Scalar& tmp = other.coeffRef(i,col);
+        if (tmp!=Scalar(0)) // optimization when other is actually sparse
+        {
+          if(!(Mode & UnitDiag))
+          {
+            // TODO replace this by a binary search. make sure the binary search is safe for partially sorted elements
+            LhsIterator it(lhsEval, i);
+            while(it && it.index()!=i)
+              ++it;
+            eigen_assert(it && it.index()==i);
+            other.coeffRef(i,col) /= it.value();
+          }
+          LhsIterator it(lhsEval, i);
+          for(; it && it.index()<i; ++it)
+            other.coeffRef(it.index(), col) -= tmp * it.value();
+        }
+      }
+    }
+  }
+};
+
+} // end namespace internal
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+
+template<typename ExpressionType,unsigned int Mode>
+template<typename OtherDerived>
+void TriangularViewImpl<ExpressionType,Mode,Sparse>::solveInPlace(MatrixBase<OtherDerived>& other) const
+{
+  eigen_assert(derived().cols() == derived().rows() && derived().cols() == other.rows());
+  eigen_assert((!(Mode & ZeroDiag)) && bool(Mode & (Upper|Lower)));
+
+  enum { copy = internal::traits<OtherDerived>::Flags & RowMajorBit };
+
+  typedef typename internal::conditional<copy,
+    typename internal::plain_matrix_type_column_major<OtherDerived>::type, OtherDerived&>::type OtherCopy;
+  OtherCopy otherCopy(other.derived());
+
+  internal::sparse_solve_triangular_selector<ExpressionType, typename internal::remove_reference<OtherCopy>::type, Mode>::run(derived().nestedExpression(), otherCopy);
+
+  if (copy)
+    other = otherCopy;
+}
+#endif
+
+// pure sparse path
+
+namespace internal {
+
+template<typename Lhs, typename Rhs, int Mode,
+  int UpLo = (Mode & Lower)
+           ? Lower
+           : (Mode & Upper)
+           ? Upper
+           : -1,
+  int StorageOrder = int(Lhs::Flags) & (RowMajorBit)>
+struct sparse_solve_triangular_sparse_selector;
+
+// forward substitution, col-major
+template<typename Lhs, typename Rhs, int Mode, int UpLo>
+struct sparse_solve_triangular_sparse_selector<Lhs,Rhs,Mode,UpLo,ColMajor>
+{
+  typedef typename Rhs::Scalar Scalar;
+  typedef typename promote_index_type<typename traits<Lhs>::StorageIndex,
+                                      typename traits<Rhs>::StorageIndex>::type StorageIndex;
+  static void run(const Lhs& lhs, Rhs& other)
+  {
+    const bool IsLower = (UpLo==Lower);
+    AmbiVector<Scalar,StorageIndex> tempVector(other.rows()*2);
+    tempVector.setBounds(0,other.rows());
+
+    Rhs res(other.rows(), other.cols());
+    res.reserve(other.nonZeros());
+
+    for(Index col=0 ; col<other.cols() ; ++col)
+    {
+      // FIXME estimate number of non zeros
+      tempVector.init(.99/*float(other.col(col).nonZeros())/float(other.rows())*/);
+      tempVector.setZero();
+      tempVector.restart();
+      for (typename Rhs::InnerIterator rhsIt(other, col); rhsIt; ++rhsIt)
+      {
+        tempVector.coeffRef(rhsIt.index()) = rhsIt.value();
+      }
+
+      for(Index i=IsLower?0:lhs.cols()-1;
+          IsLower?i<lhs.cols():i>=0;
+          i+=IsLower?1:-1)
+      {
+        tempVector.restart();
+        Scalar& ci = tempVector.coeffRef(i);
+        if (ci!=Scalar(0))
+        {
+          // find
+          typename Lhs::InnerIterator it(lhs, i);
+          if(!(Mode & UnitDiag))
+          {
+            if (IsLower)
+            {
+              eigen_assert(it.index()==i);
+              ci /= it.value();
+            }
+            else
+              ci /= lhs.coeff(i,i);
+          }
+          tempVector.restart();
+          if (IsLower)
+          {
+            if (it.index()==i)
+              ++it;
+            for(; it; ++it)
+              tempVector.coeffRef(it.index()) -= ci * it.value();
+          }
+          else
+          {
+            for(; it && it.index()<i; ++it)
+              tempVector.coeffRef(it.index()) -= ci * it.value();
+          }
+        }
+      }
+
+
+      Index count = 0;
+      // FIXME compute a reference value to filter zeros
+      for (typename AmbiVector<Scalar,StorageIndex>::Iterator it(tempVector/*,1e-12*/); it; ++it)
+      {
+        ++ count;
+//         std::cerr << "fill " << it.index() << ", " << col << "\n";
+//         std::cout << it.value() << "  ";
+        // FIXME use insertBack
+        res.insert(it.index(), col) = it.value();
+      }
+//       std::cout << "tempVector.nonZeros() == " << int(count) << " / " << (other.rows()) << "\n";
+    }
+    res.finalize();
+    other = res.markAsRValue();
+  }
+};
+
+} // end namespace internal
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+template<typename ExpressionType,unsigned int Mode>
+template<typename OtherDerived>
+void TriangularViewImpl<ExpressionType,Mode,Sparse>::solveInPlace(SparseMatrixBase<OtherDerived>& other) const
+{
+  eigen_assert(derived().cols() == derived().rows() && derived().cols() == other.rows());
+  eigen_assert( (!(Mode & ZeroDiag)) && bool(Mode & (Upper|Lower)));
+
+//   enum { copy = internal::traits<OtherDerived>::Flags & RowMajorBit };
+
+//   typedef typename internal::conditional<copy,
+//     typename internal::plain_matrix_type_column_major<OtherDerived>::type, OtherDerived&>::type OtherCopy;
+//   OtherCopy otherCopy(other.derived());
+
+  internal::sparse_solve_triangular_sparse_selector<ExpressionType, OtherDerived, Mode>::run(derived().nestedExpression(), other.derived());
+
+//   if (copy)
+//     other = otherCopy;
+}
+#endif
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSETRIANGULARSOLVER_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU.h
new file mode 100644
index 0000000..0c8d893
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU.h
@@ -0,0 +1,923 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2012-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_LU_H
+#define EIGEN_SPARSE_LU_H
+
+namespace Eigen {
+
+template <typename _MatrixType, typename _OrderingType = COLAMDOrdering<typename _MatrixType::StorageIndex> > class SparseLU;
+template <typename MappedSparseMatrixType> struct SparseLUMatrixLReturnType;
+template <typename MatrixLType, typename MatrixUType> struct SparseLUMatrixUReturnType;
+
+template <bool Conjugate,class SparseLUType>
+class SparseLUTransposeView : public SparseSolverBase<SparseLUTransposeView<Conjugate,SparseLUType> >
+{
+protected:
+  typedef SparseSolverBase<SparseLUTransposeView<Conjugate,SparseLUType> > APIBase;
+  using APIBase::m_isInitialized;
+public:
+  typedef typename SparseLUType::Scalar Scalar;
+  typedef typename SparseLUType::StorageIndex StorageIndex;
+  typedef typename SparseLUType::MatrixType MatrixType;
+  typedef typename SparseLUType::OrderingType OrderingType;
+
+  enum {
+    ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+  };
+
+  SparseLUTransposeView() : m_sparseLU(NULL) {}
+  SparseLUTransposeView(const SparseLUTransposeView& view) {
+    this->m_sparseLU = view.m_sparseLU;
+  }
+  void setIsInitialized(const bool isInitialized) {this->m_isInitialized = isInitialized;}
+  void setSparseLU(SparseLUType* sparseLU) {m_sparseLU = sparseLU;}
+  using APIBase::_solve_impl;
+  template<typename Rhs, typename Dest>
+  bool _solve_impl(const MatrixBase<Rhs> &B, MatrixBase<Dest> &X_base) const
+  {
+    Dest& X(X_base.derived());
+    eigen_assert(m_sparseLU->info() == Success && "The matrix should be factorized first");
+    EIGEN_STATIC_ASSERT((Dest::Flags&RowMajorBit)==0,
+                        THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+
+
+    // this ugly const_cast_derived() helps to detect aliasing when applying the permutations
+    for(Index j = 0; j < B.cols(); ++j){
+      X.col(j) = m_sparseLU->colsPermutation() * B.const_cast_derived().col(j);
+    }
+    //Forward substitution with transposed or adjoint of U
+    m_sparseLU->matrixU().template solveTransposedInPlace<Conjugate>(X);
+
+    //Backward substitution with transposed or adjoint of L
+    m_sparseLU->matrixL().template solveTransposedInPlace<Conjugate>(X);
+
+    // Permute back the solution
+    for (Index j = 0; j < B.cols(); ++j)
+      X.col(j) = m_sparseLU->rowsPermutation().transpose() * X.col(j);
+    return true;
+  }
+  inline Index rows() const { return m_sparseLU->rows(); }
+  inline Index cols() const { return m_sparseLU->cols(); }
+
+private:
+  SparseLUType *m_sparseLU;
+  SparseLUTransposeView& operator=(const SparseLUTransposeView&);
+};
+
+
+/** \ingroup SparseLU_Module
+  * \class SparseLU
+  * 
+  * \brief Sparse supernodal LU factorization for general matrices
+  * 
+  * This class implements the supernodal LU factorization for general matrices.
+  * It uses the main techniques from the sequential SuperLU package 
+  * (http://crd-legacy.lbl.gov/~xiaoye/SuperLU/). It handles transparently real 
+  * and complex arithmetic with single and double precision, depending on the 
+  * scalar type of your input matrix. 
+  * The code has been optimized to provide BLAS-3 operations during supernode-panel updates. 
+  * It benefits directly from the built-in high-performant Eigen BLAS routines. 
+  * Moreover, when the size of a supernode is very small, the BLAS calls are avoided to 
+  * enable a better optimization from the compiler. For best performance, 
+  * you should compile it with NDEBUG flag to avoid the numerous bounds checking on vectors. 
+  * 
+  * An important parameter of this class is the ordering method. It is used to reorder the columns 
+  * (and eventually the rows) of the matrix to reduce the number of new elements that are created during 
+  * numerical factorization. The cheapest method available is COLAMD. 
+  * See  \link OrderingMethods_Module the OrderingMethods module \endlink for the list of 
+  * built-in and external ordering methods. 
+  *
+  * Simple example with key steps 
+  * \code
+  * VectorXd x(n), b(n);
+  * SparseMatrix<double> A;
+  * SparseLU<SparseMatrix<double>, COLAMDOrdering<int> >   solver;
+  * // fill A and b;
+  * // Compute the ordering permutation vector from the structural pattern of A
+  * solver.analyzePattern(A); 
+  * // Compute the numerical factorization 
+  * solver.factorize(A); 
+  * //Use the factors to solve the linear system 
+  * x = solver.solve(b); 
+  * \endcode
+  * 
+  * \warning The input matrix A should be in a \b compressed and \b column-major form.
+  * Otherwise an expensive copy will be made. You can call the inexpensive makeCompressed() to get a compressed matrix.
+  * 
+  * \note Unlike the initial SuperLU implementation, there is no step to equilibrate the matrix. 
+  * For badly scaled matrices, this step can be useful to reduce the pivoting during factorization. 
+  * If this is the case for your matrices, you can try the basic scaling method at
+  *  "unsupported/Eigen/src/IterativeSolvers/Scaling.h"
+  * 
+  * \tparam _MatrixType The type of the sparse matrix. It must be a column-major SparseMatrix<>
+  * \tparam _OrderingType The ordering method to use, either AMD, COLAMD or METIS. Default is COLMAD
+  *
+  * \implsparsesolverconcept
+  * 
+  * \sa \ref TutorialSparseSolverConcept
+  * \sa \ref OrderingMethods_Module
+  */
+template <typename _MatrixType, typename _OrderingType>
+class SparseLU : public SparseSolverBase<SparseLU<_MatrixType,_OrderingType> >, public internal::SparseLUImpl<typename _MatrixType::Scalar, typename _MatrixType::StorageIndex>
+{
+  protected:
+    typedef SparseSolverBase<SparseLU<_MatrixType,_OrderingType> > APIBase;
+    using APIBase::m_isInitialized;
+  public:
+    using APIBase::_solve_impl;
+    
+    typedef _MatrixType MatrixType; 
+    typedef _OrderingType OrderingType;
+    typedef typename MatrixType::Scalar Scalar; 
+    typedef typename MatrixType::RealScalar RealScalar; 
+    typedef typename MatrixType::StorageIndex StorageIndex;
+    typedef SparseMatrix<Scalar,ColMajor,StorageIndex> NCMatrix;
+    typedef internal::MappedSuperNodalMatrix<Scalar, StorageIndex> SCMatrix;
+    typedef Matrix<Scalar,Dynamic,1> ScalarVector;
+    typedef Matrix<StorageIndex,Dynamic,1> IndexVector;
+    typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
+    typedef internal::SparseLUImpl<Scalar, StorageIndex> Base;
+
+    enum {
+      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+    };
+    
+  public:
+
+    SparseLU():m_lastError(""),m_Ustore(0,0,0,0,0,0),m_symmetricmode(false),m_diagpivotthresh(1.0),m_detPermR(1)
+    {
+      initperfvalues(); 
+    }
+    explicit SparseLU(const MatrixType& matrix)
+      : m_lastError(""),m_Ustore(0,0,0,0,0,0),m_symmetricmode(false),m_diagpivotthresh(1.0),m_detPermR(1)
+    {
+      initperfvalues(); 
+      compute(matrix);
+    }
+    
+    ~SparseLU()
+    {
+      // Free all explicit dynamic pointers 
+    }
+    
+    void analyzePattern (const MatrixType& matrix);
+    void factorize (const MatrixType& matrix);
+    void simplicialfactorize(const MatrixType& matrix);
+    
+    /**
+      * Compute the symbolic and numeric factorization of the input sparse matrix.
+      * The input matrix should be in column-major storage. 
+      */
+    void compute (const MatrixType& matrix)
+    {
+      // Analyze 
+      analyzePattern(matrix); 
+      //Factorize
+      factorize(matrix);
+    } 
+
+    /** \returns an expression of the transposed of the factored matrix.
+      *
+      * A typical usage is to solve for the transposed problem A^T x = b:
+      * \code
+      * solver.compute(A);
+      * x = solver.transpose().solve(b);
+      * \endcode
+      *
+      * \sa adjoint(), solve()
+      */
+    const SparseLUTransposeView<false,SparseLU<_MatrixType,_OrderingType> > transpose()
+    {
+      SparseLUTransposeView<false,  SparseLU<_MatrixType,_OrderingType> > transposeView;
+      transposeView.setSparseLU(this);
+      transposeView.setIsInitialized(this->m_isInitialized);
+      return transposeView;
+    }
+
+
+    /** \returns an expression of the adjoint of the factored matrix
+      *
+      * A typical usage is to solve for the adjoint problem A' x = b:
+      * \code
+      * solver.compute(A);
+      * x = solver.adjoint().solve(b);
+      * \endcode
+      *
+      * For real scalar types, this function is equivalent to transpose().
+      *
+      * \sa transpose(), solve()
+      */
+    const SparseLUTransposeView<true, SparseLU<_MatrixType,_OrderingType> > adjoint()
+    {
+      SparseLUTransposeView<true,  SparseLU<_MatrixType,_OrderingType> > adjointView;
+      adjointView.setSparseLU(this);
+      adjointView.setIsInitialized(this->m_isInitialized);
+      return adjointView;
+    }
+    
+    inline Index rows() const { return m_mat.rows(); }
+    inline Index cols() const { return m_mat.cols(); }
+    /** Indicate that the pattern of the input matrix is symmetric */
+    void isSymmetric(bool sym)
+    {
+      m_symmetricmode = sym;
+    }
+    
+    /** \returns an expression of the matrix L, internally stored as supernodes
+      * The only operation available with this expression is the triangular solve
+      * \code
+      * y = b; matrixL().solveInPlace(y);
+      * \endcode
+      */
+    SparseLUMatrixLReturnType<SCMatrix> matrixL() const
+    {
+      return SparseLUMatrixLReturnType<SCMatrix>(m_Lstore);
+    }
+    /** \returns an expression of the matrix U,
+      * The only operation available with this expression is the triangular solve
+      * \code
+      * y = b; matrixU().solveInPlace(y);
+      * \endcode
+      */
+    SparseLUMatrixUReturnType<SCMatrix,MappedSparseMatrix<Scalar,ColMajor,StorageIndex> > matrixU() const
+    {
+      return SparseLUMatrixUReturnType<SCMatrix, MappedSparseMatrix<Scalar,ColMajor,StorageIndex> >(m_Lstore, m_Ustore);
+    }
+
+    /**
+      * \returns a reference to the row matrix permutation \f$ P_r \f$ such that \f$P_r A P_c^T = L U\f$
+      * \sa colsPermutation()
+      */
+    inline const PermutationType& rowsPermutation() const
+    {
+      return m_perm_r;
+    }
+    /**
+      * \returns a reference to the column matrix permutation\f$ P_c^T \f$ such that \f$P_r A P_c^T = L U\f$
+      * \sa rowsPermutation()
+      */
+    inline const PermutationType& colsPermutation() const
+    {
+      return m_perm_c;
+    }
+    /** Set the threshold used for a diagonal entry to be an acceptable pivot. */
+    void setPivotThreshold(const RealScalar& thresh)
+    {
+      m_diagpivotthresh = thresh; 
+    }
+
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+    /** \returns the solution X of \f$ A X = B \f$ using the current decomposition of A.
+      *
+      * \warning the destination matrix X in X = this->solve(B) must be colmun-major.
+      *
+      * \sa compute()
+      */
+    template<typename Rhs>
+    inline const Solve<SparseLU, Rhs> solve(const MatrixBase<Rhs>& B) const;
+#endif // EIGEN_PARSED_BY_DOXYGEN
+    
+    /** \brief Reports whether previous computation was successful.
+      *
+      * \returns \c Success if computation was successful,
+      *          \c NumericalIssue if the LU factorization reports a problem, zero diagonal for instance
+      *          \c InvalidInput if the input matrix is invalid
+      *
+      * \sa iparm()          
+      */
+    ComputationInfo info() const
+    {
+      eigen_assert(m_isInitialized && "Decomposition is not initialized.");
+      return m_info;
+    }
+    
+    /**
+      * \returns A string describing the type of error
+      */
+    std::string lastErrorMessage() const
+    {
+      return m_lastError; 
+    }
+
+    template<typename Rhs, typename Dest>
+    bool _solve_impl(const MatrixBase<Rhs> &B, MatrixBase<Dest> &X_base) const
+    {
+      Dest& X(X_base.derived());
+      eigen_assert(m_factorizationIsOk && "The matrix should be factorized first");
+      EIGEN_STATIC_ASSERT((Dest::Flags&RowMajorBit)==0,
+                        THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+      
+      // Permute the right hand side to form X = Pr*B
+      // on return, X is overwritten by the computed solution
+      X.resize(B.rows(),B.cols());
+
+      // this ugly const_cast_derived() helps to detect aliasing when applying the permutations
+      for(Index j = 0; j < B.cols(); ++j)
+        X.col(j) = rowsPermutation() * B.const_cast_derived().col(j);
+      
+      //Forward substitution with L
+      this->matrixL().solveInPlace(X);
+      this->matrixU().solveInPlace(X);
+      
+      // Permute back the solution 
+      for (Index j = 0; j < B.cols(); ++j)
+        X.col(j) = colsPermutation().inverse() * X.col(j);
+      
+      return true; 
+    }
+    
+    /**
+      * \returns the absolute value of the determinant of the matrix of which
+      * *this is the QR decomposition.
+      *
+      * \warning a determinant can be very big or small, so for matrices
+      * of large enough dimension, there is a risk of overflow/underflow.
+      * One way to work around that is to use logAbsDeterminant() instead.
+      *
+      * \sa logAbsDeterminant(), signDeterminant()
+      */
+    Scalar absDeterminant()
+    {
+      using std::abs;
+      eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
+      // Initialize with the determinant of the row matrix
+      Scalar det = Scalar(1.);
+      // Note that the diagonal blocks of U are stored in supernodes,
+      // which are available in the  L part :)
+      for (Index j = 0; j < this->cols(); ++j)
+      {
+        for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it)
+        {
+          if(it.index() == j)
+          {
+            det *= abs(it.value());
+            break;
+          }
+        }
+      }
+      return det;
+    }
+
+    /** \returns the natural log of the absolute value of the determinant of the matrix
+      * of which **this is the QR decomposition
+      *
+      * \note This method is useful to work around the risk of overflow/underflow that's
+      * inherent to the determinant computation.
+      *
+      * \sa absDeterminant(), signDeterminant()
+      */
+    Scalar logAbsDeterminant() const
+    {
+      using std::log;
+      using std::abs;
+
+      eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
+      Scalar det = Scalar(0.);
+      for (Index j = 0; j < this->cols(); ++j)
+      {
+        for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it)
+        {
+          if(it.row() < j) continue;
+          if(it.row() == j)
+          {
+            det += log(abs(it.value()));
+            break;
+          }
+        }
+      }
+      return det;
+    }
+
+    /** \returns A number representing the sign of the determinant
+      *
+      * \sa absDeterminant(), logAbsDeterminant()
+      */
+    Scalar signDeterminant()
+    {
+      eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
+      // Initialize with the determinant of the row matrix
+      Index det = 1;
+      // Note that the diagonal blocks of U are stored in supernodes,
+      // which are available in the  L part :)
+      for (Index j = 0; j < this->cols(); ++j)
+      {
+        for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it)
+        {
+          if(it.index() == j)
+          {
+            if(it.value()<0)
+              det = -det;
+            else if(it.value()==0)
+              return 0;
+            break;
+          }
+        }
+      }
+      return det * m_detPermR * m_detPermC;
+    }
+    
+    /** \returns The determinant of the matrix.
+      *
+      * \sa absDeterminant(), logAbsDeterminant()
+      */
+    Scalar determinant()
+    {
+      eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
+      // Initialize with the determinant of the row matrix
+      Scalar det = Scalar(1.);
+      // Note that the diagonal blocks of U are stored in supernodes,
+      // which are available in the  L part :)
+      for (Index j = 0; j < this->cols(); ++j)
+      {
+        for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it)
+        {
+          if(it.index() == j)
+          {
+            det *= it.value();
+            break;
+          }
+        }
+      }
+      return (m_detPermR * m_detPermC) > 0 ? det : -det;
+    }
+
+    Index nnzL() const { return m_nnzL; };
+    Index nnzU() const { return m_nnzU; };
+
+  protected:
+    // Functions 
+    void initperfvalues()
+    {
+      m_perfv.panel_size = 16;
+      m_perfv.relax = 1; 
+      m_perfv.maxsuper = 128; 
+      m_perfv.rowblk = 16; 
+      m_perfv.colblk = 8; 
+      m_perfv.fillfactor = 20;  
+    }
+      
+    // Variables 
+    mutable ComputationInfo m_info;
+    bool m_factorizationIsOk;
+    bool m_analysisIsOk;
+    std::string m_lastError;
+    NCMatrix m_mat; // The input (permuted ) matrix 
+    SCMatrix m_Lstore; // The lower triangular matrix (supernodal)
+    MappedSparseMatrix<Scalar,ColMajor,StorageIndex> m_Ustore; // The upper triangular matrix
+    PermutationType m_perm_c; // Column permutation 
+    PermutationType m_perm_r ; // Row permutation
+    IndexVector m_etree; // Column elimination tree 
+    
+    typename Base::GlobalLU_t m_glu; 
+                               
+    // SparseLU options 
+    bool m_symmetricmode;
+    // values for performance 
+    internal::perfvalues m_perfv;
+    RealScalar m_diagpivotthresh; // Specifies the threshold used for a diagonal entry to be an acceptable pivot
+    Index m_nnzL, m_nnzU; // Nonzeros in L and U factors
+    Index m_detPermR, m_detPermC; // Determinants of the permutation matrices
+  private:
+    // Disable copy constructor 
+    SparseLU (const SparseLU& );
+}; // End class SparseLU
+
+
+
+// Functions needed by the anaysis phase
+/** 
+  * Compute the column permutation to minimize the fill-in
+  * 
+  *  - Apply this permutation to the input matrix - 
+  * 
+  *  - Compute the column elimination tree on the permuted matrix 
+  * 
+  *  - Postorder the elimination tree and the column permutation
+  * 
+  */
+template <typename MatrixType, typename OrderingType>
+void SparseLU<MatrixType, OrderingType>::analyzePattern(const MatrixType& mat)
+{
+  
+  //TODO  It is possible as in SuperLU to compute row and columns scaling vectors to equilibrate the matrix mat.
+  
+  // Firstly, copy the whole input matrix. 
+  m_mat = mat;
+  
+  // Compute fill-in ordering
+  OrderingType ord; 
+  ord(m_mat,m_perm_c);
+  
+  // Apply the permutation to the column of the input  matrix
+  if (m_perm_c.size())
+  {
+    m_mat.uncompress(); //NOTE: The effect of this command is only to create the InnerNonzeros pointers. FIXME : This vector is filled but not subsequently used.  
+    // Then, permute only the column pointers
+    ei_declare_aligned_stack_constructed_variable(StorageIndex,outerIndexPtr,mat.cols()+1,mat.isCompressed()?const_cast<StorageIndex*>(mat.outerIndexPtr()):0);
+    
+    // If the input matrix 'mat' is uncompressed, then the outer-indices do not match the ones of m_mat, and a copy is thus needed.
+    if(!mat.isCompressed()) 
+      IndexVector::Map(outerIndexPtr, mat.cols()+1) = IndexVector::Map(m_mat.outerIndexPtr(),mat.cols()+1);
+    
+    // Apply the permutation and compute the nnz per column.
+    for (Index i = 0; i < mat.cols(); i++)
+    {
+      m_mat.outerIndexPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i];
+      m_mat.innerNonZeroPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i+1] - outerIndexPtr[i];
+    }
+  }
+  
+  // Compute the column elimination tree of the permuted matrix 
+  IndexVector firstRowElt;
+  internal::coletree(m_mat, m_etree,firstRowElt); 
+     
+  // In symmetric mode, do not do postorder here
+  if (!m_symmetricmode) {
+    IndexVector post, iwork; 
+    // Post order etree
+    internal::treePostorder(StorageIndex(m_mat.cols()), m_etree, post); 
+      
+   
+    // Renumber etree in postorder 
+    Index m = m_mat.cols(); 
+    iwork.resize(m+1);
+    for (Index i = 0; i < m; ++i) iwork(post(i)) = post(m_etree(i));
+    m_etree = iwork;
+    
+    // Postmultiply A*Pc by post, i.e reorder the matrix according to the postorder of the etree
+    PermutationType post_perm(m); 
+    for (Index i = 0; i < m; i++) 
+      post_perm.indices()(i) = post(i); 
+        
+    // Combine the two permutations : postorder the permutation for future use
+    if(m_perm_c.size()) {
+      m_perm_c = post_perm * m_perm_c;
+    }
+    
+  } // end postordering 
+  
+  m_analysisIsOk = true; 
+}
+
+// Functions needed by the numerical factorization phase
+
+
+/** 
+  *  - Numerical factorization 
+  *  - Interleaved with the symbolic factorization 
+  * On exit,  info is 
+  * 
+  *    = 0: successful factorization
+  * 
+  *    > 0: if info = i, and i is
+  * 
+  *       <= A->ncol: U(i,i) is exactly zero. The factorization has
+  *          been completed, but the factor U is exactly singular,
+  *          and division by zero will occur if it is used to solve a
+  *          system of equations.
+  * 
+  *       > A->ncol: number of bytes allocated when memory allocation
+  *         failure occurred, plus A->ncol. If lwork = -1, it is
+  *         the estimated amount of space needed, plus A->ncol.  
+  */
+template <typename MatrixType, typename OrderingType>
+void SparseLU<MatrixType, OrderingType>::factorize(const MatrixType& matrix)
+{
+  using internal::emptyIdxLU;
+  eigen_assert(m_analysisIsOk && "analyzePattern() should be called first"); 
+  eigen_assert((matrix.rows() == matrix.cols()) && "Only for squared matrices");
+  
+  m_isInitialized = true;
+  
+  // Apply the column permutation computed in analyzepattern()
+  //   m_mat = matrix * m_perm_c.inverse(); 
+  m_mat = matrix;
+  if (m_perm_c.size()) 
+  {
+    m_mat.uncompress(); //NOTE: The effect of this command is only to create the InnerNonzeros pointers.
+    //Then, permute only the column pointers
+    const StorageIndex * outerIndexPtr;
+    if (matrix.isCompressed()) outerIndexPtr = matrix.outerIndexPtr();
+    else
+    {
+      StorageIndex* outerIndexPtr_t = new StorageIndex[matrix.cols()+1];
+      for(Index i = 0; i <= matrix.cols(); i++) outerIndexPtr_t[i] = m_mat.outerIndexPtr()[i];
+      outerIndexPtr = outerIndexPtr_t;
+    }
+    for (Index i = 0; i < matrix.cols(); i++)
+    {
+      m_mat.outerIndexPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i];
+      m_mat.innerNonZeroPtr()[m_perm_c.indices()(i)] = outerIndexPtr[i+1] - outerIndexPtr[i];
+    }
+    if(!matrix.isCompressed()) delete[] outerIndexPtr;
+  } 
+  else 
+  { //FIXME This should not be needed if the empty permutation is handled transparently
+    m_perm_c.resize(matrix.cols());
+    for(StorageIndex i = 0; i < matrix.cols(); ++i) m_perm_c.indices()(i) = i;
+  }
+  
+  Index m = m_mat.rows();
+  Index n = m_mat.cols();
+  Index nnz = m_mat.nonZeros();
+  Index maxpanel = m_perfv.panel_size * m;
+  // Allocate working storage common to the factor routines
+  Index lwork = 0;
+  Index info = Base::memInit(m, n, nnz, lwork, m_perfv.fillfactor, m_perfv.panel_size, m_glu); 
+  if (info) 
+  {
+    m_lastError = "UNABLE TO ALLOCATE WORKING MEMORY\n\n" ;
+    m_factorizationIsOk = false;
+    return ; 
+  }
+  
+  // Set up pointers for integer working arrays 
+  IndexVector segrep(m); segrep.setZero();
+  IndexVector parent(m); parent.setZero();
+  IndexVector xplore(m); xplore.setZero();
+  IndexVector repfnz(maxpanel);
+  IndexVector panel_lsub(maxpanel);
+  IndexVector xprune(n); xprune.setZero();
+  IndexVector marker(m*internal::LUNoMarker); marker.setZero();
+  
+  repfnz.setConstant(-1); 
+  panel_lsub.setConstant(-1);
+  
+  // Set up pointers for scalar working arrays 
+  ScalarVector dense; 
+  dense.setZero(maxpanel);
+  ScalarVector tempv; 
+  tempv.setZero(internal::LUnumTempV(m, m_perfv.panel_size, m_perfv.maxsuper, /*m_perfv.rowblk*/m) );
+  
+  // Compute the inverse of perm_c
+  PermutationType iperm_c(m_perm_c.inverse()); 
+  
+  // Identify initial relaxed snodes
+  IndexVector relax_end(n);
+  if ( m_symmetricmode == true ) 
+    Base::heap_relax_snode(n, m_etree, m_perfv.relax, marker, relax_end);
+  else
+    Base::relax_snode(n, m_etree, m_perfv.relax, marker, relax_end);
+  
+  
+  m_perm_r.resize(m); 
+  m_perm_r.indices().setConstant(-1);
+  marker.setConstant(-1);
+  m_detPermR = 1; // Record the determinant of the row permutation
+  
+  m_glu.supno(0) = emptyIdxLU; m_glu.xsup.setConstant(0);
+  m_glu.xsup(0) = m_glu.xlsub(0) = m_glu.xusub(0) = m_glu.xlusup(0) = Index(0);
+  
+  // Work on one 'panel' at a time. A panel is one of the following :
+  //  (a) a relaxed supernode at the bottom of the etree, or
+  //  (b) panel_size contiguous columns, <panel_size> defined by the user
+  Index jcol; 
+  Index pivrow; // Pivotal row number in the original row matrix
+  Index nseg1; // Number of segments in U-column above panel row jcol
+  Index nseg; // Number of segments in each U-column 
+  Index irep; 
+  Index i, k, jj; 
+  for (jcol = 0; jcol < n; )
+  {
+    // Adjust panel size so that a panel won't overlap with the next relaxed snode. 
+    Index panel_size = m_perfv.panel_size; // upper bound on panel width
+    for (k = jcol + 1; k < (std::min)(jcol+panel_size, n); k++)
+    {
+      if (relax_end(k) != emptyIdxLU) 
+      {
+        panel_size = k - jcol; 
+        break; 
+      }
+    }
+    if (k == n) 
+      panel_size = n - jcol; 
+      
+    // Symbolic outer factorization on a panel of columns 
+    Base::panel_dfs(m, panel_size, jcol, m_mat, m_perm_r.indices(), nseg1, dense, panel_lsub, segrep, repfnz, xprune, marker, parent, xplore, m_glu); 
+    
+    // Numeric sup-panel updates in topological order 
+    Base::panel_bmod(m, panel_size, jcol, nseg1, dense, tempv, segrep, repfnz, m_glu); 
+    
+    // Sparse LU within the panel, and below the panel diagonal 
+    for ( jj = jcol; jj< jcol + panel_size; jj++) 
+    {
+      k = (jj - jcol) * m; // Column index for w-wide arrays 
+      
+      nseg = nseg1; // begin after all the panel segments
+      //Depth-first-search for the current column
+      VectorBlock<IndexVector> panel_lsubk(panel_lsub, k, m);
+      VectorBlock<IndexVector> repfnz_k(repfnz, k, m); 
+      info = Base::column_dfs(m, jj, m_perm_r.indices(), m_perfv.maxsuper, nseg, panel_lsubk, segrep, repfnz_k, xprune, marker, parent, xplore, m_glu); 
+      if ( info ) 
+      {
+        m_lastError =  "UNABLE TO EXPAND MEMORY IN COLUMN_DFS() ";
+        m_info = NumericalIssue; 
+        m_factorizationIsOk = false; 
+        return; 
+      }
+      // Numeric updates to this column 
+      VectorBlock<ScalarVector> dense_k(dense, k, m); 
+      VectorBlock<IndexVector> segrep_k(segrep, nseg1, m-nseg1); 
+      info = Base::column_bmod(jj, (nseg - nseg1), dense_k, tempv, segrep_k, repfnz_k, jcol, m_glu); 
+      if ( info ) 
+      {
+        m_lastError = "UNABLE TO EXPAND MEMORY IN COLUMN_BMOD() ";
+        m_info = NumericalIssue; 
+        m_factorizationIsOk = false; 
+        return; 
+      }
+      
+      // Copy the U-segments to ucol(*)
+      info = Base::copy_to_ucol(jj, nseg, segrep, repfnz_k ,m_perm_r.indices(), dense_k, m_glu); 
+      if ( info ) 
+      {
+        m_lastError = "UNABLE TO EXPAND MEMORY IN COPY_TO_UCOL() ";
+        m_info = NumericalIssue; 
+        m_factorizationIsOk = false; 
+        return; 
+      }
+      
+      // Form the L-segment 
+      info = Base::pivotL(jj, m_diagpivotthresh, m_perm_r.indices(), iperm_c.indices(), pivrow, m_glu);
+      if ( info ) 
+      {
+        m_lastError = "THE MATRIX IS STRUCTURALLY SINGULAR ... ZERO COLUMN AT ";
+        std::ostringstream returnInfo;
+        returnInfo << info; 
+        m_lastError += returnInfo.str();
+        m_info = NumericalIssue; 
+        m_factorizationIsOk = false; 
+        return; 
+      }
+      
+      // Update the determinant of the row permutation matrix
+      // FIXME: the following test is not correct, we should probably take iperm_c into account and pivrow is not directly the row pivot.
+      if (pivrow != jj) m_detPermR = -m_detPermR;
+
+      // Prune columns (0:jj-1) using column jj
+      Base::pruneL(jj, m_perm_r.indices(), pivrow, nseg, segrep, repfnz_k, xprune, m_glu); 
+      
+      // Reset repfnz for this column 
+      for (i = 0; i < nseg; i++)
+      {
+        irep = segrep(i); 
+        repfnz_k(irep) = emptyIdxLU; 
+      }
+    } // end SparseLU within the panel  
+    jcol += panel_size;  // Move to the next panel
+  } // end for -- end elimination 
+  
+  m_detPermR = m_perm_r.determinant();
+  m_detPermC = m_perm_c.determinant();
+  
+  // Count the number of nonzeros in factors 
+  Base::countnz(n, m_nnzL, m_nnzU, m_glu); 
+  // Apply permutation  to the L subscripts 
+  Base::fixupL(n, m_perm_r.indices(), m_glu);
+  
+  // Create supernode matrix L 
+  m_Lstore.setInfos(m, n, m_glu.lusup, m_glu.xlusup, m_glu.lsub, m_glu.xlsub, m_glu.supno, m_glu.xsup); 
+  // Create the column major upper sparse matrix  U; 
+  new (&m_Ustore) MappedSparseMatrix<Scalar, ColMajor, StorageIndex> ( m, n, m_nnzU, m_glu.xusub.data(), m_glu.usub.data(), m_glu.ucol.data() );
+  
+  m_info = Success;
+  m_factorizationIsOk = true;
+}
+
+template<typename MappedSupernodalType>
+struct SparseLUMatrixLReturnType : internal::no_assignment_operator
+{
+  typedef typename MappedSupernodalType::Scalar Scalar;
+  explicit SparseLUMatrixLReturnType(const MappedSupernodalType& mapL) : m_mapL(mapL)
+  { }
+  Index rows() const { return m_mapL.rows(); }
+  Index cols() const { return m_mapL.cols(); }
+  template<typename Dest>
+  void solveInPlace( MatrixBase<Dest> &X) const
+  {
+    m_mapL.solveInPlace(X);
+  }
+  template<bool Conjugate, typename Dest>
+  void solveTransposedInPlace( MatrixBase<Dest> &X) const
+  {
+    m_mapL.template solveTransposedInPlace<Conjugate>(X);
+  }
+
+  const MappedSupernodalType& m_mapL;
+};
+
+template<typename MatrixLType, typename MatrixUType>
+struct SparseLUMatrixUReturnType : internal::no_assignment_operator
+{
+  typedef typename MatrixLType::Scalar Scalar;
+  SparseLUMatrixUReturnType(const MatrixLType& mapL, const MatrixUType& mapU)
+  : m_mapL(mapL),m_mapU(mapU)
+  { }
+  Index rows() const { return m_mapL.rows(); }
+  Index cols() const { return m_mapL.cols(); }
+
+  template<typename Dest>   void solveInPlace(MatrixBase<Dest> &X) const
+  {
+    Index nrhs = X.cols();
+    Index n    = X.rows();
+    // Backward solve with U
+    for (Index k = m_mapL.nsuper(); k >= 0; k--)
+    {
+      Index fsupc = m_mapL.supToCol()[k];
+      Index lda = m_mapL.colIndexPtr()[fsupc+1] - m_mapL.colIndexPtr()[fsupc]; // leading dimension
+      Index nsupc = m_mapL.supToCol()[k+1] - fsupc;
+      Index luptr = m_mapL.colIndexPtr()[fsupc];
+
+      if (nsupc == 1)
+      {
+        for (Index j = 0; j < nrhs; j++)
+        {
+          X(fsupc, j) /= m_mapL.valuePtr()[luptr];
+        }
+      }
+      else
+      {
+        // FIXME: the following lines should use Block expressions and not Map!
+        Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > A( &(m_mapL.valuePtr()[luptr]), nsupc, nsupc, OuterStride<>(lda) );
+        Map< Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor>, 0, OuterStride<> > U (&(X.coeffRef(fsupc,0)), nsupc, nrhs, OuterStride<>(n) );
+        U = A.template triangularView<Upper>().solve(U);
+      }
+
+      for (Index j = 0; j < nrhs; ++j)
+      {
+        for (Index jcol = fsupc; jcol < fsupc + nsupc; jcol++)
+        {
+          typename MatrixUType::InnerIterator it(m_mapU, jcol);
+          for ( ; it; ++it)
+          {
+            Index irow = it.index();
+            X(irow, j) -= X(jcol, j) * it.value();
+          }
+        }
+      }
+    } // End For U-solve
+  }
+
+  template<bool Conjugate, typename Dest>   void solveTransposedInPlace(MatrixBase<Dest> &X) const
+  {
+    using numext::conj;
+    Index nrhs = X.cols();
+    Index n    = X.rows();
+    // Forward solve with U
+    for (Index k = 0; k <=  m_mapL.nsuper(); k++)
+    {
+      Index fsupc = m_mapL.supToCol()[k];
+      Index lda = m_mapL.colIndexPtr()[fsupc+1] - m_mapL.colIndexPtr()[fsupc]; // leading dimension
+      Index nsupc = m_mapL.supToCol()[k+1] - fsupc;
+      Index luptr = m_mapL.colIndexPtr()[fsupc];
+
+      for (Index j = 0; j < nrhs; ++j)
+      {
+        for (Index jcol = fsupc; jcol < fsupc + nsupc; jcol++)
+        {
+          typename MatrixUType::InnerIterator it(m_mapU, jcol);
+          for ( ; it; ++it)
+          {
+            Index irow = it.index();
+            X(jcol, j) -= X(irow, j) * (Conjugate? conj(it.value()): it.value());
+          }
+        }
+      }
+      if (nsupc == 1)
+      {
+        for (Index j = 0; j < nrhs; j++)
+        {
+          X(fsupc, j) /= (Conjugate? conj(m_mapL.valuePtr()[luptr]) : m_mapL.valuePtr()[luptr]);
+        }
+      }
+      else
+      {
+        Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > A( &(m_mapL.valuePtr()[luptr]), nsupc, nsupc, OuterStride<>(lda) );
+        Map< Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor>, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) );
+        if(Conjugate)
+          U = A.adjoint().template triangularView<Lower>().solve(U);
+        else
+          U = A.transpose().template triangularView<Lower>().solve(U);
+      }
+    }// End For U-solve
+  }
+
+
+  const MatrixLType& m_mapL;
+  const MatrixUType& m_mapU;
+};
+
+} // End namespace Eigen 
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLUImpl.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLUImpl.h
new file mode 100644
index 0000000..fc0cfc4
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLUImpl.h
@@ -0,0 +1,66 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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 SPARSELU_IMPL_H
+#define SPARSELU_IMPL_H
+
+namespace Eigen {
+namespace internal {
+  
+/** \ingroup SparseLU_Module
+  * \class SparseLUImpl
+  * Base class for sparseLU
+  */
+template <typename Scalar, typename StorageIndex>
+class SparseLUImpl
+{
+  public:
+    typedef Matrix<Scalar,Dynamic,1> ScalarVector;
+    typedef Matrix<StorageIndex,Dynamic,1> IndexVector; 
+    typedef Matrix<Scalar,Dynamic,Dynamic,ColMajor> ScalarMatrix;
+    typedef Map<ScalarMatrix, 0,  OuterStride<> > MappedMatrixBlock;
+    typedef typename ScalarVector::RealScalar RealScalar; 
+    typedef Ref<Matrix<Scalar,Dynamic,1> > BlockScalarVector;
+    typedef Ref<Matrix<StorageIndex,Dynamic,1> > BlockIndexVector;
+    typedef LU_GlobalLU_t<IndexVector, ScalarVector> GlobalLU_t; 
+    typedef SparseMatrix<Scalar,ColMajor,StorageIndex> MatrixType; 
+    
+  protected:
+     template <typename VectorType>
+     Index expand(VectorType& vec, Index& length, Index nbElts, Index keep_prev, Index& num_expansions);
+     Index memInit(Index m, Index n, Index annz, Index lwork, Index fillratio, Index panel_size,  GlobalLU_t& glu); 
+     template <typename VectorType>
+     Index memXpand(VectorType& vec, Index& maxlen, Index nbElts, MemType memtype, Index& num_expansions);
+     void heap_relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end); 
+     void relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end); 
+     Index snode_dfs(const Index jcol, const Index kcol,const MatrixType& mat,  IndexVector& xprune, IndexVector& marker, GlobalLU_t& glu); 
+     Index snode_bmod (const Index jcol, const Index fsupc, ScalarVector& dense, GlobalLU_t& glu);
+     Index pivotL(const Index jcol, const RealScalar& diagpivotthresh, IndexVector& perm_r, IndexVector& iperm_c, Index& pivrow, GlobalLU_t& glu);
+     template <typename Traits>
+     void dfs_kernel(const StorageIndex jj, IndexVector& perm_r,
+                    Index& nseg, IndexVector& panel_lsub, IndexVector& segrep,
+                    Ref<IndexVector> repfnz_col, IndexVector& xprune, Ref<IndexVector> marker, IndexVector& parent,
+                    IndexVector& xplore, GlobalLU_t& glu, Index& nextl_col, Index krow, Traits& traits);
+     void panel_dfs(const Index m, const Index w, const Index jcol, MatrixType& A, IndexVector& perm_r, Index& nseg, ScalarVector& dense, IndexVector& panel_lsub, IndexVector& segrep, IndexVector& repfnz, IndexVector& xprune, IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu);
+    
+     void panel_bmod(const Index m, const Index w, const Index jcol, const Index nseg, ScalarVector& dense, ScalarVector& tempv, IndexVector& segrep, IndexVector& repfnz, GlobalLU_t& glu);
+     Index column_dfs(const Index m, const Index jcol, IndexVector& perm_r, Index maxsuper, Index& nseg,  BlockIndexVector lsub_col, IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune, IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu);
+     Index column_bmod(const Index jcol, const Index nseg, BlockScalarVector dense, ScalarVector& tempv, BlockIndexVector segrep, BlockIndexVector repfnz, Index fpanelc, GlobalLU_t& glu); 
+     Index copy_to_ucol(const Index jcol, const Index nseg, IndexVector& segrep, BlockIndexVector repfnz ,IndexVector& perm_r, BlockScalarVector dense, GlobalLU_t& glu); 
+     void pruneL(const Index jcol, const IndexVector& perm_r, const Index pivrow, const Index nseg, const IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune, GlobalLU_t& glu);
+     void countnz(const Index n, Index& nnzL, Index& nnzU, GlobalLU_t& glu); 
+     void fixupL(const Index n, const IndexVector& perm_r, GlobalLU_t& glu); 
+     
+     template<typename , typename >
+     friend struct column_dfs_traits;
+}; 
+
+} // end namespace internal
+} // namespace Eigen
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Memory.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Memory.h
new file mode 100644
index 0000000..349bfd5
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Memory.h
@@ -0,0 +1,226 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+/* 
+ 
+ * NOTE: This file is the modified version of [s,d,c,z]memory.c files in SuperLU 
+ 
+ * -- SuperLU routine (version 3.1) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * August 1, 2008
+ *
+ * Copyright (c) 1994 by Xerox Corporation.  All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+
+#ifndef EIGEN_SPARSELU_MEMORY
+#define EIGEN_SPARSELU_MEMORY
+
+namespace Eigen {
+namespace internal {
+  
+enum { LUNoMarker = 3 };
+enum {emptyIdxLU = -1};
+inline Index LUnumTempV(Index& m, Index& w, Index& t, Index& b)
+{
+  return (std::max)(m, (t+b)*w);
+}
+
+template< typename Scalar>
+inline Index LUTempSpace(Index&m, Index& w)
+{
+  return (2*w + 4 + LUNoMarker) * m * sizeof(Index) + (w + 1) * m * sizeof(Scalar);
+}
+
+
+
+
+/** 
+  * Expand the existing storage to accommodate more fill-ins
+  * \param vec Valid pointer to the vector to allocate or expand
+  * \param[in,out] length  At input, contain the current length of the vector that is to be increased. At output, length of the newly allocated vector
+  * \param[in] nbElts Current number of elements in the factors
+  * \param keep_prev  1: use length  and do not expand the vector; 0: compute new_len and expand
+  * \param[in,out] num_expansions Number of times the memory has been expanded
+  */
+template <typename Scalar, typename StorageIndex>
+template <typename VectorType>
+Index  SparseLUImpl<Scalar,StorageIndex>::expand(VectorType& vec, Index& length, Index nbElts, Index keep_prev, Index& num_expansions) 
+{
+  
+  float alpha = 1.5; // Ratio of the memory increase 
+  Index new_len; // New size of the allocated memory
+  
+  if(num_expansions == 0 || keep_prev) 
+    new_len = length ; // First time allocate requested
+  else 
+    new_len = (std::max)(length+1,Index(alpha * length));
+  
+  VectorType old_vec; // Temporary vector to hold the previous values   
+  if (nbElts > 0 )
+    old_vec = vec.segment(0,nbElts); 
+  
+  //Allocate or expand the current vector
+#ifdef EIGEN_EXCEPTIONS
+  try
+#endif
+  {
+    vec.resize(new_len); 
+  }
+#ifdef EIGEN_EXCEPTIONS
+  catch(std::bad_alloc& )
+#else
+  if(!vec.size())
+#endif
+  {
+    if (!num_expansions)
+    {
+      // First time to allocate from LUMemInit()
+      // Let LUMemInit() deals with it.
+      return -1;
+    }
+    if (keep_prev)
+    {
+      // In this case, the memory length should not not be reduced
+      return new_len;
+    }
+    else 
+    {
+      // Reduce the size and increase again 
+      Index tries = 0; // Number of attempts
+      do 
+      {
+        alpha = (alpha + 1)/2;
+        new_len = (std::max)(length+1,Index(alpha * length));
+#ifdef EIGEN_EXCEPTIONS
+        try
+#endif
+        {
+          vec.resize(new_len); 
+        }
+#ifdef EIGEN_EXCEPTIONS
+        catch(std::bad_alloc& )
+#else
+        if (!vec.size())
+#endif
+        {
+          tries += 1; 
+          if ( tries > 10) return new_len; 
+        }
+      } while (!vec.size());
+    }
+  }
+  //Copy the previous values to the newly allocated space 
+  if (nbElts > 0)
+    vec.segment(0, nbElts) = old_vec;   
+   
+  
+  length  = new_len;
+  if(num_expansions) ++num_expansions;
+  return 0; 
+}
+
+/**
+ * \brief  Allocate various working space for the numerical factorization phase.
+ * \param m number of rows of the input matrix 
+ * \param n number of columns 
+ * \param annz number of initial nonzeros in the matrix 
+ * \param lwork  if lwork=-1, this routine returns an estimated size of the required memory
+ * \param glu persistent data to facilitate multiple factors : will be deleted later ??
+ * \param fillratio estimated ratio of fill in the factors
+ * \param panel_size Size of a panel
+ * \return an estimated size of the required memory if lwork = -1; otherwise, return the size of actually allocated memory when allocation failed, and 0 on success
+ * \note Unlike SuperLU, this routine does not support successive factorization with the same pattern and the same row permutation
+ */
+template <typename Scalar, typename StorageIndex>
+Index SparseLUImpl<Scalar,StorageIndex>::memInit(Index m, Index n, Index annz, Index lwork, Index fillratio, Index panel_size,  GlobalLU_t& glu)
+{
+  Index& num_expansions = glu.num_expansions; //No memory expansions so far
+  num_expansions = 0;
+  glu.nzumax = glu.nzlumax = (std::min)(fillratio * (annz+1) / n, m) * n; // estimated number of nonzeros in U 
+  glu.nzlmax = (std::max)(Index(4), fillratio) * (annz+1) / 4; // estimated  nnz in L factor
+  // Return the estimated size to the user if necessary
+  Index tempSpace;
+  tempSpace = (2*panel_size + 4 + LUNoMarker) * m * sizeof(Index) + (panel_size + 1) * m * sizeof(Scalar);
+  if (lwork == emptyIdxLU) 
+  {
+    Index estimated_size;
+    estimated_size = (5 * n + 5) * sizeof(Index)  + tempSpace
+                    + (glu.nzlmax + glu.nzumax) * sizeof(Index) + (glu.nzlumax+glu.nzumax) *  sizeof(Scalar) + n; 
+    return estimated_size;
+  }
+  
+  // Setup the required space 
+  
+  // First allocate Integer pointers for L\U factors
+  glu.xsup.resize(n+1);
+  glu.supno.resize(n+1);
+  glu.xlsub.resize(n+1);
+  glu.xlusup.resize(n+1);
+  glu.xusub.resize(n+1);
+
+  // Reserve memory for L/U factors
+  do 
+  {
+    if(     (expand<ScalarVector>(glu.lusup, glu.nzlumax, 0, 0, num_expansions)<0)
+        ||  (expand<ScalarVector>(glu.ucol,  glu.nzumax,  0, 0, num_expansions)<0)
+        ||  (expand<IndexVector> (glu.lsub,  glu.nzlmax,  0, 0, num_expansions)<0)
+        ||  (expand<IndexVector> (glu.usub,  glu.nzumax,  0, 1, num_expansions)<0) )
+    {
+      //Reduce the estimated size and retry
+      glu.nzlumax /= 2;
+      glu.nzumax /= 2;
+      glu.nzlmax /= 2;
+      if (glu.nzlumax < annz ) return glu.nzlumax; 
+    }
+  } while (!glu.lusup.size() || !glu.ucol.size() || !glu.lsub.size() || !glu.usub.size());
+  
+  ++num_expansions;
+  return 0;
+  
+} // end LuMemInit
+
+/** 
+ * \brief Expand the existing storage 
+ * \param vec vector to expand 
+ * \param[in,out] maxlen On input, previous size of vec (Number of elements to copy ). on output, new size
+ * \param nbElts current number of elements in the vector.
+ * \param memtype Type of the element to expand
+ * \param num_expansions Number of expansions 
+ * \return 0 on success, > 0 size of the memory allocated so far
+ */
+template <typename Scalar, typename StorageIndex>
+template <typename VectorType>
+Index SparseLUImpl<Scalar,StorageIndex>::memXpand(VectorType& vec, Index& maxlen, Index nbElts, MemType memtype, Index& num_expansions)
+{
+  Index failed_size; 
+  if (memtype == USUB)
+     failed_size = this->expand<VectorType>(vec, maxlen, nbElts, 1, num_expansions);
+  else
+    failed_size = this->expand<VectorType>(vec, maxlen, nbElts, 0, num_expansions);
+
+  if (failed_size)
+    return failed_size; 
+  
+  return 0 ;  
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
+#endif // EIGEN_SPARSELU_MEMORY
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Structs.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Structs.h
new file mode 100644
index 0000000..cf5ec44
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Structs.h
@@ -0,0 +1,110 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+/* 
+ * NOTE: This file comes from a partly modified version of files slu_[s,d,c,z]defs.h
+ * -- SuperLU routine (version 4.1) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * November, 2010
+ * 
+ * Global data structures used in LU factorization -
+ * 
+ *   nsuper: #supernodes = nsuper + 1, numbered [0, nsuper].
+ *   (xsup,supno): supno[i] is the supernode no to which i belongs;
+ *  xsup(s) points to the beginning of the s-th supernode.
+ *  e.g.   supno 0 1 2 2 3 3 3 4 4 4 4 4   (n=12)
+ *          xsup 0 1 2 4 7 12
+ *  Note: dfs will be performed on supernode rep. relative to the new 
+ *        row pivoting ordering
+ *
+ *   (xlsub,lsub): lsub[*] contains the compressed subscript of
+ *  rectangular supernodes; xlsub[j] points to the starting
+ *  location of the j-th column in lsub[*]. Note that xlsub 
+ *  is indexed by column.
+ *  Storage: original row subscripts
+ *
+ *      During the course of sparse LU factorization, we also use
+ *  (xlsub,lsub) for the purpose of symmetric pruning. For each
+ *  supernode {s,s+1,...,t=s+r} with first column s and last
+ *  column t, the subscript set
+ *    lsub[j], j=xlsub[s], .., xlsub[s+1]-1
+ *  is the structure of column s (i.e. structure of this supernode).
+ *  It is used for the storage of numerical values.
+ *  Furthermore,
+ *    lsub[j], j=xlsub[t], .., xlsub[t+1]-1
+ *  is the structure of the last column t of this supernode.
+ *  It is for the purpose of symmetric pruning. Therefore, the
+ *  structural subscripts can be rearranged without making physical
+ *  interchanges among the numerical values.
+ *
+ *  However, if the supernode has only one column, then we
+ *  only keep one set of subscripts. For any subscript interchange
+ *  performed, similar interchange must be done on the numerical
+ *  values.
+ *
+ *  The last column structures (for pruning) will be removed
+ *  after the numercial LU factorization phase.
+ *
+ *   (xlusup,lusup): lusup[*] contains the numerical values of the
+ *  rectangular supernodes; xlusup[j] points to the starting
+ *  location of the j-th column in storage vector lusup[*]
+ *  Note: xlusup is indexed by column.
+ *  Each rectangular supernode is stored by column-major
+ *  scheme, consistent with Fortran 2-dim array storage.
+ *
+ *   (xusub,ucol,usub): ucol[*] stores the numerical values of
+ *  U-columns outside the rectangular supernodes. The row
+ *  subscript of nonzero ucol[k] is stored in usub[k].
+ *  xusub[i] points to the starting location of column i in ucol.
+ *  Storage: new row subscripts; that is subscripts of PA.
+ */
+
+#ifndef EIGEN_LU_STRUCTS
+#define EIGEN_LU_STRUCTS
+namespace Eigen {
+namespace internal {
+  
+typedef enum {LUSUP, UCOL, LSUB, USUB, LLVL, ULVL} MemType; 
+
+template <typename IndexVector, typename ScalarVector>
+struct LU_GlobalLU_t {
+  typedef typename IndexVector::Scalar StorageIndex; 
+  IndexVector xsup; //First supernode column ... xsup(s) points to the beginning of the s-th supernode
+  IndexVector supno; // Supernode number corresponding to this column (column to supernode mapping)
+  ScalarVector  lusup; // nonzero values of L ordered by columns 
+  IndexVector lsub; // Compressed row indices of L rectangular supernodes. 
+  IndexVector xlusup; // pointers to the beginning of each column in lusup
+  IndexVector xlsub; // pointers to the beginning of each column in lsub
+  Index   nzlmax; // Current max size of lsub
+  Index   nzlumax; // Current max size of lusup
+  ScalarVector  ucol; // nonzero values of U ordered by columns 
+  IndexVector usub; // row indices of U columns in ucol
+  IndexVector xusub; // Pointers to the beginning of each column of U in ucol 
+  Index   nzumax; // Current max size of ucol
+  Index   n; // Number of columns in the matrix  
+  Index   num_expansions; 
+};
+
+// Values to set for performance
+struct perfvalues {
+  Index panel_size; // a panel consists of at most <panel_size> consecutive columns
+  Index relax; // To control degree of relaxing supernodes. If the number of nodes (columns) 
+                // in a subtree of the elimination tree is less than relax, this subtree is considered 
+                // as one supernode regardless of the row structures of those columns
+  Index maxsuper; // The maximum size for a supernode in complete LU
+  Index rowblk; // The minimum row dimension for 2-D blocking to be used;
+  Index colblk; // The minimum column dimension for 2-D blocking to be used;
+  Index fillfactor; // The estimated fills factors for L and U, compared with A
+}; 
+
+} // end namespace internal
+
+} // end namespace Eigen
+#endif // EIGEN_LU_STRUCTS
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h
new file mode 100644
index 0000000..0be293d
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h
@@ -0,0 +1,375 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSELU_SUPERNODAL_MATRIX_H
+#define EIGEN_SPARSELU_SUPERNODAL_MATRIX_H
+
+namespace Eigen {
+namespace internal {
+
+/** \ingroup SparseLU_Module
+ * \brief a class to manipulate the L supernodal factor from the SparseLU factorization
+ * 
+ * This class  contain the data to easily store 
+ * and manipulate the supernodes during the factorization and solution phase of Sparse LU. 
+ * Only the lower triangular matrix has supernodes.
+ * 
+ * NOTE : This class corresponds to the SCformat structure in SuperLU
+ * 
+ */
+/* TODO
+ * InnerIterator as for sparsematrix 
+ * SuperInnerIterator to iterate through all supernodes 
+ * Function for triangular solve
+ */
+template <typename _Scalar, typename _StorageIndex>
+class MappedSuperNodalMatrix
+{
+  public:
+    typedef _Scalar Scalar; 
+    typedef _StorageIndex StorageIndex;
+    typedef Matrix<StorageIndex,Dynamic,1> IndexVector;
+    typedef Matrix<Scalar,Dynamic,1> ScalarVector;
+  public:
+    MappedSuperNodalMatrix()
+    {
+      
+    }
+    MappedSuperNodalMatrix(Index m, Index n,  ScalarVector& nzval, IndexVector& nzval_colptr, IndexVector& rowind,
+             IndexVector& rowind_colptr, IndexVector& col_to_sup, IndexVector& sup_to_col )
+    {
+      setInfos(m, n, nzval, nzval_colptr, rowind, rowind_colptr, col_to_sup, sup_to_col);
+    }
+    
+    ~MappedSuperNodalMatrix()
+    {
+      
+    }
+    /**
+     * Set appropriate pointers for the lower triangular supernodal matrix
+     * These infos are available at the end of the numerical factorization
+     * FIXME This class will be modified such that it can be use in the course 
+     * of the factorization.
+     */
+    void setInfos(Index m, Index n, ScalarVector& nzval, IndexVector& nzval_colptr, IndexVector& rowind,
+             IndexVector& rowind_colptr, IndexVector& col_to_sup, IndexVector& sup_to_col )
+    {
+      m_row = m;
+      m_col = n; 
+      m_nzval = nzval.data(); 
+      m_nzval_colptr = nzval_colptr.data(); 
+      m_rowind = rowind.data(); 
+      m_rowind_colptr = rowind_colptr.data(); 
+      m_nsuper = col_to_sup(n); 
+      m_col_to_sup = col_to_sup.data(); 
+      m_sup_to_col = sup_to_col.data(); 
+    }
+    
+    /**
+     * Number of rows
+     */
+    Index rows() const { return m_row; }
+    
+    /**
+     * Number of columns
+     */
+    Index cols() const { return m_col; }
+    
+    /**
+     * Return the array of nonzero values packed by column
+     * 
+     * The size is nnz
+     */
+    Scalar* valuePtr() {  return m_nzval; }
+    
+    const Scalar* valuePtr() const 
+    {
+      return m_nzval; 
+    }
+    /**
+     * Return the pointers to the beginning of each column in \ref valuePtr()
+     */
+    StorageIndex* colIndexPtr()
+    {
+      return m_nzval_colptr; 
+    }
+    
+    const StorageIndex* colIndexPtr() const
+    {
+      return m_nzval_colptr; 
+    }
+    
+    /**
+     * Return the array of compressed row indices of all supernodes
+     */
+    StorageIndex* rowIndex()  { return m_rowind; }
+    
+    const StorageIndex* rowIndex() const
+    {
+      return m_rowind; 
+    }
+    
+    /**
+     * Return the location in \em rowvaluePtr() which starts each column
+     */
+    StorageIndex* rowIndexPtr() { return m_rowind_colptr; }
+    
+    const StorageIndex* rowIndexPtr() const
+    {
+      return m_rowind_colptr; 
+    }
+    
+    /** 
+     * Return the array of column-to-supernode mapping 
+     */
+    StorageIndex* colToSup()  { return m_col_to_sup; }
+    
+    const StorageIndex* colToSup() const
+    {
+      return m_col_to_sup;       
+    }
+    /**
+     * Return the array of supernode-to-column mapping
+     */
+    StorageIndex* supToCol() { return m_sup_to_col; }
+    
+    const StorageIndex* supToCol() const
+    {
+      return m_sup_to_col;
+    }
+    
+    /**
+     * Return the number of supernodes
+     */
+    Index nsuper() const
+    {
+      return m_nsuper; 
+    }
+    
+    class InnerIterator; 
+    template<typename Dest>
+    void solveInPlace( MatrixBase<Dest>&X) const;
+    template<bool Conjugate, typename Dest>
+    void solveTransposedInPlace( MatrixBase<Dest>&X) const;
+
+    
+      
+      
+    
+  protected:
+    Index m_row; // Number of rows
+    Index m_col; // Number of columns
+    Index m_nsuper; // Number of supernodes
+    Scalar* m_nzval; //array of nonzero values packed by column
+    StorageIndex* m_nzval_colptr; //nzval_colptr[j] Stores the location in nzval[] which starts column j
+    StorageIndex* m_rowind; // Array of compressed row indices of rectangular supernodes
+    StorageIndex* m_rowind_colptr; //rowind_colptr[j] stores the location in rowind[] which starts column j
+    StorageIndex* m_col_to_sup; // col_to_sup[j] is the supernode number to which column j belongs
+    StorageIndex* m_sup_to_col; //sup_to_col[s] points to the starting column of the s-th supernode
+    
+  private :
+};
+
+/**
+  * \brief InnerIterator class to iterate over nonzero values of the current column in the supernodal matrix L
+  * 
+  */
+template<typename Scalar, typename StorageIndex>
+class MappedSuperNodalMatrix<Scalar,StorageIndex>::InnerIterator
+{
+  public:
+     InnerIterator(const MappedSuperNodalMatrix& mat, Index outer)
+      : m_matrix(mat),
+        m_outer(outer),
+        m_supno(mat.colToSup()[outer]),
+        m_idval(mat.colIndexPtr()[outer]),
+        m_startidval(m_idval),
+        m_endidval(mat.colIndexPtr()[outer+1]),
+        m_idrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]]),
+        m_endidrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]+1])
+    {}
+    inline InnerIterator& operator++()
+    { 
+      m_idval++; 
+      m_idrow++;
+      return *this;
+    }
+    inline Scalar value() const { return m_matrix.valuePtr()[m_idval]; }
+    
+    inline Scalar& valueRef() { return const_cast<Scalar&>(m_matrix.valuePtr()[m_idval]); }
+    
+    inline Index index() const { return m_matrix.rowIndex()[m_idrow]; }
+    inline Index row() const { return index(); }
+    inline Index col() const { return m_outer; }
+    
+    inline Index supIndex() const { return m_supno; }
+    
+    inline operator bool() const 
+    { 
+      return ( (m_idval < m_endidval) && (m_idval >= m_startidval)
+                && (m_idrow < m_endidrow) );
+    }
+    
+  protected:
+    const MappedSuperNodalMatrix& m_matrix; // Supernodal lower triangular matrix 
+    const Index m_outer;                    // Current column 
+    const Index m_supno;                    // Current SuperNode number
+    Index m_idval;                          // Index to browse the values in the current column
+    const Index m_startidval;               // Start of the column value
+    const Index m_endidval;                 // End of the column value
+    Index m_idrow;                          // Index to browse the row indices 
+    Index m_endidrow;                       // End index of row indices of the current column
+};
+
+/**
+ * \brief Solve with the supernode triangular matrix
+ * 
+ */
+template<typename Scalar, typename Index_>
+template<typename Dest>
+void MappedSuperNodalMatrix<Scalar,Index_>::solveInPlace( MatrixBase<Dest>&X) const
+{
+    /* Explicit type conversion as the Index type of MatrixBase<Dest> may be wider than Index */
+//    eigen_assert(X.rows() <= NumTraits<Index>::highest());
+//    eigen_assert(X.cols() <= NumTraits<Index>::highest());
+    Index n    = int(X.rows());
+    Index nrhs = Index(X.cols());
+    const Scalar * Lval = valuePtr();                 // Nonzero values 
+    Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor> work(n, nrhs);     // working vector
+    work.setZero();
+    for (Index k = 0; k <= nsuper(); k ++)
+    {
+      Index fsupc = supToCol()[k];                    // First column of the current supernode 
+      Index istart = rowIndexPtr()[fsupc];            // Pointer index to the subscript of the current column
+      Index nsupr = rowIndexPtr()[fsupc+1] - istart;  // Number of rows in the current supernode
+      Index nsupc = supToCol()[k+1] - fsupc;          // Number of columns in the current supernode
+      Index nrow = nsupr - nsupc;                     // Number of rows in the non-diagonal part of the supernode
+      Index irow;                                     //Current index row
+      
+      if (nsupc == 1 )
+      {
+        for (Index j = 0; j < nrhs; j++)
+        {
+          InnerIterator it(*this, fsupc);
+          ++it; // Skip the diagonal element
+          for (; it; ++it)
+          {
+            irow = it.row();
+            X(irow, j) -= X(fsupc, j) * it.value();
+          }
+        }
+      }
+      else
+      {
+        // The supernode has more than one column 
+        Index luptr = colIndexPtr()[fsupc]; 
+        Index lda = colIndexPtr()[fsupc+1] - luptr;
+        
+        // Triangular solve 
+        Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > A( &(Lval[luptr]), nsupc, nsupc, OuterStride<>(lda) );
+        Map< Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor>, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) );
+        U = A.template triangularView<UnitLower>().solve(U); 
+        
+        // Matrix-vector product 
+        new (&A) Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > ( &(Lval[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) );
+        work.topRows(nrow).noalias() = A * U;
+        
+        //Begin Scatter 
+        for (Index j = 0; j < nrhs; j++)
+        {
+          Index iptr = istart + nsupc; 
+          for (Index i = 0; i < nrow; i++)
+          {
+            irow = rowIndex()[iptr]; 
+            X(irow, j) -= work(i, j); // Scatter operation
+            work(i, j) = Scalar(0); 
+            iptr++;
+          }
+        }
+      }
+    } 
+}
+
+template<typename Scalar, typename Index_>
+template<bool Conjugate, typename Dest>
+void MappedSuperNodalMatrix<Scalar,Index_>::solveTransposedInPlace( MatrixBase<Dest>&X) const
+{
+    using numext::conj;
+  Index n    = int(X.rows());
+  Index nrhs = Index(X.cols());
+  const Scalar * Lval = valuePtr();                 // Nonzero values
+  Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor> work(n, nrhs);     // working vector
+  work.setZero();
+  for (Index k = nsuper(); k >= 0; k--)
+  {
+    Index fsupc = supToCol()[k];                    // First column of the current supernode
+    Index istart = rowIndexPtr()[fsupc];            // Pointer index to the subscript of the current column
+    Index nsupr = rowIndexPtr()[fsupc+1] - istart;  // Number of rows in the current supernode
+    Index nsupc = supToCol()[k+1] - fsupc;          // Number of columns in the current supernode
+    Index nrow = nsupr - nsupc;                     // Number of rows in the non-diagonal part of the supernode
+    Index irow;                                     //Current index row
+
+    if (nsupc == 1 )
+    {
+      for (Index j = 0; j < nrhs; j++)
+      {
+        InnerIterator it(*this, fsupc);
+        ++it; // Skip the diagonal element
+        for (; it; ++it)
+        {
+          irow = it.row();
+          X(fsupc,j) -= X(irow, j) * (Conjugate?conj(it.value()):it.value());
+        }
+      }
+    }
+    else
+    {
+      // The supernode has more than one column
+      Index luptr = colIndexPtr()[fsupc];
+      Index lda = colIndexPtr()[fsupc+1] - luptr;
+
+      //Begin Gather
+      for (Index j = 0; j < nrhs; j++)
+      {
+        Index iptr = istart + nsupc;
+        for (Index i = 0; i < nrow; i++)
+        {
+          irow = rowIndex()[iptr];
+          work.topRows(nrow)(i,j)= X(irow,j); // Gather operation
+          iptr++;
+        }
+      }
+
+      // Matrix-vector product with transposed submatrix
+      Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > A( &(Lval[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) );
+      Map< Matrix<Scalar,Dynamic,Dest::ColsAtCompileTime, ColMajor>, 0, OuterStride<> > U (&(X(fsupc,0)), nsupc, nrhs, OuterStride<>(n) );
+      if(Conjugate)
+        U = U - A.adjoint() * work.topRows(nrow);
+      else
+        U = U - A.transpose() * work.topRows(nrow);
+
+      // Triangular solve (of transposed diagonal block)
+      new (&A) Map<const Matrix<Scalar,Dynamic,Dynamic, ColMajor>, 0, OuterStride<> > ( &(Lval[luptr]), nsupc, nsupc, OuterStride<>(lda) );
+      if(Conjugate)
+        U = A.adjoint().template triangularView<UnitUpper>().solve(U);
+      else
+        U = A.transpose().template triangularView<UnitUpper>().solve(U);
+
+    }
+
+  }
+}
+
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_SPARSELU_MATRIX_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Utils.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Utils.h
new file mode 100644
index 0000000..9e3dab4
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_Utils.h
@@ -0,0 +1,80 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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_SPARSELU_UTILS_H
+#define EIGEN_SPARSELU_UTILS_H
+
+namespace Eigen {
+namespace internal {
+
+/**
+ * \brief Count Nonzero elements in the factors
+ */
+template <typename Scalar, typename StorageIndex>
+void SparseLUImpl<Scalar,StorageIndex>::countnz(const Index n, Index& nnzL, Index& nnzU, GlobalLU_t& glu)
+{
+ nnzL = 0; 
+ nnzU = (glu.xusub)(n); 
+ Index nsuper = (glu.supno)(n); 
+ Index jlen; 
+ Index i, j, fsupc;
+ if (n <= 0 ) return; 
+ // For each supernode
+ for (i = 0; i <= nsuper; i++)
+ {
+   fsupc = glu.xsup(i); 
+   jlen = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); 
+   
+   for (j = fsupc; j < glu.xsup(i+1); j++)
+   {
+     nnzL += jlen; 
+     nnzU += j - fsupc + 1; 
+     jlen--; 
+   }
+ }
+}
+
+/**
+ * \brief Fix up the data storage lsub for L-subscripts. 
+ * 
+ * It removes the subscripts sets for structural pruning, 
+ * and applies permutation to the remaining subscripts
+ * 
+ */
+template <typename Scalar, typename StorageIndex>
+void SparseLUImpl<Scalar,StorageIndex>::fixupL(const Index n, const IndexVector& perm_r, GlobalLU_t& glu)
+{
+  Index fsupc, i, j, k, jstart; 
+  
+  StorageIndex nextl = 0; 
+  Index nsuper = (glu.supno)(n); 
+  
+  // For each supernode 
+  for (i = 0; i <= nsuper; i++)
+  {
+    fsupc = glu.xsup(i); 
+    jstart = glu.xlsub(fsupc); 
+    glu.xlsub(fsupc) = nextl; 
+    for (j = jstart; j < glu.xlsub(fsupc + 1); j++)
+    {
+      glu.lsub(nextl) = perm_r(glu.lsub(j)); // Now indexed into P*A
+      nextl++;
+    }
+    for (k = fsupc+1; k < glu.xsup(i+1); k++)
+      glu.xlsub(k) = nextl; // other columns in supernode i
+  }
+  
+  glu.xlsub(n) = nextl; 
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
+#endif // EIGEN_SPARSELU_UTILS_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_bmod.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_bmod.h
new file mode 100644
index 0000000..b57f068
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_bmod.h
@@ -0,0 +1,181 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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/.
+
+/* 
+ 
+ * NOTE: This file is the modified version of xcolumn_bmod.c file in SuperLU 
+ 
+ * -- SuperLU routine (version 3.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * October 15, 2003
+ *
+ * Copyright (c) 1994 by Xerox Corporation.  All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+#ifndef SPARSELU_COLUMN_BMOD_H
+#define SPARSELU_COLUMN_BMOD_H
+
+namespace Eigen {
+
+namespace internal {
+/**
+ * \brief Performs numeric block updates (sup-col) in topological order
+ * 
+ * \param jcol current column to update
+ * \param nseg Number of segments in the U part
+ * \param dense Store the full representation of the column
+ * \param tempv working array 
+ * \param segrep segment representative ...
+ * \param repfnz ??? First nonzero column in each row ???  ...
+ * \param fpanelc First column in the current panel
+ * \param glu Global LU data. 
+ * \return 0 - successful return 
+ *         > 0 - number of bytes allocated when run out of space
+ * 
+ */
+template <typename Scalar, typename StorageIndex>
+Index SparseLUImpl<Scalar,StorageIndex>::column_bmod(const Index jcol, const Index nseg, BlockScalarVector dense, ScalarVector& tempv,
+                                                     BlockIndexVector segrep, BlockIndexVector repfnz, Index fpanelc, GlobalLU_t& glu)
+{
+  Index  jsupno, k, ksub, krep, ksupno; 
+  Index lptr, nrow, isub, irow, nextlu, new_next, ufirst; 
+  Index fsupc, nsupc, nsupr, luptr, kfnz, no_zeros; 
+  /* krep = representative of current k-th supernode
+    * fsupc =  first supernodal column
+    * nsupc = number of columns in a supernode
+    * nsupr = number of rows in a supernode
+    * luptr = location of supernodal LU-block in storage
+    * kfnz = first nonz in the k-th supernodal segment
+    * no_zeros = no lf leading zeros in a supernodal U-segment
+    */
+  
+  jsupno = glu.supno(jcol);
+  // For each nonzero supernode segment of U[*,j] in topological order 
+  k = nseg - 1; 
+  Index d_fsupc; // distance between the first column of the current panel and the 
+               // first column of the current snode
+  Index fst_col; // First column within small LU update
+  Index segsize; 
+  for (ksub = 0; ksub < nseg; ksub++)
+  {
+    krep = segrep(k); k--; 
+    ksupno = glu.supno(krep); 
+    if (jsupno != ksupno )
+    {
+      // outside the rectangular supernode 
+      fsupc = glu.xsup(ksupno); 
+      fst_col = (std::max)(fsupc, fpanelc); 
+      
+      // Distance from the current supernode to the current panel; 
+      // d_fsupc = 0 if fsupc > fpanelc
+      d_fsupc = fst_col - fsupc; 
+      
+      luptr = glu.xlusup(fst_col) + d_fsupc; 
+      lptr = glu.xlsub(fsupc) + d_fsupc; 
+      
+      kfnz = repfnz(krep); 
+      kfnz = (std::max)(kfnz, fpanelc); 
+      
+      segsize = krep - kfnz + 1; 
+      nsupc = krep - fst_col + 1; 
+      nsupr = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); 
+      nrow = nsupr - d_fsupc - nsupc;
+      Index lda = glu.xlusup(fst_col+1) - glu.xlusup(fst_col);
+      
+      
+      // Perform a triangular solver and block update, 
+      // then scatter the result of sup-col update to dense
+      no_zeros = kfnz - fst_col; 
+      if(segsize==1)
+        LU_kernel_bmod<1>::run(segsize, dense, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
+      else
+        LU_kernel_bmod<Dynamic>::run(segsize, dense, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
+    } // end if jsupno 
+  } // end for each segment
+  
+  // Process the supernodal portion of  L\U[*,j]
+  nextlu = glu.xlusup(jcol); 
+  fsupc = glu.xsup(jsupno);
+  
+  // copy the SPA dense into L\U[*,j]
+  Index mem; 
+  new_next = nextlu + glu.xlsub(fsupc + 1) - glu.xlsub(fsupc); 
+  Index offset = internal::first_multiple<Index>(new_next, internal::packet_traits<Scalar>::size) - new_next;
+  if(offset)
+    new_next += offset;
+  while (new_next > glu.nzlumax )
+  {
+    mem = memXpand<ScalarVector>(glu.lusup, glu.nzlumax, nextlu, LUSUP, glu.num_expansions);  
+    if (mem) return mem; 
+  }
+  
+  for (isub = glu.xlsub(fsupc); isub < glu.xlsub(fsupc+1); isub++)
+  {
+    irow = glu.lsub(isub);
+    glu.lusup(nextlu) = dense(irow);
+    dense(irow) = Scalar(0.0); 
+    ++nextlu; 
+  }
+  
+  if(offset)
+  {
+    glu.lusup.segment(nextlu,offset).setZero();
+    nextlu += offset;
+  }
+  glu.xlusup(jcol + 1) = StorageIndex(nextlu);  // close L\U(*,jcol); 
+  
+  /* For more updates within the panel (also within the current supernode),
+   * should start from the first column of the panel, or the first column
+   * of the supernode, whichever is bigger. There are two cases:
+   *  1) fsupc < fpanelc, then fst_col <-- fpanelc
+   *  2) fsupc >= fpanelc, then fst_col <-- fsupc
+   */
+  fst_col = (std::max)(fsupc, fpanelc); 
+  
+  if (fst_col  < jcol)
+  {
+    // Distance between the current supernode and the current panel
+    // d_fsupc = 0 if fsupc >= fpanelc
+    d_fsupc = fst_col - fsupc; 
+    
+    lptr = glu.xlsub(fsupc) + d_fsupc; 
+    luptr = glu.xlusup(fst_col) + d_fsupc; 
+    nsupr = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); // leading dimension
+    nsupc = jcol - fst_col; // excluding jcol 
+    nrow = nsupr - d_fsupc - nsupc; 
+    
+    // points to the beginning of jcol in snode L\U(jsupno) 
+    ufirst = glu.xlusup(jcol) + d_fsupc; 
+    Index lda = glu.xlusup(jcol+1) - glu.xlusup(jcol);
+    MappedMatrixBlock A( &(glu.lusup.data()[luptr]), nsupc, nsupc, OuterStride<>(lda) );
+    VectorBlock<ScalarVector> u(glu.lusup, ufirst, nsupc); 
+    u = A.template triangularView<UnitLower>().solve(u); 
+    
+    new (&A) MappedMatrixBlock ( &(glu.lusup.data()[luptr+nsupc]), nrow, nsupc, OuterStride<>(lda) );
+    VectorBlock<ScalarVector> l(glu.lusup, ufirst+nsupc, nrow); 
+    l.noalias() -= A * u;
+    
+  } // End if fst_col
+  return 0; 
+}
+
+} // end namespace internal
+} // end namespace Eigen
+
+#endif // SPARSELU_COLUMN_BMOD_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h
new file mode 100644
index 0000000..5a2c941
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_column_dfs.h
@@ -0,0 +1,179 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+/* 
+ 
+ * NOTE: This file is the modified version of [s,d,c,z]column_dfs.c file in SuperLU 
+ 
+ * -- SuperLU routine (version 2.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * November 15, 1997
+ *
+ * Copyright (c) 1994 by Xerox Corporation.  All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+#ifndef SPARSELU_COLUMN_DFS_H
+#define SPARSELU_COLUMN_DFS_H
+
+template <typename Scalar, typename StorageIndex> class SparseLUImpl;
+namespace Eigen {
+
+namespace internal {
+
+template<typename IndexVector, typename ScalarVector>
+struct column_dfs_traits : no_assignment_operator
+{
+  typedef typename ScalarVector::Scalar Scalar;
+  typedef typename IndexVector::Scalar StorageIndex;
+  column_dfs_traits(Index jcol, Index& jsuper, typename SparseLUImpl<Scalar, StorageIndex>::GlobalLU_t& glu, SparseLUImpl<Scalar, StorageIndex>& luImpl)
+   : m_jcol(jcol), m_jsuper_ref(jsuper), m_glu(glu), m_luImpl(luImpl)
+ {}
+  bool update_segrep(Index /*krep*/, Index /*jj*/)
+  {
+    return true;
+  }
+  void mem_expand(IndexVector& lsub, Index& nextl, Index chmark)
+  {
+    if (nextl >= m_glu.nzlmax)
+      m_luImpl.memXpand(lsub, m_glu.nzlmax, nextl, LSUB, m_glu.num_expansions); 
+    if (chmark != (m_jcol-1)) m_jsuper_ref = emptyIdxLU;
+  }
+  enum { ExpandMem = true };
+  
+  Index m_jcol;
+  Index& m_jsuper_ref;
+  typename SparseLUImpl<Scalar, StorageIndex>::GlobalLU_t& m_glu;
+  SparseLUImpl<Scalar, StorageIndex>& m_luImpl;
+};
+
+
+/**
+ * \brief Performs a symbolic factorization on column jcol and decide the supernode boundary
+ * 
+ * A supernode representative is the last column of a supernode.
+ * The nonzeros in U[*,j] are segments that end at supernodes representatives. 
+ * The routine returns a list of the supernodal representatives 
+ * in topological order of the dfs that generates them. 
+ * The location of the first nonzero in each supernodal segment 
+ * (supernodal entry location) is also returned. 
+ * 
+ * \param m number of rows in the matrix
+ * \param jcol Current column 
+ * \param perm_r Row permutation
+ * \param maxsuper  Maximum number of column allowed in a supernode
+ * \param [in,out] nseg Number of segments in current U[*,j] - new segments appended
+ * \param lsub_col defines the rhs vector to start the dfs
+ * \param [in,out] segrep Segment representatives - new segments appended 
+ * \param repfnz  First nonzero location in each row
+ * \param xprune 
+ * \param marker  marker[i] == jj, if i was visited during dfs of current column jj;
+ * \param parent
+ * \param xplore working array
+ * \param glu global LU data 
+ * \return 0 success
+ *         > 0 number of bytes allocated when run out of space
+ * 
+ */
+template <typename Scalar, typename StorageIndex>
+Index SparseLUImpl<Scalar,StorageIndex>::column_dfs(const Index m, const Index jcol, IndexVector& perm_r, Index maxsuper, Index& nseg,
+                                                    BlockIndexVector lsub_col, IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune,
+                                                    IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu)
+{
+  
+  Index jsuper = glu.supno(jcol); 
+  Index nextl = glu.xlsub(jcol); 
+  VectorBlock<IndexVector> marker2(marker, 2*m, m); 
+  
+  
+  column_dfs_traits<IndexVector, ScalarVector> traits(jcol, jsuper, glu, *this);
+  
+  // For each nonzero in A(*,jcol) do dfs 
+  for (Index k = 0; ((k < m) ? lsub_col[k] != emptyIdxLU : false) ; k++)
+  {
+    Index krow = lsub_col(k); 
+    lsub_col(k) = emptyIdxLU; 
+    Index kmark = marker2(krow); 
+    
+    // krow was visited before, go to the next nonz; 
+    if (kmark == jcol) continue;
+    
+    dfs_kernel(StorageIndex(jcol), perm_r, nseg, glu.lsub, segrep, repfnz, xprune, marker2, parent,
+                   xplore, glu, nextl, krow, traits);
+  } // for each nonzero ... 
+  
+  Index fsupc;
+  StorageIndex nsuper = glu.supno(jcol);
+  StorageIndex jcolp1 = StorageIndex(jcol) + 1;
+  Index jcolm1 = jcol - 1;
+  
+  // check to see if j belongs in the same supernode as j-1
+  if ( jcol == 0 )
+  { // Do nothing for column 0 
+    nsuper = glu.supno(0) = 0 ;
+  }
+  else 
+  {
+    fsupc = glu.xsup(nsuper); 
+    StorageIndex jptr = glu.xlsub(jcol); // Not yet compressed
+    StorageIndex jm1ptr = glu.xlsub(jcolm1); 
+    
+    // Use supernodes of type T2 : see SuperLU paper
+    if ( (nextl-jptr != jptr-jm1ptr-1) ) jsuper = emptyIdxLU;
+    
+    // Make sure the number of columns in a supernode doesn't
+    // exceed threshold
+    if ( (jcol - fsupc) >= maxsuper) jsuper = emptyIdxLU; 
+    
+    /* If jcol starts a new supernode, reclaim storage space in
+     * glu.lsub from previous supernode. Note we only store 
+     * the subscript set of the first and last columns of 
+     * a supernode. (first for num values, last for pruning)
+     */
+    if (jsuper == emptyIdxLU)
+    { // starts a new supernode 
+      if ( (fsupc < jcolm1-1) ) 
+      { // >= 3 columns in nsuper
+        StorageIndex ito = glu.xlsub(fsupc+1);
+        glu.xlsub(jcolm1) = ito; 
+        StorageIndex istop = ito + jptr - jm1ptr; 
+        xprune(jcolm1) = istop; // initialize xprune(jcol-1)
+        glu.xlsub(jcol) = istop; 
+        
+        for (StorageIndex ifrom = jm1ptr; ifrom < nextl; ++ifrom, ++ito)
+          glu.lsub(ito) = glu.lsub(ifrom); 
+        nextl = ito;  // = istop + length(jcol)
+      }
+      nsuper++; 
+      glu.supno(jcol) = nsuper; 
+    } // if a new supernode 
+  } // end else:  jcol > 0
+  
+  // Tidy up the pointers before exit
+  glu.xsup(nsuper+1) = jcolp1; 
+  glu.supno(jcolp1) = nsuper; 
+  xprune(jcol) = StorageIndex(nextl);  // Initialize upper bound for pruning
+  glu.xlsub(jcolp1) = StorageIndex(nextl); 
+  
+  return 0; 
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h
new file mode 100644
index 0000000..c32d8d8
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h
@@ -0,0 +1,107 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+/* 
+ 
+ * NOTE: This file is the modified version of [s,d,c,z]copy_to_ucol.c file in SuperLU 
+ 
+ * -- SuperLU routine (version 2.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * November 15, 1997
+ *
+ * Copyright (c) 1994 by Xerox Corporation.  All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+#ifndef SPARSELU_COPY_TO_UCOL_H
+#define SPARSELU_COPY_TO_UCOL_H
+
+namespace Eigen {
+namespace internal {
+
+/**
+ * \brief Performs numeric block updates (sup-col) in topological order
+ * 
+ * \param jcol current column to update
+ * \param nseg Number of segments in the U part
+ * \param segrep segment representative ...
+ * \param repfnz First nonzero column in each row  ...
+ * \param perm_r Row permutation 
+ * \param dense Store the full representation of the column
+ * \param glu Global LU data. 
+ * \return 0 - successful return 
+ *         > 0 - number of bytes allocated when run out of space
+ * 
+ */
+template <typename Scalar, typename StorageIndex>
+Index SparseLUImpl<Scalar,StorageIndex>::copy_to_ucol(const Index jcol, const Index nseg, IndexVector& segrep,
+                                                      BlockIndexVector repfnz ,IndexVector& perm_r, BlockScalarVector dense, GlobalLU_t& glu)
+{  
+  Index ksub, krep, ksupno; 
+    
+  Index jsupno = glu.supno(jcol);
+  
+  // For each nonzero supernode segment of U[*,j] in topological order 
+  Index k = nseg - 1, i; 
+  StorageIndex nextu = glu.xusub(jcol); 
+  Index kfnz, isub, segsize; 
+  Index new_next,irow; 
+  Index fsupc, mem; 
+  for (ksub = 0; ksub < nseg; ksub++)
+  {
+    krep = segrep(k); k--; 
+    ksupno = glu.supno(krep); 
+    if (jsupno != ksupno ) // should go into ucol(); 
+    {
+      kfnz = repfnz(krep); 
+      if (kfnz != emptyIdxLU)
+      { // Nonzero U-segment 
+        fsupc = glu.xsup(ksupno); 
+        isub = glu.xlsub(fsupc) + kfnz - fsupc; 
+        segsize = krep - kfnz + 1; 
+        new_next = nextu + segsize; 
+        while (new_next > glu.nzumax) 
+        {
+          mem = memXpand<ScalarVector>(glu.ucol, glu.nzumax, nextu, UCOL, glu.num_expansions); 
+          if (mem) return mem; 
+          mem = memXpand<IndexVector>(glu.usub, glu.nzumax, nextu, USUB, glu.num_expansions); 
+          if (mem) return mem; 
+          
+        }
+        
+        for (i = 0; i < segsize; i++)
+        {
+          irow = glu.lsub(isub); 
+          glu.usub(nextu) = perm_r(irow); // Unlike the L part, the U part is stored in its final order
+          glu.ucol(nextu) = dense(irow); 
+          dense(irow) = Scalar(0.0); 
+          nextu++;
+          isub++;
+        }
+        
+      } // end nonzero U-segment 
+      
+    } // end if jsupno 
+    
+  } // end for each segment
+  glu.xusub(jcol + 1) = nextu; // close U(*,jcol)
+  return 0; 
+}
+
+} // namespace internal
+} // end namespace Eigen
+
+#endif // SPARSELU_COPY_TO_UCOL_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_gemm_kernel.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_gemm_kernel.h
new file mode 100644
index 0000000..e37c2fe
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_gemm_kernel.h
@@ -0,0 +1,280 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSELU_GEMM_KERNEL_H
+#define EIGEN_SPARSELU_GEMM_KERNEL_H
+
+namespace Eigen {
+
+namespace internal {
+
+
+/** \internal
+  * A general matrix-matrix product kernel optimized for the SparseLU factorization.
+  *  - A, B, and C must be column major
+  *  - lda and ldc must be multiples of the respective packet size
+  *  - C must have the same alignment as A
+  */
+template<typename Scalar>
+EIGEN_DONT_INLINE
+void sparselu_gemm(Index m, Index n, Index d, const Scalar* A, Index lda, const Scalar* B, Index ldb, Scalar* C, Index ldc)
+{
+  using namespace Eigen::internal;
+  
+  typedef typename packet_traits<Scalar>::type Packet;
+  enum {
+    NumberOfRegisters = EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS,
+    PacketSize = packet_traits<Scalar>::size,
+    PM = 8,                             // peeling in M
+    RN = 2,                             // register blocking
+    RK = NumberOfRegisters>=16 ? 4 : 2, // register blocking
+    BM = 4096/sizeof(Scalar),           // number of rows of A-C per chunk
+    SM = PM*PacketSize                  // step along M
+  };
+  Index d_end = (d/RK)*RK;    // number of columns of A (rows of B) suitable for full register blocking
+  Index n_end = (n/RN)*RN;    // number of columns of B-C suitable for processing RN columns at once
+  Index i0 = internal::first_default_aligned(A,m);
+  
+  eigen_internal_assert(((lda%PacketSize)==0) && ((ldc%PacketSize)==0) && (i0==internal::first_default_aligned(C,m)));
+  
+  // handle the non aligned rows of A and C without any optimization:
+  for(Index i=0; i<i0; ++i)
+  {
+    for(Index j=0; j<n; ++j)
+    {
+      Scalar c = C[i+j*ldc];
+      for(Index k=0; k<d; ++k)
+        c += B[k+j*ldb] * A[i+k*lda];
+      C[i+j*ldc] = c;
+    }
+  }
+  // process the remaining rows per chunk of BM rows
+  for(Index ib=i0; ib<m; ib+=BM)
+  {
+    Index actual_b = std::min<Index>(BM, m-ib);                 // actual number of rows
+    Index actual_b_end1 = (actual_b/SM)*SM;                   // actual number of rows suitable for peeling
+    Index actual_b_end2 = (actual_b/PacketSize)*PacketSize;   // actual number of rows suitable for vectorization
+    
+    // Let's process two columns of B-C at once
+    for(Index j=0; j<n_end; j+=RN)
+    {
+      const Scalar* Bc0 = B+(j+0)*ldb;
+      const Scalar* Bc1 = B+(j+1)*ldb;
+      
+      for(Index k=0; k<d_end; k+=RK)
+      {
+        
+        // load and expand a RN x RK block of B
+        Packet b00, b10, b20, b30, b01, b11, b21, b31;
+                  { b00 = pset1<Packet>(Bc0[0]); }
+                  { b10 = pset1<Packet>(Bc0[1]); }
+        if(RK==4) { b20 = pset1<Packet>(Bc0[2]); }
+        if(RK==4) { b30 = pset1<Packet>(Bc0[3]); }
+                  { b01 = pset1<Packet>(Bc1[0]); }
+                  { b11 = pset1<Packet>(Bc1[1]); }
+        if(RK==4) { b21 = pset1<Packet>(Bc1[2]); }
+        if(RK==4) { b31 = pset1<Packet>(Bc1[3]); }
+        
+        Packet a0, a1, a2, a3, c0, c1, t0, t1;
+        
+        const Scalar* A0 = A+ib+(k+0)*lda;
+        const Scalar* A1 = A+ib+(k+1)*lda;
+        const Scalar* A2 = A+ib+(k+2)*lda;
+        const Scalar* A3 = A+ib+(k+3)*lda;
+        
+        Scalar* C0 = C+ib+(j+0)*ldc;
+        Scalar* C1 = C+ib+(j+1)*ldc;
+        
+                  a0 = pload<Packet>(A0);
+                  a1 = pload<Packet>(A1);
+        if(RK==4)
+        {
+          a2 = pload<Packet>(A2);
+          a3 = pload<Packet>(A3);
+        }
+        else
+        {
+          // workaround "may be used uninitialized in this function" warning
+          a2 = a3 = a0;
+        }
+        
+#define KMADD(c, a, b, tmp) {tmp = b; tmp = pmul(a,tmp); c = padd(c,tmp);}
+#define WORK(I)  \
+                     c0 = pload<Packet>(C0+i+(I)*PacketSize);    \
+                     c1 = pload<Packet>(C1+i+(I)*PacketSize);    \
+                     KMADD(c0, a0, b00, t0)                      \
+                     KMADD(c1, a0, b01, t1)                      \
+                     a0 = pload<Packet>(A0+i+(I+1)*PacketSize);  \
+                     KMADD(c0, a1, b10, t0)                      \
+                     KMADD(c1, a1, b11, t1)                      \
+                     a1 = pload<Packet>(A1+i+(I+1)*PacketSize);  \
+          if(RK==4){ KMADD(c0, a2, b20, t0)                     }\
+          if(RK==4){ KMADD(c1, a2, b21, t1)                     }\
+          if(RK==4){ a2 = pload<Packet>(A2+i+(I+1)*PacketSize); }\
+          if(RK==4){ KMADD(c0, a3, b30, t0)                     }\
+          if(RK==4){ KMADD(c1, a3, b31, t1)                     }\
+          if(RK==4){ a3 = pload<Packet>(A3+i+(I+1)*PacketSize); }\
+                     pstore(C0+i+(I)*PacketSize, c0);            \
+                     pstore(C1+i+(I)*PacketSize, c1)
+        
+        // process rows of A' - C' with aggressive vectorization and peeling 
+        for(Index i=0; i<actual_b_end1; i+=PacketSize*8)
+        {
+          EIGEN_ASM_COMMENT("SPARSELU_GEMML_KERNEL1");
+                    prefetch((A0+i+(5)*PacketSize));
+                    prefetch((A1+i+(5)*PacketSize));
+          if(RK==4) prefetch((A2+i+(5)*PacketSize));
+          if(RK==4) prefetch((A3+i+(5)*PacketSize));
+
+          WORK(0);
+          WORK(1);
+          WORK(2);
+          WORK(3);
+          WORK(4);
+          WORK(5);
+          WORK(6);
+          WORK(7);
+        }
+        // process the remaining rows with vectorization only
+        for(Index i=actual_b_end1; i<actual_b_end2; i+=PacketSize)
+        {
+          WORK(0);
+        }
+#undef WORK
+        // process the remaining rows without vectorization
+        for(Index i=actual_b_end2; i<actual_b; ++i)
+        {
+          if(RK==4)
+          {
+            C0[i] += A0[i]*Bc0[0]+A1[i]*Bc0[1]+A2[i]*Bc0[2]+A3[i]*Bc0[3];
+            C1[i] += A0[i]*Bc1[0]+A1[i]*Bc1[1]+A2[i]*Bc1[2]+A3[i]*Bc1[3];
+          }
+          else
+          {
+            C0[i] += A0[i]*Bc0[0]+A1[i]*Bc0[1];
+            C1[i] += A0[i]*Bc1[0]+A1[i]*Bc1[1];
+          }
+        }
+        
+        Bc0 += RK;
+        Bc1 += RK;
+      } // peeled loop on k
+    } // peeled loop on the columns j
+    // process the last column (we now perform a matrix-vector product)
+    if((n-n_end)>0)
+    {
+      const Scalar* Bc0 = B+(n-1)*ldb;
+      
+      for(Index k=0; k<d_end; k+=RK)
+      {
+        
+        // load and expand a 1 x RK block of B
+        Packet b00, b10, b20, b30;
+                  b00 = pset1<Packet>(Bc0[0]);
+                  b10 = pset1<Packet>(Bc0[1]);
+        if(RK==4) b20 = pset1<Packet>(Bc0[2]);
+        if(RK==4) b30 = pset1<Packet>(Bc0[3]);
+        
+        Packet a0, a1, a2, a3, c0, t0/*, t1*/;
+        
+        const Scalar* A0 = A+ib+(k+0)*lda;
+        const Scalar* A1 = A+ib+(k+1)*lda;
+        const Scalar* A2 = A+ib+(k+2)*lda;
+        const Scalar* A3 = A+ib+(k+3)*lda;
+        
+        Scalar* C0 = C+ib+(n_end)*ldc;
+        
+                  a0 = pload<Packet>(A0);
+                  a1 = pload<Packet>(A1);
+        if(RK==4)
+        {
+          a2 = pload<Packet>(A2);
+          a3 = pload<Packet>(A3);
+        }
+        else
+        {
+          // workaround "may be used uninitialized in this function" warning
+          a2 = a3 = a0;
+        }
+        
+#define WORK(I) \
+                   c0 = pload<Packet>(C0+i+(I)*PacketSize);     \
+                   KMADD(c0, a0, b00, t0)                       \
+                   a0 = pload<Packet>(A0+i+(I+1)*PacketSize);   \
+                   KMADD(c0, a1, b10, t0)                       \
+                   a1 = pload<Packet>(A1+i+(I+1)*PacketSize);   \
+        if(RK==4){ KMADD(c0, a2, b20, t0)                      }\
+        if(RK==4){ a2 = pload<Packet>(A2+i+(I+1)*PacketSize);  }\
+        if(RK==4){ KMADD(c0, a3, b30, t0)                      }\
+        if(RK==4){ a3 = pload<Packet>(A3+i+(I+1)*PacketSize);  }\
+                   pstore(C0+i+(I)*PacketSize, c0);
+        
+        // aggressive vectorization and peeling
+        for(Index i=0; i<actual_b_end1; i+=PacketSize*8)
+        {
+          EIGEN_ASM_COMMENT("SPARSELU_GEMML_KERNEL2");
+          WORK(0);
+          WORK(1);
+          WORK(2);
+          WORK(3);
+          WORK(4);
+          WORK(5);
+          WORK(6);
+          WORK(7);
+        }
+        // vectorization only
+        for(Index i=actual_b_end1; i<actual_b_end2; i+=PacketSize)
+        {
+          WORK(0);
+        }
+        // remaining scalars
+        for(Index i=actual_b_end2; i<actual_b; ++i)
+        {
+          if(RK==4) 
+            C0[i] += A0[i]*Bc0[0]+A1[i]*Bc0[1]+A2[i]*Bc0[2]+A3[i]*Bc0[3];
+          else
+            C0[i] += A0[i]*Bc0[0]+A1[i]*Bc0[1];
+        }
+        
+        Bc0 += RK;
+#undef WORK
+      }
+    }
+    
+    // process the last columns of A, corresponding to the last rows of B
+    Index rd = d-d_end;
+    if(rd>0)
+    {
+      for(Index j=0; j<n; ++j)
+      {
+        enum {
+          Alignment = PacketSize>1 ? Aligned : 0
+        };
+        typedef Map<Matrix<Scalar,Dynamic,1>, Alignment > MapVector;
+        typedef Map<const Matrix<Scalar,Dynamic,1>, Alignment > ConstMapVector;
+        if(rd==1)       MapVector(C+j*ldc+ib,actual_b) += B[0+d_end+j*ldb] * ConstMapVector(A+(d_end+0)*lda+ib, actual_b);
+        
+        else if(rd==2)  MapVector(C+j*ldc+ib,actual_b) += B[0+d_end+j*ldb] * ConstMapVector(A+(d_end+0)*lda+ib, actual_b)
+                                                        + B[1+d_end+j*ldb] * ConstMapVector(A+(d_end+1)*lda+ib, actual_b);
+        
+        else            MapVector(C+j*ldc+ib,actual_b) += B[0+d_end+j*ldb] * ConstMapVector(A+(d_end+0)*lda+ib, actual_b)
+                                                        + B[1+d_end+j*ldb] * ConstMapVector(A+(d_end+1)*lda+ib, actual_b)
+                                                        + B[2+d_end+j*ldb] * ConstMapVector(A+(d_end+2)*lda+ib, actual_b);
+      }
+    }
+  
+  } // blocking on the rows of A and C
+}
+#undef KMADD
+
+} // namespace internal
+
+} // namespace Eigen
+
+#endif // EIGEN_SPARSELU_GEMM_KERNEL_H
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
new file mode 100644
index 0000000..6f75d50
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
@@ -0,0 +1,126 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+/* This file is a modified version of heap_relax_snode.c file in SuperLU
+ * -- SuperLU routine (version 3.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * October 15, 2003
+ *
+ * Copyright (c) 1994 by Xerox Corporation.  All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+
+#ifndef SPARSELU_HEAP_RELAX_SNODE_H
+#define SPARSELU_HEAP_RELAX_SNODE_H
+
+namespace Eigen {
+namespace internal {
+
+/** 
+ * \brief Identify the initial relaxed supernodes
+ * 
+ * This routine applied to a symmetric elimination tree. 
+ * It assumes that the matrix has been reordered according to the postorder of the etree
+ * \param n The number of columns
+ * \param et elimination tree 
+ * \param relax_columns Maximum number of columns allowed in a relaxed snode 
+ * \param descendants Number of descendants of each node in the etree
+ * \param relax_end last column in a supernode
+ */
+template <typename Scalar, typename StorageIndex>
+void SparseLUImpl<Scalar,StorageIndex>::heap_relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end)
+{
+  
+  // The etree may not be postordered, but its heap ordered  
+  IndexVector post;
+  internal::treePostorder(StorageIndex(n), et, post); // Post order etree
+  IndexVector inv_post(n+1); 
+  for (StorageIndex i = 0; i < n+1; ++i) inv_post(post(i)) = i; // inv_post = post.inverse()???
+  
+  // Renumber etree in postorder 
+  IndexVector iwork(n);
+  IndexVector et_save(n+1);
+  for (Index i = 0; i < n; ++i)
+  {
+    iwork(post(i)) = post(et(i));
+  }
+  et_save = et; // Save the original etree
+  et = iwork; 
+  
+  // compute the number of descendants of each node in the etree
+  relax_end.setConstant(emptyIdxLU);
+  Index j, parent; 
+  descendants.setZero();
+  for (j = 0; j < n; j++) 
+  {
+    parent = et(j);
+    if (parent != n) // not the dummy root
+      descendants(parent) += descendants(j) + 1;
+  }
+  // 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; )
+  {
+    parent = et(j);
+    snode_start = j; 
+    while ( parent != n && descendants(parent) < relax_columns ) 
+    {
+      j = parent; 
+      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));
+    l = inv_post(j);
+    if ( (l - k) == (j - snode_start) )  // Same number of columns in the snode
+    {
+      // This is also a supernode in the original etree
+      relax_end(k) = l; // Record last column 
+      ++nsuper_et; 
+    }
+    else 
+    {
+      for (Index i = snode_start; i <= j; ++i) 
+      {
+        l = inv_post(i);
+        if (descendants(i) == 0) 
+        {
+          relax_end(l) = l;
+          ++nsuper_et;
+        }
+      }
+    }
+    j++;
+    // Search for a new leaf
+    while (descendants(j) != 0 && j < n) j++;
+  } // End postorder traversal of the etree
+  
+  // Recover the original etree
+  et = et_save; 
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
+#endif // SPARSELU_HEAP_RELAX_SNODE_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_kernel_bmod.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_kernel_bmod.h
new file mode 100644
index 0000000..8c1b3e8
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_kernel_bmod.h
@@ -0,0 +1,130 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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 SPARSELU_KERNEL_BMOD_H
+#define SPARSELU_KERNEL_BMOD_H
+
+namespace Eigen {
+namespace internal {
+  
+template <int SegSizeAtCompileTime> struct LU_kernel_bmod
+{
+  /** \internal
+    * \brief Performs numeric block updates from a given supernode to a single column
+    *
+    * \param segsize Size of the segment (and blocks ) to use for updates
+    * \param[in,out] dense Packed values of the original matrix
+    * \param tempv temporary vector to use for updates
+    * \param lusup array containing the supernodes
+    * \param lda Leading dimension in the supernode
+    * \param nrow Number of rows in the rectangular part of the supernode
+    * \param lsub compressed row subscripts of supernodes
+    * \param lptr pointer to the first column of the current supernode in lsub
+    * \param no_zeros Number of nonzeros elements before the diagonal part of the supernode
+    */
+  template <typename BlockScalarVector, typename ScalarVector, typename IndexVector>
+  static EIGEN_DONT_INLINE void run(const Index segsize, BlockScalarVector& dense, ScalarVector& tempv, ScalarVector& lusup, Index& luptr, const Index lda,
+                                    const Index nrow, IndexVector& lsub, const Index lptr, const Index no_zeros);
+};
+
+template <int SegSizeAtCompileTime>
+template <typename BlockScalarVector, typename ScalarVector, typename IndexVector>
+EIGEN_DONT_INLINE void LU_kernel_bmod<SegSizeAtCompileTime>::run(const Index segsize, BlockScalarVector& dense, ScalarVector& tempv, ScalarVector& lusup, Index& luptr, const Index lda,
+                                                                  const Index nrow, IndexVector& lsub, const Index lptr, const Index no_zeros)
+{
+  typedef typename ScalarVector::Scalar Scalar;
+  // First, copy U[*,j] segment from dense(*) to tempv(*)
+  // The result of triangular solve is in tempv[*]; 
+    // The result of matric-vector update is in dense[*]
+  Index isub = lptr + no_zeros; 
+  Index i;
+  Index irow;
+  for (i = 0; i < ((SegSizeAtCompileTime==Dynamic)?segsize:SegSizeAtCompileTime); i++)
+  {
+    irow = lsub(isub); 
+    tempv(i) = dense(irow); 
+    ++isub; 
+  }
+  // Dense triangular solve -- start effective triangle
+  luptr += lda * no_zeros + no_zeros; 
+  // Form Eigen matrix and vector 
+  Map<Matrix<Scalar,SegSizeAtCompileTime,SegSizeAtCompileTime, ColMajor>, 0, OuterStride<> > A( &(lusup.data()[luptr]), segsize, segsize, OuterStride<>(lda) );
+  Map<Matrix<Scalar,SegSizeAtCompileTime,1> > u(tempv.data(), segsize);
+  
+  u = A.template triangularView<UnitLower>().solve(u); 
+  
+  // Dense matrix-vector product y <-- B*x 
+  luptr += segsize;
+  const Index PacketSize = internal::packet_traits<Scalar>::size;
+  Index ldl = internal::first_multiple(nrow, PacketSize);
+  Map<Matrix<Scalar,Dynamic,SegSizeAtCompileTime, ColMajor>, 0, OuterStride<> > B( &(lusup.data()[luptr]), nrow, segsize, OuterStride<>(lda) );
+  Index aligned_offset = internal::first_default_aligned(tempv.data()+segsize, PacketSize);
+  Index aligned_with_B_offset = (PacketSize-internal::first_default_aligned(B.data(), PacketSize))%PacketSize;
+  Map<Matrix<Scalar,Dynamic,1>, 0, OuterStride<> > l(tempv.data()+segsize+aligned_offset+aligned_with_B_offset, nrow, OuterStride<>(ldl) );
+  
+  l.setZero();
+  internal::sparselu_gemm<Scalar>(l.rows(), l.cols(), B.cols(), B.data(), B.outerStride(), u.data(), u.outerStride(), l.data(), l.outerStride());
+  
+  // Scatter tempv[] into SPA dense[] as a temporary storage 
+  isub = lptr + no_zeros;
+  for (i = 0; i < ((SegSizeAtCompileTime==Dynamic)?segsize:SegSizeAtCompileTime); i++)
+  {
+    irow = lsub(isub++); 
+    dense(irow) = tempv(i);
+  }
+  
+  // Scatter l into SPA dense[]
+  for (i = 0; i < nrow; i++)
+  {
+    irow = lsub(isub++); 
+    dense(irow) -= l(i);
+  } 
+}
+
+template <> struct LU_kernel_bmod<1>
+{
+  template <typename BlockScalarVector, typename ScalarVector, typename IndexVector>
+  static EIGEN_DONT_INLINE void run(const Index /*segsize*/, BlockScalarVector& dense, ScalarVector& /*tempv*/, ScalarVector& lusup, Index& luptr,
+                                    const Index lda, const Index nrow, IndexVector& lsub, const Index lptr, const Index no_zeros);
+};
+
+
+template <typename BlockScalarVector, typename ScalarVector, typename IndexVector>
+EIGEN_DONT_INLINE void LU_kernel_bmod<1>::run(const Index /*segsize*/, BlockScalarVector& dense, ScalarVector& /*tempv*/, ScalarVector& lusup, Index& luptr,
+                                              const Index lda, const Index nrow, IndexVector& lsub, const Index lptr, const Index no_zeros)
+{
+  typedef typename ScalarVector::Scalar Scalar;
+  typedef typename IndexVector::Scalar StorageIndex;
+  Scalar f = dense(lsub(lptr + no_zeros));
+  luptr += lda * no_zeros + no_zeros + 1;
+  const Scalar* a(lusup.data() + luptr);
+  const StorageIndex*  irow(lsub.data()+lptr + no_zeros + 1);
+  Index i = 0;
+  for (; i+1 < nrow; i+=2)
+  {
+    Index i0 = *(irow++);
+    Index i1 = *(irow++);
+    Scalar a0 = *(a++);
+    Scalar a1 = *(a++);
+    Scalar d0 = dense.coeff(i0);
+    Scalar d1 = dense.coeff(i1);
+    d0 -= f*a0;
+    d1 -= f*a1;
+    dense.coeffRef(i0) = d0;
+    dense.coeffRef(i1) = d1;
+  }
+  if(i<nrow)
+    dense.coeffRef(*(irow++)) -= f * *(a++);
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
+#endif // SPARSELU_KERNEL_BMOD_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_bmod.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_bmod.h
new file mode 100644
index 0000000..f052001
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_bmod.h
@@ -0,0 +1,223 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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/.
+
+/* 
+ 
+ * NOTE: This file is the modified version of [s,d,c,z]panel_bmod.c file in SuperLU 
+ 
+ * -- SuperLU routine (version 3.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * October 15, 2003
+ *
+ * Copyright (c) 1994 by Xerox Corporation.  All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+#ifndef SPARSELU_PANEL_BMOD_H
+#define SPARSELU_PANEL_BMOD_H
+
+namespace Eigen {
+namespace internal {
+
+/**
+ * \brief Performs numeric block updates (sup-panel) in topological order.
+ * 
+ * Before entering this routine, the original nonzeros in the panel
+ * were already copied into the spa[m,w]
+ * 
+ * \param m number of rows in the matrix
+ * \param w Panel size
+ * \param jcol Starting  column of the panel
+ * \param nseg Number of segments in the U part
+ * \param dense Store the full representation of the panel 
+ * \param tempv working array 
+ * \param segrep segment representative... first row in the segment
+ * \param repfnz First nonzero rows
+ * \param glu Global LU data. 
+ * 
+ * 
+ */
+template <typename Scalar, typename StorageIndex>
+void SparseLUImpl<Scalar,StorageIndex>::panel_bmod(const Index m, const Index w, const Index jcol, 
+                                            const Index nseg, ScalarVector& dense, ScalarVector& tempv,
+                                            IndexVector& segrep, IndexVector& repfnz, GlobalLU_t& glu)
+{
+  
+  Index ksub,jj,nextl_col; 
+  Index fsupc, nsupc, nsupr, nrow; 
+  Index krep, kfnz; 
+  Index lptr; // points to the row subscripts of a supernode 
+  Index luptr; // ...
+  Index segsize,no_zeros ; 
+  // For each nonz supernode segment of U[*,j] in topological order
+  Index k = nseg - 1; 
+  const Index PacketSize = internal::packet_traits<Scalar>::size;
+  
+  for (ksub = 0; ksub < nseg; ksub++)
+  { // For each updating supernode
+    /* krep = representative of current k-th supernode
+     * fsupc =  first supernodal column
+     * nsupc = number of columns in a supernode
+     * nsupr = number of rows in a supernode
+     */
+    krep = segrep(k); k--; 
+    fsupc = glu.xsup(glu.supno(krep)); 
+    nsupc = krep - fsupc + 1; 
+    nsupr = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); 
+    nrow = nsupr - nsupc; 
+    lptr = glu.xlsub(fsupc); 
+    
+    // loop over the panel columns to detect the actual number of columns and rows
+    Index u_rows = 0;
+    Index u_cols = 0;
+    for (jj = jcol; jj < jcol + w; jj++)
+    {
+      nextl_col = (jj-jcol) * m; 
+      VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m); // First nonzero column index for each row
+      
+      kfnz = repfnz_col(krep); 
+      if ( kfnz == emptyIdxLU ) 
+        continue; // skip any zero segment
+      
+      segsize = krep - kfnz + 1;
+      u_cols++;
+      u_rows = (std::max)(segsize,u_rows);
+    }
+    
+    if(nsupc >= 2)
+    { 
+      Index ldu = internal::first_multiple<Index>(u_rows, PacketSize);
+      Map<ScalarMatrix, Aligned,  OuterStride<> > U(tempv.data(), u_rows, u_cols, OuterStride<>(ldu));
+      
+      // gather U
+      Index u_col = 0;
+      for (jj = jcol; jj < jcol + w; jj++)
+      {
+        nextl_col = (jj-jcol) * m; 
+        VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m); // First nonzero column index for each row
+        VectorBlock<ScalarVector> dense_col(dense, nextl_col, m); // Scatter/gather entire matrix column from/to here
+        
+        kfnz = repfnz_col(krep); 
+        if ( kfnz == emptyIdxLU ) 
+          continue; // skip any zero segment
+        
+        segsize = krep - kfnz + 1;
+        luptr = glu.xlusup(fsupc);    
+        no_zeros = kfnz - fsupc; 
+        
+        Index isub = lptr + no_zeros;
+        Index off = u_rows-segsize;
+        for (Index i = 0; i < off; i++) U(i,u_col) = 0;
+        for (Index i = 0; i < segsize; i++)
+        {
+          Index irow = glu.lsub(isub); 
+          U(i+off,u_col) = dense_col(irow); 
+          ++isub; 
+        }
+        u_col++;
+      }
+      // solve U = A^-1 U
+      luptr = glu.xlusup(fsupc);
+      Index lda = glu.xlusup(fsupc+1) - glu.xlusup(fsupc);
+      no_zeros = (krep - u_rows + 1) - fsupc;
+      luptr += lda * no_zeros + no_zeros;
+      MappedMatrixBlock A(glu.lusup.data()+luptr, u_rows, u_rows, OuterStride<>(lda) );
+      U = A.template triangularView<UnitLower>().solve(U);
+      
+      // update
+      luptr += u_rows;
+      MappedMatrixBlock B(glu.lusup.data()+luptr, nrow, u_rows, OuterStride<>(lda) );
+      eigen_assert(tempv.size()>w*ldu + nrow*w + 1);
+      
+      Index ldl = internal::first_multiple<Index>(nrow, PacketSize);
+      Index offset = (PacketSize-internal::first_default_aligned(B.data(), PacketSize)) % PacketSize;
+      MappedMatrixBlock L(tempv.data()+w*ldu+offset, nrow, u_cols, OuterStride<>(ldl));
+      
+      L.setZero();
+      internal::sparselu_gemm<Scalar>(L.rows(), L.cols(), B.cols(), B.data(), B.outerStride(), U.data(), U.outerStride(), L.data(), L.outerStride());
+      
+      // scatter U and L
+      u_col = 0;
+      for (jj = jcol; jj < jcol + w; jj++)
+      {
+        nextl_col = (jj-jcol) * m; 
+        VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m); // First nonzero column index for each row
+        VectorBlock<ScalarVector> dense_col(dense, nextl_col, m); // Scatter/gather entire matrix column from/to here
+        
+        kfnz = repfnz_col(krep); 
+        if ( kfnz == emptyIdxLU ) 
+          continue; // skip any zero segment
+        
+        segsize = krep - kfnz + 1;
+        no_zeros = kfnz - fsupc; 
+        Index isub = lptr + no_zeros;
+        
+        Index off = u_rows-segsize;
+        for (Index i = 0; i < segsize; i++)
+        {
+          Index irow = glu.lsub(isub++); 
+          dense_col(irow) = U.coeff(i+off,u_col);
+          U.coeffRef(i+off,u_col) = 0;
+        }
+        
+        // Scatter l into SPA dense[]
+        for (Index i = 0; i < nrow; i++)
+        {
+          Index irow = glu.lsub(isub++); 
+          dense_col(irow) -= L.coeff(i,u_col);
+          L.coeffRef(i,u_col) = 0;
+        }
+        u_col++;
+      }
+    }
+    else // level 2 only
+    {
+      // Sequence through each column in the panel
+      for (jj = jcol; jj < jcol + w; jj++)
+      {
+        nextl_col = (jj-jcol) * m; 
+        VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m); // First nonzero column index for each row
+        VectorBlock<ScalarVector> dense_col(dense, nextl_col, m); // Scatter/gather entire matrix column from/to here
+        
+        kfnz = repfnz_col(krep); 
+        if ( kfnz == emptyIdxLU ) 
+          continue; // skip any zero segment
+        
+        segsize = krep - kfnz + 1;
+        luptr = glu.xlusup(fsupc);
+        
+        Index lda = glu.xlusup(fsupc+1)-glu.xlusup(fsupc);// nsupr
+        
+        // Perform a trianglar solve and block update, 
+        // then scatter the result of sup-col update to dense[]
+        no_zeros = kfnz - fsupc; 
+              if(segsize==1)  LU_kernel_bmod<1>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
+        else  if(segsize==2)  LU_kernel_bmod<2>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
+        else  if(segsize==3)  LU_kernel_bmod<3>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros);
+        else                  LU_kernel_bmod<Dynamic>::run(segsize, dense_col, tempv, glu.lusup, luptr, lda, nrow, glu.lsub, lptr, no_zeros); 
+      } // End for each column in the panel 
+    }
+    
+  } // End for each updating supernode
+} // end panel bmod
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // SPARSELU_PANEL_BMOD_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_dfs.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_dfs.h
new file mode 100644
index 0000000..155df73
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_panel_dfs.h
@@ -0,0 +1,258 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+/* 
+ 
+ * NOTE: This file is the modified version of [s,d,c,z]panel_dfs.c file in SuperLU 
+ 
+ * -- SuperLU routine (version 2.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * November 15, 1997
+ *
+ * Copyright (c) 1994 by Xerox Corporation.  All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+#ifndef SPARSELU_PANEL_DFS_H
+#define SPARSELU_PANEL_DFS_H
+
+namespace Eigen {
+
+namespace internal {
+  
+template<typename IndexVector>
+struct panel_dfs_traits
+{
+  typedef typename IndexVector::Scalar StorageIndex;
+  panel_dfs_traits(Index jcol, StorageIndex* marker)
+    : m_jcol(jcol), m_marker(marker)
+  {}
+  bool update_segrep(Index krep, StorageIndex jj)
+  {
+    if(m_marker[krep]<m_jcol)
+    {
+      m_marker[krep] = jj; 
+      return true;
+    }
+    return false;
+  }
+  void mem_expand(IndexVector& /*glu.lsub*/, Index /*nextl*/, Index /*chmark*/) {}
+  enum { ExpandMem = false };
+  Index m_jcol;
+  StorageIndex* m_marker;
+};
+
+
+template <typename Scalar, typename StorageIndex>
+template <typename Traits>
+void SparseLUImpl<Scalar,StorageIndex>::dfs_kernel(const StorageIndex jj, IndexVector& perm_r,
+                   Index& nseg, IndexVector& panel_lsub, IndexVector& segrep,
+                   Ref<IndexVector> repfnz_col, IndexVector& xprune, Ref<IndexVector> marker, IndexVector& parent,
+                   IndexVector& xplore, GlobalLU_t& glu,
+                   Index& nextl_col, Index krow, Traits& traits
+                  )
+{
+  
+  StorageIndex kmark = marker(krow);
+      
+  // For each unmarked krow of jj
+  marker(krow) = jj; 
+  StorageIndex kperm = perm_r(krow); 
+  if (kperm == emptyIdxLU ) {
+    // krow is in L : place it in structure of L(*, jj)
+    panel_lsub(nextl_col++) = StorageIndex(krow);  // krow is indexed into A
+    
+    traits.mem_expand(panel_lsub, nextl_col, kmark);
+  }
+  else 
+  {
+    // krow is in U : if its supernode-representative krep
+    // has been explored, update repfnz(*)
+    // krep = supernode representative of the current row
+    StorageIndex krep = glu.xsup(glu.supno(kperm)+1) - 1; 
+    // First nonzero element in the current column:
+    StorageIndex myfnz = repfnz_col(krep); 
+    
+    if (myfnz != emptyIdxLU )
+    {
+      // Representative visited before
+      if (myfnz > kperm ) repfnz_col(krep) = kperm; 
+      
+    }
+    else 
+    {
+      // Otherwise, perform dfs starting at krep
+      StorageIndex oldrep = emptyIdxLU; 
+      parent(krep) = oldrep; 
+      repfnz_col(krep) = kperm; 
+      StorageIndex xdfs =  glu.xlsub(krep); 
+      Index maxdfs = xprune(krep); 
+      
+      StorageIndex kpar;
+      do 
+      {
+        // For each unmarked kchild of krep
+        while (xdfs < maxdfs) 
+        {
+          StorageIndex kchild = glu.lsub(xdfs); 
+          xdfs++; 
+          StorageIndex chmark = marker(kchild); 
+          
+          if (chmark != jj ) 
+          {
+            marker(kchild) = jj; 
+            StorageIndex chperm = perm_r(kchild); 
+            
+            if (chperm == emptyIdxLU) 
+            {
+              // case kchild is in L: place it in L(*, j)
+              panel_lsub(nextl_col++) = kchild;
+              traits.mem_expand(panel_lsub, nextl_col, chmark);
+            }
+            else
+            {
+              // case kchild is in U :
+              // chrep = its supernode-rep. If its rep has been explored, 
+              // update its repfnz(*)
+              StorageIndex chrep = glu.xsup(glu.supno(chperm)+1) - 1; 
+              myfnz = repfnz_col(chrep); 
+              
+              if (myfnz != emptyIdxLU) 
+              { // Visited before 
+                if (myfnz > chperm) 
+                  repfnz_col(chrep) = chperm; 
+              }
+              else 
+              { // Cont. dfs at snode-rep of kchild
+                xplore(krep) = xdfs; 
+                oldrep = krep; 
+                krep = chrep; // Go deeper down G(L)
+                parent(krep) = oldrep; 
+                repfnz_col(krep) = chperm; 
+                xdfs = glu.xlsub(krep); 
+                maxdfs = xprune(krep); 
+                
+              } // end if myfnz != -1
+            } // end if chperm == -1 
+                
+          } // end if chmark !=jj
+        } // end while xdfs < maxdfs
+        
+        // krow has no more unexplored nbrs :
+        //    Place snode-rep krep in postorder DFS, if this 
+        //    segment is seen for the first time. (Note that 
+        //    "repfnz(krep)" may change later.)
+        //    Baktrack dfs to its parent
+        if(traits.update_segrep(krep,jj))
+        //if (marker1(krep) < jcol )
+        {
+          segrep(nseg) = krep; 
+          ++nseg; 
+          //marker1(krep) = jj; 
+        }
+        
+        kpar = parent(krep); // Pop recursion, mimic recursion 
+        if (kpar == emptyIdxLU) 
+          break; // dfs done 
+        krep = kpar; 
+        xdfs = xplore(krep); 
+        maxdfs = xprune(krep); 
+
+      } while (kpar != emptyIdxLU); // Do until empty stack 
+      
+    } // end if (myfnz = -1)
+
+  } // end if (kperm == -1)   
+}
+
+/**
+ * \brief Performs a symbolic factorization on a panel of columns [jcol, jcol+w)
+ * 
+ * A supernode representative is the last column of a supernode.
+ * The nonzeros in U[*,j] are segments that end at supernodes representatives
+ * 
+ * The routine returns a list of the supernodal representatives 
+ * in topological order of the dfs that generates them. This list is 
+ * a superset of the topological order of each individual column within 
+ * the panel.
+ * The location of the first nonzero in each supernodal segment 
+ * (supernodal entry location) is also returned. Each column has 
+ * a separate list for this purpose. 
+ * 
+ * Two markers arrays are used for dfs :
+ *    marker[i] == jj, if i was visited during dfs of current column jj;
+ *    marker1[i] >= jcol, if i was visited by earlier columns in this panel; 
+ * 
+ * \param[in] m number of rows in the matrix
+ * \param[in] w Panel size
+ * \param[in] jcol Starting  column of the panel
+ * \param[in] A Input matrix in column-major storage
+ * \param[in] perm_r Row permutation
+ * \param[out] nseg Number of U segments
+ * \param[out] dense Accumulate the column vectors of the panel
+ * \param[out] panel_lsub Subscripts of the row in the panel 
+ * \param[out] segrep Segment representative i.e first nonzero row of each segment
+ * \param[out] repfnz First nonzero location in each row
+ * \param[out] xprune The pruned elimination tree
+ * \param[out] marker work vector
+ * \param  parent The elimination tree
+ * \param xplore work vector
+ * \param glu The global data structure
+ * 
+ */
+
+template <typename Scalar, typename StorageIndex>
+void SparseLUImpl<Scalar,StorageIndex>::panel_dfs(const Index m, const Index w, const Index jcol, MatrixType& A, IndexVector& perm_r, Index& nseg, ScalarVector& dense, IndexVector& panel_lsub, IndexVector& segrep, IndexVector& repfnz, IndexVector& xprune, IndexVector& marker, IndexVector& parent, IndexVector& xplore, GlobalLU_t& glu)
+{
+  Index nextl_col; // Next available position in panel_lsub[*,jj] 
+  
+  // Initialize pointers 
+  VectorBlock<IndexVector> marker1(marker, m, m); 
+  nseg = 0; 
+  
+  panel_dfs_traits<IndexVector> traits(jcol, marker1.data());
+  
+  // For each column in the panel 
+  for (StorageIndex jj = StorageIndex(jcol); jj < jcol + w; jj++) 
+  {
+    nextl_col = (jj - jcol) * m; 
+    
+    VectorBlock<IndexVector> repfnz_col(repfnz, nextl_col, m); // First nonzero location in each row
+    VectorBlock<ScalarVector> dense_col(dense,nextl_col, m); // Accumulate a column vector here
+    
+    
+    // For each nnz in A[*, jj] do depth first search
+    for (typename MatrixType::InnerIterator it(A, jj); it; ++it)
+    {
+      Index krow = it.row(); 
+      dense_col(krow) = it.value();
+      
+      StorageIndex kmark = marker(krow); 
+      if (kmark == jj) 
+        continue; // krow visited before, go to the next nonzero
+      
+      dfs_kernel(jj, perm_r, nseg, panel_lsub, segrep, repfnz_col, xprune, marker, parent,
+                   xplore, glu, nextl_col, krow, traits);
+    }// end for nonzeros in column jj
+    
+  } // end for column jj
+}
+
+} // end namespace internal
+} // end namespace Eigen
+
+#endif // SPARSELU_PANEL_DFS_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pivotL.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pivotL.h
new file mode 100644
index 0000000..a86dac9
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pivotL.h
@@ -0,0 +1,137 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+/* 
+ 
+ * NOTE: This file is the modified version of xpivotL.c file in SuperLU 
+ 
+ * -- SuperLU routine (version 3.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * October 15, 2003
+ *
+ * Copyright (c) 1994 by Xerox Corporation.  All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+#ifndef SPARSELU_PIVOTL_H
+#define SPARSELU_PIVOTL_H
+
+namespace Eigen {
+namespace internal {
+  
+/**
+ * \brief Performs the numerical pivotin on the current column of L, and the CDIV operation.
+ * 
+ * Pivot policy :
+ * (1) Compute thresh = u * max_(i>=j) abs(A_ij);
+ * (2) IF user specifies pivot row k and abs(A_kj) >= thresh THEN
+ *           pivot row = k;
+ *       ELSE IF abs(A_jj) >= thresh THEN
+ *           pivot row = j;
+ *       ELSE
+ *           pivot row = m;
+ * 
+ *   Note: If you absolutely want to use a given pivot order, then set u=0.0.
+ * 
+ * \param jcol The current column of L
+ * \param diagpivotthresh diagonal pivoting threshold
+ * \param[in,out] perm_r Row permutation (threshold pivoting)
+ * \param[in] iperm_c column permutation - used to finf diagonal of Pc*A*Pc'
+ * \param[out] pivrow  The pivot row
+ * \param glu Global LU data
+ * \return 0 if success, i > 0 if U(i,i) is exactly zero 
+ * 
+ */
+template <typename Scalar, typename StorageIndex>
+Index SparseLUImpl<Scalar,StorageIndex>::pivotL(const Index jcol, const RealScalar& diagpivotthresh, IndexVector& perm_r, IndexVector& iperm_c, Index& pivrow, GlobalLU_t& glu)
+{
+  
+  Index fsupc = (glu.xsup)((glu.supno)(jcol)); // First column in the supernode containing the column jcol
+  Index nsupc = jcol - fsupc; // Number of columns in the supernode portion, excluding jcol; nsupc >=0
+  Index lptr = glu.xlsub(fsupc); // pointer to the starting location of the row subscripts for this supernode portion
+  Index nsupr = glu.xlsub(fsupc+1) - lptr; // Number of rows in the supernode
+  Index lda = glu.xlusup(fsupc+1) - glu.xlusup(fsupc); // leading dimension
+  Scalar* lu_sup_ptr = &(glu.lusup.data()[glu.xlusup(fsupc)]); // Start of the current supernode
+  Scalar* lu_col_ptr = &(glu.lusup.data()[glu.xlusup(jcol)]); // Start of jcol in the supernode
+  StorageIndex* lsub_ptr = &(glu.lsub.data()[lptr]); // Start of row indices of the supernode
+  
+  // Determine the largest abs numerical value for partial pivoting 
+  Index diagind = iperm_c(jcol); // diagonal index 
+  RealScalar pivmax(-1.0);
+  Index pivptr = nsupc; 
+  Index diag = emptyIdxLU; 
+  RealScalar rtemp;
+  Index isub, icol, itemp, k; 
+  for (isub = nsupc; isub < nsupr; ++isub) {
+    using std::abs;
+    rtemp = abs(lu_col_ptr[isub]);
+    if (rtemp > pivmax) {
+      pivmax = rtemp; 
+      pivptr = isub;
+    } 
+    if (lsub_ptr[isub] == diagind) diag = isub;
+  }
+  
+  // Test for singularity
+  if ( pivmax <= RealScalar(0.0) ) {
+    // if pivmax == -1, the column is structurally empty, otherwise it is only numerically zero
+    pivrow = pivmax < RealScalar(0.0) ? diagind : lsub_ptr[pivptr];
+    perm_r(pivrow) = StorageIndex(jcol);
+    return (jcol+1);
+  }
+  
+  RealScalar thresh = diagpivotthresh * pivmax; 
+  
+  // Choose appropriate pivotal element 
+  
+  {
+    // Test if the diagonal element can be used as a pivot (given the threshold value)
+    if (diag >= 0 ) 
+    {
+      // Diagonal element exists
+      using std::abs;
+      rtemp = abs(lu_col_ptr[diag]);
+      if (rtemp != RealScalar(0.0) && rtemp >= thresh) pivptr = diag;
+    }
+    pivrow = lsub_ptr[pivptr];
+  }
+  
+  // Record pivot row
+  perm_r(pivrow) = StorageIndex(jcol);
+  // Interchange row subscripts
+  if (pivptr != nsupc )
+  {
+    std::swap( lsub_ptr[pivptr], lsub_ptr[nsupc] );
+    // Interchange numerical values as well, for the two rows in the whole snode
+    // such that L is indexed the same way as A
+    for (icol = 0; icol <= nsupc; icol++)
+    {
+      itemp = pivptr + icol * lda; 
+      std::swap(lu_sup_ptr[itemp], lu_sup_ptr[nsupc + icol * lda]);
+    }
+  }
+  // cdiv operations
+  Scalar temp = Scalar(1.0) / lu_col_ptr[nsupc];
+  for (k = nsupc+1; k < nsupr; k++)
+    lu_col_ptr[k] *= temp; 
+  return 0;
+}
+
+} // end namespace internal
+} // end namespace Eigen
+
+#endif // SPARSELU_PIVOTL_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pruneL.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pruneL.h
new file mode 100644
index 0000000..ad32fed
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_pruneL.h
@@ -0,0 +1,136 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+/* 
+ 
+ * NOTE: This file is the modified version of [s,d,c,z]pruneL.c file in SuperLU 
+ 
+ * -- SuperLU routine (version 2.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * November 15, 1997
+ *
+ * Copyright (c) 1994 by Xerox Corporation.  All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+#ifndef SPARSELU_PRUNEL_H
+#define SPARSELU_PRUNEL_H
+
+namespace Eigen {
+namespace internal {
+
+/**
+ * \brief Prunes the L-structure.
+ *
+ * It prunes the L-structure  of supernodes whose L-structure contains the current pivot row "pivrow"
+ * 
+ * 
+ * \param jcol The current column of L
+ * \param[in] perm_r Row permutation
+ * \param[out] pivrow  The pivot row
+ * \param nseg Number of segments
+ * \param segrep 
+ * \param repfnz
+ * \param[out] xprune 
+ * \param glu Global LU data
+ * 
+ */
+template <typename Scalar, typename StorageIndex>
+void SparseLUImpl<Scalar,StorageIndex>::pruneL(const Index jcol, const IndexVector& perm_r, const Index pivrow, const Index nseg,
+                                               const IndexVector& segrep, BlockIndexVector repfnz, IndexVector& xprune, GlobalLU_t& glu)
+{
+  // For each supernode-rep irep in U(*,j]
+  Index jsupno = glu.supno(jcol); 
+  Index i,irep,irep1; 
+  bool movnum, do_prune = false; 
+  Index kmin = 0, kmax = 0, minloc, maxloc,krow; 
+  for (i = 0; i < nseg; i++)
+  {
+    irep = segrep(i); 
+    irep1 = irep + 1; 
+    do_prune = false; 
+    
+    // Don't prune with a zero U-segment 
+    if (repfnz(irep) == emptyIdxLU) continue; 
+    
+    // If a snode overlaps with the next panel, then the U-segment
+    // is fragmented into two parts -- irep and irep1. We should let 
+    // pruning occur at the rep-column in irep1s snode. 
+    if (glu.supno(irep) == glu.supno(irep1) ) continue; // don't prune 
+    
+    // If it has not been pruned & it has a nonz in row L(pivrow,i)
+    if (glu.supno(irep) != jsupno )
+    {
+      if ( xprune (irep) >= glu.xlsub(irep1) )
+      {
+        kmin = glu.xlsub(irep);
+        kmax = glu.xlsub(irep1) - 1; 
+        for (krow = kmin; krow <= kmax; krow++)
+        {
+          if (glu.lsub(krow) == pivrow) 
+          {
+            do_prune = true; 
+            break; 
+          }
+        }
+      }
+      
+      if (do_prune) 
+      {
+        // do a quicksort-type partition
+        // movnum=true means that the num values have to be exchanged
+        movnum = false; 
+        if (irep == glu.xsup(glu.supno(irep)) ) // Snode of size 1 
+          movnum = true; 
+        
+        while (kmin <= kmax)
+        {
+          if (perm_r(glu.lsub(kmax)) == emptyIdxLU)
+            kmax--; 
+          else if ( perm_r(glu.lsub(kmin)) != emptyIdxLU)
+            kmin++;
+          else 
+          {
+            // kmin below pivrow (not yet pivoted), and kmax
+            // above pivrow: interchange the two suscripts
+            std::swap(glu.lsub(kmin), glu.lsub(kmax)); 
+            
+            // If the supernode has only one column, then we 
+            // only keep one set of subscripts. For any subscript
+            // intercnahge performed, similar interchange must be 
+            // done on the numerical values. 
+            if (movnum) 
+            {
+              minloc = glu.xlusup(irep) + ( kmin - glu.xlsub(irep) ); 
+              maxloc = glu.xlusup(irep) + ( kmax - glu.xlsub(irep) ); 
+              std::swap(glu.lusup(minloc), glu.lusup(maxloc)); 
+            }
+            kmin++;
+            kmax--;
+          }
+        } // end while 
+        
+        xprune(irep) = StorageIndex(kmin);  //Pruning 
+      } // end if do_prune 
+    } // end pruning 
+  } // End for each U-segment
+}
+
+} // end namespace internal
+} // end namespace Eigen
+
+#endif // SPARSELU_PRUNEL_H
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_relax_snode.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_relax_snode.h
new file mode 100644
index 0000000..c408d01
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseLU/SparseLU_relax_snode.h
@@ -0,0 +1,83 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+//
+// 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/.
+
+/* This file is a modified version of heap_relax_snode.c file in SuperLU
+ * -- SuperLU routine (version 3.0) --
+ * Univ. of California Berkeley, Xerox Palo Alto Research Center,
+ * and Lawrence Berkeley National Lab.
+ * October 15, 2003
+ *
+ * Copyright (c) 1994 by Xerox Corporation.  All rights reserved.
+ *
+ * THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+ * EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
+ *
+ * Permission is hereby granted to use or copy this program for any
+ * purpose, provided the above notices are retained on all copies.
+ * Permission to modify the code and to distribute modified code is
+ * granted, provided the above notices are retained, and a notice that
+ * the code was modified is included with the above copyright notice.
+ */
+
+#ifndef SPARSELU_RELAX_SNODE_H
+#define SPARSELU_RELAX_SNODE_H
+
+namespace Eigen {
+
+namespace internal {
+ 
+/** 
+ * \brief Identify the initial relaxed supernodes
+ * 
+ * This routine is applied to a column elimination tree. 
+ * It assumes that the matrix has been reordered according to the postorder of the etree
+ * \param n  the number of columns
+ * \param et elimination tree 
+ * \param relax_columns Maximum number of columns allowed in a relaxed snode 
+ * \param descendants Number of descendants of each node in the etree
+ * \param relax_end last column in a supernode
+ */
+template <typename Scalar, typename StorageIndex>
+void SparseLUImpl<Scalar,StorageIndex>::relax_snode (const Index n, IndexVector& et, const Index relax_columns, IndexVector& descendants, IndexVector& relax_end)
+{
+  
+  // compute the number of descendants of each node in the etree
+  Index parent; 
+  relax_end.setConstant(emptyIdxLU);
+  descendants.setZero();
+  for (Index j = 0; j < n; j++) 
+  {
+    parent = et(j);
+    if (parent != n) // not the dummy root
+      descendants(parent) += descendants(j) + 1;
+  }
+  // Identify the relaxed supernodes by postorder traversal of the etree
+  Index snode_start; // beginning of a snode 
+  for (Index j = 0; j < n; )
+  {
+    parent = et(j);
+    snode_start = j; 
+    while ( parent != n && descendants(parent) < relax_columns ) 
+    {
+      j = parent; 
+      parent = et(j);
+    }
+    // Found a supernode in postordered etree, j is the last column 
+    relax_end(snode_start) = StorageIndex(j); // Record last column
+    j++;
+    // Search for a new leaf
+    while (descendants(j) != 0 && j < n) j++;
+  } // End postorder traversal of the etree
+  
+}
+
+} // end namespace internal
+
+} // end namespace Eigen
+#endif
diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h
new file mode 100644
index 0000000..d1fb96f
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseQR/SparseQR.h
@@ -0,0 +1,758 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012-2013 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2012-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// 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_SPARSE_QR_H
+#define EIGEN_SPARSE_QR_H
+
+namespace Eigen {
+
+template<typename MatrixType, typename OrderingType> class SparseQR;
+template<typename SparseQRType> struct SparseQRMatrixQReturnType;
+template<typename SparseQRType> struct SparseQRMatrixQTransposeReturnType;
+template<typename SparseQRType, typename Derived> struct SparseQR_QProduct;
+namespace internal {
+  template <typename SparseQRType> struct traits<SparseQRMatrixQReturnType<SparseQRType> >
+  {
+    typedef typename SparseQRType::MatrixType ReturnType;
+    typedef typename ReturnType::StorageIndex StorageIndex;
+    typedef typename ReturnType::StorageKind StorageKind;
+    enum {
+      RowsAtCompileTime = Dynamic,
+      ColsAtCompileTime = Dynamic
+    };
+  };
+  template <typename SparseQRType> struct traits<SparseQRMatrixQTransposeReturnType<SparseQRType> >
+  {
+    typedef typename SparseQRType::MatrixType ReturnType;
+  };
+  template <typename SparseQRType, typename Derived> struct traits<SparseQR_QProduct<SparseQRType, Derived> >
+  {
+    typedef typename Derived::PlainObject ReturnType;
+  };
+} // End namespace internal
+
+/**
+  * \ingroup SparseQR_Module
+  * \class SparseQR
+  * \brief Sparse left-looking QR factorization with numerical column pivoting
+  * 
+  * This class implements a left-looking QR decomposition of sparse matrices
+  * with numerical column pivoting.
+  * When a column has a norm less than a given tolerance
+  * it is implicitly permuted to the end. The QR factorization thus obtained is 
+  * given by A*P = Q*R where R is upper triangular or trapezoidal. 
+  * 
+  * P is the column permutation which is the product of the fill-reducing and the
+  * numerical permutations. Use colsPermutation() to get it.
+  * 
+  * Q is the orthogonal matrix represented as products of Householder reflectors. 
+  * Use matrixQ() to get an expression and matrixQ().adjoint() to get the adjoint.
+  * You can then apply it to a vector.
+  * 
+  * R is the sparse triangular or trapezoidal matrix. The later occurs when A is rank-deficient.
+  * matrixR().topLeftCorner(rank(), rank()) always returns a triangular factor of full rank.
+  * 
+  * \tparam _MatrixType The type of the sparse matrix A, must be a column-major SparseMatrix<>
+  * \tparam _OrderingType The fill-reducing ordering method. See the \link OrderingMethods_Module 
+  *  OrderingMethods \endlink module for the list of built-in and external ordering methods.
+  * 
+  * \implsparsesolverconcept
+  *
+  * The numerical pivoting strategy and default threshold are the same as in SuiteSparse QR, and
+  * detailed in the following paper:
+  * <i>
+  * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing
+  * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011.
+  * </i>
+  * Even though it is qualified as "rank-revealing", this strategy might fail for some 
+  * rank deficient problems. When this class is used to solve linear or least-square problems
+  * it is thus strongly recommended to check the accuracy of the computed solution. If it
+  * failed, it usually helps to increase the threshold with setPivotThreshold.
+  * 
+  * \warning The input sparse matrix A must be in compressed mode (see SparseMatrix::makeCompressed()).
+  * \warning For complex matrices matrixQ().transpose() will actually return the adjoint matrix.
+  * 
+  */
+template<typename _MatrixType, typename _OrderingType>
+class SparseQR : public SparseSolverBase<SparseQR<_MatrixType,_OrderingType> >
+{
+  protected:
+    typedef SparseSolverBase<SparseQR<_MatrixType,_OrderingType> > Base;
+    using Base::m_isInitialized;
+  public:
+    using Base::_solve_impl;
+    typedef _MatrixType MatrixType;
+    typedef _OrderingType OrderingType;
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename MatrixType::RealScalar RealScalar;
+    typedef typename MatrixType::StorageIndex StorageIndex;
+    typedef SparseMatrix<Scalar,ColMajor,StorageIndex> QRMatrixType;
+    typedef Matrix<StorageIndex, Dynamic, 1> IndexVector;
+    typedef Matrix<Scalar, Dynamic, 1> ScalarVector;
+    typedef PermutationMatrix<Dynamic, Dynamic, StorageIndex> PermutationType;
+
+    enum {
+      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+    };
+    
+  public:
+    SparseQR () :  m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false)
+    { }
+    
+    /** Construct a QR factorization of the matrix \a mat.
+      * 
+      * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
+      * 
+      * \sa compute()
+      */
+    explicit SparseQR(const MatrixType& mat) : m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false)
+    {
+      compute(mat);
+    }
+    
+    /** Computes the QR factorization of the sparse matrix \a mat.
+      * 
+      * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
+      * 
+      * \sa analyzePattern(), factorize()
+      */
+    void compute(const MatrixType& mat)
+    {
+      analyzePattern(mat);
+      factorize(mat);
+    }
+    void analyzePattern(const MatrixType& mat);
+    void factorize(const MatrixType& mat);
+    
+    /** \returns the number of rows of the represented matrix. 
+      */
+    inline Index rows() const { return m_pmat.rows(); }
+    
+    /** \returns the number of columns of the represented matrix. 
+      */
+    inline Index cols() const { return m_pmat.cols();}
+    
+    /** \returns a const reference to the \b sparse upper triangular matrix R of the QR factorization.
+      * \warning The entries of the returned matrix are not sorted. This means that using it in algorithms
+      *          expecting sorted entries will fail. This include random coefficient accesses (SpaseMatrix::coeff()),
+      *          and coefficient-wise operations. Matrix products and triangular solves are fine though.
+      *
+      * To sort the entries, you can assign it to a row-major matrix, and if a column-major matrix
+      * is required, you can copy it again:
+      * \code
+      * SparseMatrix<double>          R  = qr.matrixR();  // column-major, not sorted!
+      * SparseMatrix<double,RowMajor> Rr = qr.matrixR();  // row-major, sorted
+      * SparseMatrix<double>          Rc = Rr;            // column-major, sorted
+      * \endcode
+      */
+    const QRMatrixType& matrixR() const { return m_R; }
+    
+    /** \returns the number of non linearly dependent columns as determined by the pivoting threshold.
+      *
+      * \sa setPivotThreshold()
+      */
+    Index rank() const
+    {
+      eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
+      return m_nonzeropivots; 
+    }
+    
+    /** \returns an expression of the matrix Q as products of sparse Householder reflectors.
+    * The common usage of this function is to apply it to a dense matrix or vector
+    * \code
+    * VectorXd B1, B2;
+    * // Initialize B1
+    * B2 = matrixQ() * B1;
+    * \endcode
+    *
+    * To get a plain SparseMatrix representation of Q:
+    * \code
+    * SparseMatrix<double> Q;
+    * Q = SparseQR<SparseMatrix<double> >(A).matrixQ();
+    * \endcode
+    * Internally, this call simply performs a sparse product between the matrix Q
+    * and a sparse identity matrix. However, due to the fact that the sparse
+    * reflectors are stored unsorted, two transpositions are needed to sort
+    * them before performing the product.
+    */
+    SparseQRMatrixQReturnType<SparseQR> matrixQ() const 
+    { return SparseQRMatrixQReturnType<SparseQR>(*this); }
+    
+    /** \returns a const reference to the column permutation P that was applied to A such that A*P = Q*R
+      * It is the combination of the fill-in reducing permutation and numerical column pivoting.
+      */
+    const PermutationType& colsPermutation() const
+    { 
+      eigen_assert(m_isInitialized && "Decomposition is not initialized.");
+      return m_outputPerm_c;
+    }
+    
+    /** \returns A string describing the type of error.
+      * This method is provided to ease debugging, not to handle errors.
+      */
+    std::string lastErrorMessage() const { return m_lastError; }
+    
+    /** \internal */
+    template<typename Rhs, typename Dest>
+    bool _solve_impl(const MatrixBase<Rhs> &B, MatrixBase<Dest> &dest) const
+    {
+      eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
+      eigen_assert(this->rows() == B.rows() && "SparseQR::solve() : invalid number of rows in the right hand side matrix");
+
+      Index rank = this->rank();
+      
+      // Compute Q^* * b;
+      typename Dest::PlainObject y, b;
+      y = this->matrixQ().adjoint() * B;
+      b = y;
+      
+      // Solve with the triangular matrix R
+      y.resize((std::max<Index>)(cols(),y.rows()),y.cols());
+      y.topRows(rank) = this->matrixR().topLeftCorner(rank, rank).template triangularView<Upper>().solve(b.topRows(rank));
+      y.bottomRows(y.rows()-rank).setZero();
+      
+      // Apply the column permutation
+      if (m_perm_c.size())  dest = colsPermutation() * y.topRows(cols());
+      else                  dest = y.topRows(cols());
+      
+      m_info = Success;
+      return true;
+    }
+
+    /** Sets the threshold that is used to determine linearly dependent columns during the factorization.
+      *
+      * In practice, if during the factorization the norm of the column that has to be eliminated is below
+      * this threshold, then the entire column is treated as zero, and it is moved at the end.
+      */
+    void setPivotThreshold(const RealScalar& threshold)
+    {
+      m_useDefaultThreshold = false;
+      m_threshold = threshold;
+    }
+    
+    /** \returns the solution X of \f$ A X = B \f$ using the current decomposition of A.
+      *
+      * \sa compute()
+      */
+    template<typename Rhs>
+    inline const Solve<SparseQR, Rhs> solve(const MatrixBase<Rhs>& B) const 
+    {
+      eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
+      eigen_assert(this->rows() == B.rows() && "SparseQR::solve() : invalid number of rows in the right hand side matrix");
+      return Solve<SparseQR, Rhs>(*this, B.derived());
+    }
+    template<typename Rhs>
+    inline const Solve<SparseQR, Rhs> solve(const SparseMatrixBase<Rhs>& B) const
+    {
+          eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
+          eigen_assert(this->rows() == B.rows() && "SparseQR::solve() : invalid number of rows in the right hand side matrix");
+          return Solve<SparseQR, Rhs>(*this, B.derived());
+    }
+    
+    /** \brief Reports whether previous computation was successful.
+      *
+      * \returns \c Success if computation was successful,
+      *          \c NumericalIssue if the QR factorization reports a numerical problem
+      *          \c InvalidInput if the input matrix is invalid
+      *
+      * \sa iparm()          
+      */
+    ComputationInfo info() const
+    {
+      eigen_assert(m_isInitialized && "Decomposition is not initialized.");
+      return m_info;
+    }
+
+
+    /** \internal */
+    inline void _sort_matrix_Q()
+    {
+      if(this->m_isQSorted) return;
+      // The matrix Q is sorted during the transposition
+      SparseMatrix<Scalar, RowMajor, Index> mQrm(this->m_Q);
+      this->m_Q = mQrm;
+      this->m_isQSorted = true;
+    }
+
+    
+  protected:
+    bool m_analysisIsok;
+    bool m_factorizationIsok;
+    mutable ComputationInfo m_info;
+    std::string m_lastError;
+    QRMatrixType m_pmat;            // Temporary matrix
+    QRMatrixType m_R;               // The triangular factor matrix
+    QRMatrixType m_Q;               // The orthogonal reflectors
+    ScalarVector m_hcoeffs;         // The Householder coefficients
+    PermutationType m_perm_c;       // Fill-reducing  Column  permutation
+    PermutationType m_pivotperm;    // The permutation for rank revealing
+    PermutationType m_outputPerm_c; // The final column permutation
+    RealScalar m_threshold;         // Threshold to determine null Householder reflections
+    bool m_useDefaultThreshold;     // Use default threshold
+    Index m_nonzeropivots;          // Number of non zero pivots found
+    IndexVector m_etree;            // Column elimination tree
+    IndexVector m_firstRowElt;      // First element in each row
+    bool m_isQSorted;               // whether Q is sorted or not
+    bool m_isEtreeOk;               // whether the elimination tree match the initial input matrix
+    
+    template <typename, typename > friend struct SparseQR_QProduct;
+    
+};
+
+/** \brief Preprocessing step of a QR factorization 
+  * 
+  * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
+  * 
+  * In this step, the fill-reducing permutation is computed and applied to the columns of A
+  * and the column elimination tree is computed as well. Only the sparsity pattern of \a mat is exploited.
+  * 
+  * \note In this step it is assumed that there is no empty row in the matrix \a mat.
+  */
+template <typename MatrixType, typename OrderingType>
+void SparseQR<MatrixType,OrderingType>::analyzePattern(const MatrixType& mat)
+{
+  eigen_assert(mat.isCompressed() && "SparseQR requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to SparseQR");
+  // Copy to a column major matrix if the input is rowmajor
+  typename internal::conditional<MatrixType::IsRowMajor,QRMatrixType,const MatrixType&>::type matCpy(mat);
+  // Compute the column fill reducing ordering
+  OrderingType ord; 
+  ord(matCpy, m_perm_c); 
+  Index n = mat.cols();
+  Index m = mat.rows();
+  Index diagSize = (std::min)(m,n);
+  
+  if (!m_perm_c.size())
+  {
+    m_perm_c.resize(n);
+    m_perm_c.indices().setLinSpaced(n, 0,StorageIndex(n-1));
+  }
+  
+  // Compute the column elimination tree of the permuted matrix
+  m_outputPerm_c = m_perm_c.inverse();
+  internal::coletree(matCpy, m_etree, m_firstRowElt, m_outputPerm_c.indices().data());
+  m_isEtreeOk = true;
+  
+  m_R.resize(m, n);
+  m_Q.resize(m, diagSize);
+  
+  // Allocate space for nonzero elements: rough estimation
+  m_R.reserve(2*mat.nonZeros()); //FIXME Get a more accurate estimation through symbolic factorization with the etree
+  m_Q.reserve(2*mat.nonZeros());
+  m_hcoeffs.resize(diagSize);
+  m_analysisIsok = true;
+}
+
+/** \brief Performs the numerical QR factorization of the input matrix
+  * 
+  * The function SparseQR::analyzePattern(const MatrixType&) must have been called beforehand with
+  * a matrix having the same sparsity pattern than \a mat.
+  * 
+  * \param mat The sparse column-major matrix
+  */
+template <typename MatrixType, typename OrderingType>
+void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
+{
+  using std::abs;
+  
+  eigen_assert(m_analysisIsok && "analyzePattern() should be called before this step");
+  StorageIndex m = StorageIndex(mat.rows());
+  StorageIndex n = StorageIndex(mat.cols());
+  StorageIndex diagSize = (std::min)(m,n);
+  IndexVector mark((std::max)(m,n)); mark.setConstant(-1);  // Record the visited nodes
+  IndexVector Ridx(n), Qidx(m);                             // Store temporarily the row indexes for the current column of R and Q
+  Index nzcolR, nzcolQ;                                     // Number of nonzero for the current column of R and Q
+  ScalarVector tval(m);                                     // The dense vector used to compute the current column
+  RealScalar pivotThreshold = m_threshold;
+  
+  m_R.setZero();
+  m_Q.setZero();
+  m_pmat = mat;
+  if(!m_isEtreeOk)
+  {
+    m_outputPerm_c = m_perm_c.inverse();
+    internal::coletree(m_pmat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data());
+    m_isEtreeOk = true;
+  }
+
+  m_pmat.uncompress(); // To have the innerNonZeroPtr allocated
+  
+  // Apply the fill-in reducing permutation lazily:
+  {
+    // If the input is row major, copy the original column indices,
+    // otherwise directly use the input matrix
+    // 
+    IndexVector originalOuterIndicesCpy;
+    const StorageIndex *originalOuterIndices = mat.outerIndexPtr();
+    if(MatrixType::IsRowMajor)
+    {
+      originalOuterIndicesCpy = IndexVector::Map(m_pmat.outerIndexPtr(),n+1);
+      originalOuterIndices = originalOuterIndicesCpy.data();
+    }
+    
+    for (int i = 0; i < n; i++)
+    {
+      Index p = m_perm_c.size() ? m_perm_c.indices()(i) : i;
+      m_pmat.outerIndexPtr()[p] = originalOuterIndices[i]; 
+      m_pmat.innerNonZeroPtr()[p] = originalOuterIndices[i+1] - originalOuterIndices[i]; 
+    }
+  }
+  
+  /* Compute the default threshold as in MatLab, see:
+   * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing
+   * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011, Page 8:3 
+   */
+  if(m_useDefaultThreshold) 
+  {
+    RealScalar max2Norm = 0.0;
+    for (int j = 0; j < n; j++) max2Norm = numext::maxi(max2Norm, m_pmat.col(j).norm());
+    if(max2Norm==RealScalar(0))
+      max2Norm = RealScalar(1);
+    pivotThreshold = 20 * (m + n) * max2Norm * NumTraits<RealScalar>::epsilon();
+  }
+  
+  // Initialize the numerical permutation
+  m_pivotperm.setIdentity(n);
+  
+  StorageIndex nonzeroCol = 0; // Record the number of valid pivots
+  m_Q.startVec(0);
+
+  // Left looking rank-revealing QR factorization: compute a column of R and Q at a time
+  for (StorageIndex col = 0; col < n; ++col)
+  {
+    mark.setConstant(-1);
+    m_R.startVec(col);
+    mark(nonzeroCol) = col;
+    Qidx(0) = nonzeroCol;
+    nzcolR = 0; nzcolQ = 1;
+    bool found_diag = nonzeroCol>=m;
+    tval.setZero(); 
+    
+    // Symbolic factorization: find the nonzero locations of the column k of the factors R and Q, i.e.,
+    // all the nodes (with indexes lower than rank) reachable through the column elimination tree (etree) rooted at node k.
+    // Note: if the diagonal entry does not exist, then its contribution must be explicitly added,
+    // thus the trick with found_diag that permits to do one more iteration on the diagonal element if this one has not been found.
+    for (typename QRMatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp)
+    {
+      StorageIndex curIdx = nonzeroCol;
+      if(itp) curIdx = StorageIndex(itp.row());
+      if(curIdx == nonzeroCol) found_diag = true;
+      
+      // Get the nonzeros indexes of the current column of R
+      StorageIndex st = m_firstRowElt(curIdx); // The traversal of the etree starts here
+      if (st < 0 )
+      {
+        m_lastError = "Empty row found during numerical factorization";
+        m_info = InvalidInput;
+        return;
+      }
+
+      // Traverse the etree 
+      Index bi = nzcolR;
+      for (; mark(st) != col; st = m_etree(st))
+      {
+        Ridx(nzcolR) = st;  // Add this row to the list,
+        mark(st) = col;     // and mark this row as visited
+        nzcolR++;
+      }
+
+      // Reverse the list to get the topological ordering
+      Index nt = nzcolR-bi;
+      for(Index i = 0; i < nt/2; i++) std::swap(Ridx(bi+i), Ridx(nzcolR-i-1));
+       
+      // Copy the current (curIdx,pcol) value of the input matrix
+      if(itp) tval(curIdx) = itp.value();
+      else    tval(curIdx) = Scalar(0);
+      
+      // Compute the pattern of Q(:,k)
+      if(curIdx > nonzeroCol && mark(curIdx) != col ) 
+      {
+        Qidx(nzcolQ) = curIdx;  // Add this row to the pattern of Q,
+        mark(curIdx) = col;     // and mark it as visited
+        nzcolQ++;
+      }
+    }
+
+    // Browse all the indexes of R(:,col) in reverse order
+    for (Index i = nzcolR-1; i >= 0; i--)
+    {
+      Index curIdx = Ridx(i);
+      
+      // Apply the curIdx-th householder vector to the current column (temporarily stored into tval)
+      Scalar tdot(0);
+      
+      // First compute q' * tval
+      tdot = m_Q.col(curIdx).dot(tval);
+
+      tdot *= m_hcoeffs(curIdx);
+      
+      // Then update tval = tval - q * tau
+      // FIXME: tval -= tdot * m_Q.col(curIdx) should amount to the same (need to check/add support for efficient "dense ?= sparse")
+      for (typename QRMatrixType::InnerIterator itq(m_Q, curIdx); itq; ++itq)
+        tval(itq.row()) -= itq.value() * tdot;
+
+      // Detect fill-in for the current column of Q
+      if(m_etree(Ridx(i)) == nonzeroCol)
+      {
+        for (typename QRMatrixType::InnerIterator itq(m_Q, curIdx); itq; ++itq)
+        {
+          StorageIndex iQ = StorageIndex(itq.row());
+          if (mark(iQ) != col)
+          {
+            Qidx(nzcolQ++) = iQ;  // Add this row to the pattern of Q,
+            mark(iQ) = col;       // and mark it as visited
+          }
+        }
+      }
+    } // End update current column
+    
+    Scalar tau = RealScalar(0);
+    RealScalar beta = 0;
+    
+    if(nonzeroCol < diagSize)
+    {
+      // Compute the Householder reflection that eliminate the current column
+      // FIXME this step should call the Householder module.
+      Scalar c0 = nzcolQ ? tval(Qidx(0)) : Scalar(0);
+      
+      // First, the squared norm of Q((col+1):m, col)
+      RealScalar sqrNorm = 0.;
+      for (Index itq = 1; itq < nzcolQ; ++itq) sqrNorm += numext::abs2(tval(Qidx(itq)));
+      if(sqrNorm == RealScalar(0) && numext::imag(c0) == RealScalar(0))
+      {
+        beta = numext::real(c0);
+        tval(Qidx(0)) = 1;
+      }
+      else
+      {
+        using std::sqrt;
+        beta = sqrt(numext::abs2(c0) + sqrNorm);
+        if(numext::real(c0) >= RealScalar(0))
+          beta = -beta;
+        tval(Qidx(0)) = 1;
+        for (Index itq = 1; itq < nzcolQ; ++itq)
+          tval(Qidx(itq)) /= (c0 - beta);
+        tau = numext::conj((beta-c0) / beta);
+          
+      }
+    }
+
+    // Insert values in R
+    for (Index  i = nzcolR-1; i >= 0; i--)
+    {
+      Index curIdx = Ridx(i);
+      if(curIdx < nonzeroCol) 
+      {
+        m_R.insertBackByOuterInnerUnordered(col, curIdx) = tval(curIdx);
+        tval(curIdx) = Scalar(0.);
+      }
+    }
+
+    if(nonzeroCol < diagSize && abs(beta) >= pivotThreshold)
+    {
+      m_R.insertBackByOuterInner(col, nonzeroCol) = beta;
+      // The householder coefficient
+      m_hcoeffs(nonzeroCol) = tau;
+      // Record the householder reflections
+      for (Index itq = 0; itq < nzcolQ; ++itq)
+      {
+        Index iQ = Qidx(itq);
+        m_Q.insertBackByOuterInnerUnordered(nonzeroCol,iQ) = tval(iQ);
+        tval(iQ) = Scalar(0.);
+      }
+      nonzeroCol++;
+      if(nonzeroCol<diagSize)
+        m_Q.startVec(nonzeroCol);
+    }
+    else
+    {
+      // Zero pivot found: move implicitly this column to the end
+      for (Index j = nonzeroCol; j < n-1; j++) 
+        std::swap(m_pivotperm.indices()(j), m_pivotperm.indices()[j+1]);
+      
+      // Recompute the column elimination tree
+      internal::coletree(m_pmat, m_etree, m_firstRowElt, m_pivotperm.indices().data());
+      m_isEtreeOk = false;
+    }
+  }
+  
+  m_hcoeffs.tail(diagSize-nonzeroCol).setZero();
+  
+  // Finalize the column pointers of the sparse matrices R and Q
+  m_Q.finalize();
+  m_Q.makeCompressed();
+  m_R.finalize();
+  m_R.makeCompressed();
+  m_isQSorted = false;
+
+  m_nonzeropivots = nonzeroCol;
+  
+  if(nonzeroCol<n)
+  {
+    // Permute the triangular factor to put the 'dead' columns to the end
+    QRMatrixType tempR(m_R);
+    m_R = tempR * m_pivotperm;
+    
+    // Update the column permutation
+    m_outputPerm_c = m_outputPerm_c * m_pivotperm;
+  }
+  
+  m_isInitialized = true; 
+  m_factorizationIsok = true;
+  m_info = Success;
+}
+
+template <typename SparseQRType, typename Derived>
+struct SparseQR_QProduct : ReturnByValue<SparseQR_QProduct<SparseQRType, Derived> >
+{
+  typedef typename SparseQRType::QRMatrixType MatrixType;
+  typedef typename SparseQRType::Scalar Scalar;
+  // Get the references 
+  SparseQR_QProduct(const SparseQRType& qr, const Derived& other, bool transpose) : 
+  m_qr(qr),m_other(other),m_transpose(transpose) {}
+  inline Index rows() const { return m_qr.matrixQ().rows(); }
+  inline Index cols() const { return m_other.cols(); }
+  
+  // Assign to a vector
+  template<typename DesType>
+  void evalTo(DesType& res) const
+  {
+    Index m = m_qr.rows();
+    Index n = m_qr.cols();
+    Index diagSize = (std::min)(m,n);
+    res = m_other;
+    if (m_transpose)
+    {
+      eigen_assert(m_qr.m_Q.rows() == m_other.rows() && "Non conforming object sizes");
+      //Compute res = Q' * other column by column
+      for(Index j = 0; j < res.cols(); j++){
+        for (Index k = 0; k < diagSize; k++)
+        {
+          Scalar tau = Scalar(0);
+          tau = m_qr.m_Q.col(k).dot(res.col(j));
+          if(tau==Scalar(0)) continue;
+          tau = tau * m_qr.m_hcoeffs(k);
+          res.col(j) -= tau * m_qr.m_Q.col(k);
+        }
+      }
+    }
+    else
+    {
+      eigen_assert(m_qr.matrixQ().cols() == m_other.rows() && "Non conforming object sizes");
+
+      res.conservativeResize(rows(), cols());
+
+      // Compute res = Q * other column by column
+      for(Index j = 0; j < res.cols(); j++)
+      {
+        Index start_k = internal::is_identity<Derived>::value ? numext::mini(j,diagSize-1) : diagSize-1;
+        for (Index k = start_k; k >=0; k--)
+        {
+          Scalar tau = Scalar(0);
+          tau = m_qr.m_Q.col(k).dot(res.col(j));
+          if(tau==Scalar(0)) continue;
+          tau = tau * numext::conj(m_qr.m_hcoeffs(k));
+          res.col(j) -= tau * m_qr.m_Q.col(k);
+        }
+      }
+    }
+  }
+  
+  const SparseQRType& m_qr;
+  const Derived& m_other;
+  bool m_transpose; // TODO this actually means adjoint
+};
+
+template<typename SparseQRType>
+struct SparseQRMatrixQReturnType : public EigenBase<SparseQRMatrixQReturnType<SparseQRType> >
+{  
+  typedef typename SparseQRType::Scalar Scalar;
+  typedef Matrix<Scalar,Dynamic,Dynamic> DenseMatrix;
+  enum {
+    RowsAtCompileTime = Dynamic,
+    ColsAtCompileTime = Dynamic
+  };
+  explicit SparseQRMatrixQReturnType(const SparseQRType& qr) : m_qr(qr) {}
+  template<typename Derived>
+  SparseQR_QProduct<SparseQRType, Derived> operator*(const MatrixBase<Derived>& other)
+  {
+    return SparseQR_QProduct<SparseQRType,Derived>(m_qr,other.derived(),false);
+  }
+  // To use for operations with the adjoint of Q
+  SparseQRMatrixQTransposeReturnType<SparseQRType> adjoint() const
+  {
+    return SparseQRMatrixQTransposeReturnType<SparseQRType>(m_qr);
+  }
+  inline Index rows() const { return m_qr.rows(); }
+  inline Index cols() const { return m_qr.rows(); }
+  // To use for operations with the transpose of Q FIXME this is the same as adjoint at the moment
+  SparseQRMatrixQTransposeReturnType<SparseQRType> transpose() const
+  {
+    return SparseQRMatrixQTransposeReturnType<SparseQRType>(m_qr);
+  }
+  const SparseQRType& m_qr;
+};
+
+// TODO this actually represents the adjoint of Q
+template<typename SparseQRType>
+struct SparseQRMatrixQTransposeReturnType
+{
+  explicit SparseQRMatrixQTransposeReturnType(const SparseQRType& qr) : m_qr(qr) {}
+  template<typename Derived>
+  SparseQR_QProduct<SparseQRType,Derived> operator*(const MatrixBase<Derived>& other)
+  {
+    return SparseQR_QProduct<SparseQRType,Derived>(m_qr,other.derived(), true);
+  }
+  const SparseQRType& m_qr;
+};
+
+namespace internal {
+  
+template<typename SparseQRType>
+struct evaluator_traits<SparseQRMatrixQReturnType<SparseQRType> >
+{
+  typedef typename SparseQRType::MatrixType MatrixType;
+  typedef typename storage_kind_to_evaluator_kind<typename MatrixType::StorageKind>::Kind Kind;
+  typedef SparseShape Shape;
+};
+
+template< typename DstXprType, typename SparseQRType>
+struct Assignment<DstXprType, SparseQRMatrixQReturnType<SparseQRType>, internal::assign_op<typename DstXprType::Scalar,typename DstXprType::Scalar>, Sparse2Sparse>
+{
+  typedef SparseQRMatrixQReturnType<SparseQRType> SrcXprType;
+  typedef typename DstXprType::Scalar Scalar;
+  typedef typename DstXprType::StorageIndex StorageIndex;
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &/*func*/)
+  {
+    typename DstXprType::PlainObject idMat(src.rows(), src.cols());
+    idMat.setIdentity();
+    // Sort the sparse householder reflectors if needed
+    const_cast<SparseQRType *>(&src.m_qr)->_sort_matrix_Q();
+    dst = SparseQR_QProduct<SparseQRType, DstXprType>(src.m_qr, idMat, false);
+  }
+};
+
+template< typename DstXprType, typename SparseQRType>
+struct Assignment<DstXprType, SparseQRMatrixQReturnType<SparseQRType>, internal::assign_op<typename DstXprType::Scalar,typename DstXprType::Scalar>, Sparse2Dense>
+{
+  typedef SparseQRMatrixQReturnType<SparseQRType> SrcXprType;
+  typedef typename DstXprType::Scalar Scalar;
+  typedef typename DstXprType::StorageIndex StorageIndex;
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &/*func*/)
+  {
+    dst = src.m_qr.matrixQ() * DstXprType::Identity(src.m_qr.rows(), src.m_qr.rows());
+  }
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdDeque.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdDeque.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdDeque.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdDeque.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdList.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdList.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdList.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdList.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdVector.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdVector.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdVector.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/StdVector.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/details.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/details.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/details.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/StlSupport/details.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/misc/Image.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/Image.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/misc/Image.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/Image.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/misc/Kernel.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/Kernel.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/misc/Kernel.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/Kernel.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/misc/RealSvd2x2.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/RealSvd2x2.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/misc/RealSvd2x2.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/misc/RealSvd2x2.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseBinaryOps.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseBinaryOps.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseBinaryOps.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseBinaryOps.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseUnaryOps.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseUnaryOps.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseUnaryOps.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ArrayCwiseUnaryOps.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/BlockMethods.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/BlockMethods.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/BlockMethods.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/BlockMethods.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseBinaryOps.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseBinaryOps.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseBinaryOps.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseBinaryOps.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseUnaryOps.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseUnaryOps.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseUnaryOps.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/CommonCwiseUnaryOps.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/IndexedViewMethods.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/IndexedViewMethods.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/IndexedViewMethods.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/IndexedViewMethods.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseBinaryOps.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseBinaryOps.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseBinaryOps.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseBinaryOps.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseUnaryOps.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseUnaryOps.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseUnaryOps.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/MatrixCwiseUnaryOps.h
diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ReshapedMethods.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ReshapedMethods.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ReshapedMethods.h
rename to wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/plugins/ReshapedMethods.h
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/MatrixFunctions b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/MatrixFunctions
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/unsupported/Eigen/MatrixFunctions
rename to wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/MatrixFunctions
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
rename to wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
rename to wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
rename to wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
rename to wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
rename to wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
diff --git a/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/StemFunction.h b/wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/StemFunction.h
similarity index 100%
rename from wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/StemFunction.h
rename to wpimath/src/main/native/thirdparty/eigen/include/unsupported/Eigen/src/MatrixFunctions/StemFunction.h
diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp
new file mode 100644
index 0000000..cb28ff0
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem.hpp
@@ -0,0 +1,104 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_HPP
+#define _gcem_HPP
+
+#include "gcem_incl/gcem_options.hpp"
+
+namespace gcem
+{
+    #include "gcem_incl/quadrature/gauss_legendre_50.hpp"
+
+    #include "gcem_incl/is_inf.hpp"
+    #include "gcem_incl/is_nan.hpp"
+    #include "gcem_incl/is_finite.hpp"
+    
+    #include "gcem_incl/signbit.hpp"
+    #include "gcem_incl/copysign.hpp"
+    #include "gcem_incl/neg_zero.hpp"
+    #include "gcem_incl/sgn.hpp"
+
+    #include "gcem_incl/abs.hpp"
+    #include "gcem_incl/ceil.hpp"
+    #include "gcem_incl/floor.hpp"
+    #include "gcem_incl/trunc.hpp"
+    #include "gcem_incl/is_odd.hpp"
+    #include "gcem_incl/is_even.hpp"
+    #include "gcem_incl/max.hpp"
+    #include "gcem_incl/min.hpp"
+    #include "gcem_incl/sqrt.hpp"
+    #include "gcem_incl/inv_sqrt.hpp"
+    #include "gcem_incl/hypot.hpp"
+
+    #include "gcem_incl/find_exponent.hpp"
+    #include "gcem_incl/find_fraction.hpp"
+    #include "gcem_incl/find_whole.hpp"
+    #include "gcem_incl/mantissa.hpp"
+    #include "gcem_incl/round.hpp"
+    #include "gcem_incl/fmod.hpp"
+
+    #include "gcem_incl/pow_integral.hpp"
+    #include "gcem_incl/exp.hpp"
+    #include "gcem_incl/expm1.hpp"
+    #include "gcem_incl/log.hpp"
+    #include "gcem_incl/log1p.hpp"
+    #include "gcem_incl/log2.hpp"
+    #include "gcem_incl/log10.hpp"
+    #include "gcem_incl/pow.hpp"
+
+    #include "gcem_incl/gcd.hpp"
+    #include "gcem_incl/lcm.hpp"
+
+    #include "gcem_incl/tan.hpp"
+    #include "gcem_incl/cos.hpp"
+    #include "gcem_incl/sin.hpp"
+
+    #include "gcem_incl/atan.hpp"
+    #include "gcem_incl/atan2.hpp"
+    #include "gcem_incl/acos.hpp"
+    #include "gcem_incl/asin.hpp"
+
+    #include "gcem_incl/tanh.hpp"
+    #include "gcem_incl/cosh.hpp"
+    #include "gcem_incl/sinh.hpp"
+
+    #include "gcem_incl/atanh.hpp"
+    #include "gcem_incl/acosh.hpp"
+    #include "gcem_incl/asinh.hpp"
+
+    #include "gcem_incl/binomial_coef.hpp"
+    #include "gcem_incl/lgamma.hpp"
+    #include "gcem_incl/tgamma.hpp"
+    #include "gcem_incl/factorial.hpp"
+    #include "gcem_incl/lbeta.hpp"
+    #include "gcem_incl/beta.hpp"
+    #include "gcem_incl/lmgamma.hpp"
+    #include "gcem_incl/log_binomial_coef.hpp"
+
+    #include "gcem_incl/erf.hpp"
+    #include "gcem_incl/erf_inv.hpp"
+    #include "gcem_incl/incomplete_beta.hpp"
+    #include "gcem_incl/incomplete_beta_inv.hpp"
+    #include "gcem_incl/incomplete_gamma.hpp"
+    #include "gcem_incl/incomplete_gamma_inv.hpp"
+}
+
+#endif
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
new file mode 100644
index 0000000..0a3ada6
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/abs.hpp
@@ -0,0 +1,45 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_abs_HPP
+#define _gcem_abs_HPP
+
+/**
+ * Compile-time absolute value function
+ *
+ * @param x a real-valued input.
+ * @return the absolute value of \c x, \f$ |x| \f$.
+ */
+
+template<typename T>
+constexpr
+T
+abs(const T x)
+noexcept
+{
+    return( // deal with signed-zeros
+            x == T(0) ? \
+                T(0) :
+            // else
+            x < T(0) ? \
+                - x : x );
+}
+
+#endif
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
new file mode 100644
index 0000000..9d3bc07
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acos.hpp
@@ -0,0 +1,84 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time arccosine function
+ */
+
+#ifndef _gcem_acos_HPP
+#define _gcem_acos_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+acos_compute(const T x)
+noexcept
+{
+    return( // only defined on [-1,1]
+            abs(x) > T(1) ? \
+                GCLIM<T>::quiet_NaN() :
+            // indistinguishable from one or zero
+            GCLIM<T>::min() > abs(x -  T(1)) ? \
+                T(0) :
+            GCLIM<T>::min() > abs(x) ? \
+                T(GCEM_HALF_PI) :
+            // else
+                atan( sqrt(T(1) - x*x)/x ) );
+}
+
+template<typename T>
+constexpr
+T
+acos_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            //
+            x > T(0) ? \
+            // if
+                acos_compute(x) :
+            // else 
+                T(GCEM_PI) - acos_compute(-x) );
+}
+
+}
+
+/**
+ * Compile-time arccosine function
+ *
+ * @param x a real-valued input, where \f$ x \in [-1,1] \f$.
+ * @return the inverse cosine function using \f[ \text{acos}(x) = \text{atan} \left( \frac{\sqrt{1-x^2}}{x} \right) \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+acos(const T x)
+noexcept
+{
+    return internal::acos_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..79579d7
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/acosh.hpp
@@ -0,0 +1,68 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time inverse hyperbolic cosine function
+ */
+
+#ifndef _gcem_acosh_HPP
+#define _gcem_acosh_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+acosh_compute(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // function defined for x >= 1
+            x < T(1) ? \
+                GCLIM<T>::quiet_NaN() :
+            // indistinguishable from 1
+            GCLIM<T>::min() > abs(x - T(1)) ? \
+                T(0) :
+            // else
+                log( x + sqrt(x*x - T(1)) ) );
+}
+
+}
+
+/**
+ * Compile-time inverse hyperbolic cosine function
+ *
+ * @param x a real-valued input.
+ * @return the inverse hyperbolic cosine function using \f[ \text{acosh}(x) = \ln \left( x + \sqrt{x^2 - 1} \right) \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+acosh(const T x)
+noexcept
+{
+    return internal::acosh_compute( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..210d9fc
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asin.hpp
@@ -0,0 +1,82 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time arcsine function
+ */
+
+#ifndef _gcem_asin_HPP
+#define _gcem_asin_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+asin_compute(const T x)
+noexcept
+{
+    return( // only defined on [-1,1]
+            x > T(1) ? \
+                GCLIM<T>::quiet_NaN() :
+            // indistinguishable from one or zero
+            GCLIM<T>::min() > abs(x -  T(1)) ? \
+                T(GCEM_HALF_PI) :
+            GCLIM<T>::min() > abs(x) ? \
+                T(0) :
+            // else
+                atan( x/sqrt(T(1) - x*x) ) );
+}
+
+template<typename T>
+constexpr
+T
+asin_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            //
+            x < T(0) ? \
+                - asin_compute(-x) : 
+                  asin_compute(x) );
+}
+
+}
+
+/**
+ * Compile-time arcsine function
+ *
+ * @param x a real-valued input, where \f$ x \in [-1,1] \f$.
+ * @return the inverse sine function using \f[ \text{asin}(x) = \text{atan} \left( \frac{x}{\sqrt{1-x^2}} \right) \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+asin(const T x)
+noexcept
+{
+    return internal::asin_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..dfad57e
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/asinh.hpp
@@ -0,0 +1,66 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time inverse hyperbolic sine function
+ */
+
+#ifndef _gcem_asinh_HPP
+#define _gcem_asinh_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+asinh_compute(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // indistinguishable from zero
+            GCLIM<T>::min() > abs(x) ? \
+                T(0) :
+            // else
+                log( x + sqrt(x*x + T(1)) ) );
+}
+
+}
+
+/**
+ * Compile-time inverse hyperbolic sine function
+ *
+ * @param x a real-valued input.
+ * @return the inverse hyperbolic sine function using \f[ \text{asinh}(x) = \ln \left( x + \sqrt{x^2 + 1} \right) \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+asinh(const T x)
+noexcept
+{
+    return internal::asinh_compute( static_cast<return_t<T>>(x) );
+}
+
+
+#endif
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
new file mode 100644
index 0000000..9aea85b
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan.hpp
@@ -0,0 +1,155 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time arctangent function
+ */
+
+// see
+// http://functions.wolfram.com/ElementaryFunctions/ArcTan/10/0001/
+// http://functions.wolfram.com/ElementaryFunctions/ArcTan/06/01/06/01/0002/
+
+#ifndef _gcem_atan_HPP
+#define _gcem_atan_HPP
+
+namespace internal
+{
+
+// Series
+
+template<typename T>
+constexpr
+T
+atan_series_order_calc(const T x, const T x_pow, const uint_t order)
+noexcept
+{
+    return( T(1)/( T((order-1)*4 - 1) * x_pow ) \
+              - T(1)/( T((order-1)*4 + 1) * x_pow*x) );
+}
+
+template<typename T>
+constexpr
+T
+atan_series_order(const T x, const T x_pow, const uint_t order, const uint_t max_order)
+noexcept
+{
+    return( order == 1 ? \
+                GCEM_HALF_PI - T(1)/x + atan_series_order(x*x,pow(x,3),order+1,max_order) :
+            // NOTE: x changes to x*x for order > 1
+            order < max_order ? \
+                atan_series_order_calc(x,x_pow,order) \
+                    + atan_series_order(x,x_pow*x*x,order+1,max_order) :
+            // order == max_order
+                atan_series_order_calc(x,x_pow,order) );
+}
+
+template<typename T>
+constexpr
+T
+atan_series_main(const T x)
+noexcept
+{
+    return( x < T(3)    ? atan_series_order(x,x,1U,10U) :  // O(1/x^39)
+            x < T(4)    ? atan_series_order(x,x,1U,9U)  :  // O(1/x^35)
+            x < T(5)    ? atan_series_order(x,x,1U,8U)  :  // O(1/x^31)
+            x < T(7)    ? atan_series_order(x,x,1U,7U)  :  // O(1/x^27)
+            x < T(11)   ? atan_series_order(x,x,1U,6U)  :  // O(1/x^23)
+            x < T(25)   ? atan_series_order(x,x,1U,5U)  :  // O(1/x^19)
+            x < T(100)  ? atan_series_order(x,x,1U,4U)  :  // O(1/x^15)
+            x < T(1000) ? atan_series_order(x,x,1U,3U)  :  // O(1/x^11)
+                          atan_series_order(x,x,1U,2U) );  // O(1/x^7)
+}
+
+// CF
+
+template<typename T>
+constexpr
+T
+atan_cf_recur(const T xx, const uint_t depth, const uint_t max_depth)
+noexcept
+{
+    return( depth < max_depth ? \
+            // if
+                T(2*depth - 1) + depth*depth*xx/atan_cf_recur(xx,depth+1,max_depth) :
+            // else
+                T(2*depth - 1) );
+}
+
+template<typename T>
+constexpr
+T
+atan_cf_main(const T x)
+noexcept
+{
+    return( x < T(0.5) ? x/atan_cf_recur(x*x,1U, 15U ) : 
+            x < T(1)   ? x/atan_cf_recur(x*x,1U, 25U ) : 
+            x < T(1.5) ? x/atan_cf_recur(x*x,1U, 35U ) : 
+            x < T(2)   ? x/atan_cf_recur(x*x,1U, 45U ) : 
+                         x/atan_cf_recur(x*x,1U, 52U ) );
+}
+
+//
+
+template<typename T>
+constexpr
+T
+atan_begin(const T x)
+noexcept
+{
+    return( x > T(2.5) ? atan_series_main(x) : atan_cf_main(x) );
+}
+
+template<typename T>
+constexpr
+T
+atan_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // indistinguishable from zero
+            GCLIM<T>::min() > abs(x) ? \
+                T(0) :
+            // negative or positive
+            x < T(0) ? \
+                - atan_begin(-x) :
+                  atan_begin( x) );
+}
+
+}
+
+/**
+ * Compile-time arctangent function
+ *
+ * @param x a real-valued input.
+ * @return the inverse tangent function using \f[ \text{atan}(x) = \dfrac{x}{1 + \dfrac{x^2}{3 + \dfrac{4x^2}{5 + \dfrac{9x^2}{7 + \ddots}}}} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+atan(const T x)
+noexcept
+{
+    return internal::atan_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..97c8d6a
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atan2.hpp
@@ -0,0 +1,88 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time two-argument arctangent function
+ */
+
+#ifndef _gcem_atan2_HPP
+#define _gcem_atan2_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+atan2_compute(const T y, const T x)
+noexcept
+{
+    return( // NaN check
+            any_nan(y,x) ? \
+                GCLIM<T>::quiet_NaN() :
+            //
+            GCLIM<T>::min() > abs(x) ? \
+            //
+                GCLIM<T>::min() > abs(y) ? \
+                    neg_zero(y) ? \
+                        neg_zero(x) ? - T(GCEM_PI) : - T(0) :
+                        neg_zero(x) ?   T(GCEM_PI) :   T(0) :
+                y > T(0) ? \
+                    T(GCEM_HALF_PI) : - T(GCEM_HALF_PI) :
+            //
+            x < T(0) ? \
+                y < T(0) ? \
+                    atan(y/x) - T(GCEM_PI) : 
+                    atan(y/x) + T(GCEM_PI) :
+            //
+                atan(y/x) );
+}
+
+template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
+constexpr
+TC
+atan2_type_check(const T1 y, const T2 x)
+noexcept
+{
+    return atan2_compute(static_cast<TC>(x),static_cast<TC>(y));
+}
+
+}
+
+/**
+ * Compile-time two-argument arctangent function
+ *
+ * @param y a real-valued input.
+ * @param x a real-valued input.
+ * @return \f[ \text{atan2}(y,x) = \begin{cases} \text{atan}(y/x) & \text{ if } x > 0 \\ \text{atan}(y/x) + \pi & \text{ if } x < 0 \text{ and } y \geq 0 \\ \text{atan}(y/x) - \pi & \text{ if } x < 0 \text{ and } y < 0 \\ + \pi/2 & \text{ if } x = 0 \text{ and } y > 0 \\ - \pi/2 & \text{ if } x = 0 \text{ and } y < 0 \end{cases} \f]
+ * The function is undefined at the origin, however the following conventions are used.
+ * \f[ \text{atan2}(y,x) = \begin{cases} +0 & \text{ if } x = +0 \text{ and } y = +0 \\ -0 & \text{ if } x = +0 \text{ and } y = -0 \\ +\pi & \text{ if } x = -0 \text{ and } y = +0 \\ - \pi & \text{ if } x = -0 \text{ and } y = -0 \end{cases} \f]
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_return_t<T1,T2>
+atan2(const T1 y, const T2 x)
+noexcept
+{
+    return internal::atan2_type_check(x,y);
+}
+
+#endif
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
new file mode 100644
index 0000000..2e1cb76
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/atanh.hpp
@@ -0,0 +1,79 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time inverse hyperbolic tangent function
+ */
+
+#ifndef _gcem_atanh_HPP
+#define _gcem_atanh_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+atanh_compute(const T x)
+noexcept
+{
+    return( log( (T(1) + x)/(T(1) - x) ) / T(2) );
+}
+
+template<typename T>
+constexpr
+T
+atanh_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // function is defined for |x| < 1
+            T(1) < abs(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            GCLIM<T>::min() > (T(1) - abs(x)) ? \
+                sgn(x)*GCLIM<T>::infinity() :
+            // indistinguishable from zero
+            GCLIM<T>::min() > abs(x) ? \
+                T(0) :
+            // else
+                atanh_compute(x) );
+}
+
+}
+
+/**
+ * Compile-time inverse hyperbolic tangent function
+ *
+ * @param x a real-valued input.
+ * @return the inverse hyperbolic tangent function using \f[ \text{atanh}(x) = \frac{1}{2} \ln \left( \frac{1+x}{1-x} \right) \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+atanh(const T x)
+noexcept
+{
+    return internal::atanh_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..e888e47
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/beta.hpp
@@ -0,0 +1,42 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_beta_HPP
+#define _gcem_beta_HPP
+
+/**
+ * Compile-time beta function
+ *
+ * @param a a real-valued input.
+ * @param b a real-valued input.
+ * @return the beta function using \f[ \text{B}(\alpha,\beta) := \int_0^1 t^{\alpha - 1} (1-t)^{\beta - 1} dt = \frac{\Gamma(\alpha)\Gamma(\beta)}{\Gamma(\alpha + \beta)} \f]
+ * where \f$ \Gamma \f$ denotes the gamma function.
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_return_t<T1,T2>
+beta(const T1 a, const T2 b)
+noexcept
+{
+    return exp( lbeta(a,b) );
+}
+
+#endif
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
new file mode 100644
index 0000000..fb050a2
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/binomial_coef.hpp
@@ -0,0 +1,91 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_binomial_coef_HPP
+#define _gcem_binomial_coef_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+binomial_coef_recur(const T n, const T k)
+noexcept
+{
+    return( // edge cases
+                (k == T(0) || n == k) ? T(1) : // deals with 0 choose 0 case
+                n == T(0) ? T(0) :
+            // else
+                binomial_coef_recur(n-1,k-1) + binomial_coef_recur(n-1,k) );
+}
+
+template<typename T, typename std::enable_if<std::is_integral<T>::value>::type* = nullptr>
+constexpr
+T
+binomial_coef_check(const T n, const T k)
+noexcept
+{
+    return binomial_coef_recur(n,k);
+}
+
+template<typename T, typename std::enable_if<!std::is_integral<T>::value>::type* = nullptr>
+constexpr
+T
+binomial_coef_check(const T n, const T k)
+noexcept
+{
+    return( // NaN check; removed due to MSVC problems; template not being ignored in <int> cases
+            // (is_nan(n) || is_nan(k)) ? GCLIM<T>::quiet_NaN() :
+            //
+            static_cast<T>(binomial_coef_recur(static_cast<ullint_t>(n),static_cast<ullint_t>(k))) );
+}
+
+template<typename T1, typename T2, typename TC = common_t<T1,T2>>
+constexpr
+TC
+binomial_coef_type_check(const T1 n, const T2 k)
+noexcept
+{
+    return binomial_coef_check(static_cast<TC>(n),static_cast<TC>(k));
+}
+
+}
+
+/**
+ * Compile-time binomial coefficient
+ *
+ * @param n integral-valued input.
+ * @param k integral-valued input.
+ * @return computes the Binomial coefficient
+ * \f[ \binom{n}{k} = \frac{n!}{k!(n-k)!} \f]
+ * also known as '\c n choose \c k '.
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_t<T1,T2>
+binomial_coef(const T1 n, const T2 k)
+noexcept
+{
+    return internal::binomial_coef_type_check(n,k);
+}
+
+#endif
\ No newline at end of file
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
new file mode 100644
index 0000000..e8570ab
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/ceil.hpp
@@ -0,0 +1,130 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_ceil_HPP
+#define _gcem_ceil_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+int
+ceil_resid(const T x, const T x_whole)
+noexcept
+{
+    return( (x > T(0)) && (x > x_whole) );
+}
+
+template<typename T>
+constexpr
+T
+ceil_int(const T x, const T x_whole)
+noexcept
+{
+    return( x_whole + static_cast<T>(ceil_resid(x,x_whole)) );
+}
+
+template<typename T>
+constexpr
+T
+ceil_check_internal(const T x)
+noexcept
+{
+    return x;
+}
+
+template<>
+constexpr
+float
+ceil_check_internal<float>(const float x)
+noexcept
+{
+    return( abs(x) >= 8388608.f ? \
+            // if
+                x : \
+            // else
+                ceil_int(x, float(static_cast<int>(x))) );
+}
+
+template<>
+constexpr
+double
+ceil_check_internal<double>(const double x)
+noexcept
+{
+    return( abs(x) >= 4503599627370496. ? \
+            // if
+                x : \
+            // else
+                ceil_int(x, double(static_cast<llint_t>(x))) );
+}
+
+template<>
+constexpr
+long double
+ceil_check_internal<long double>(const long double x)
+noexcept
+{
+    return( abs(x) >= 9223372036854775808.l ? \
+            // if
+                x : \
+            // else
+                ceil_int(x, ((long double)static_cast<ullint_t>(abs(x))) * sgn(x)) );
+}
+
+template<typename T>
+constexpr
+T
+ceil_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // +/- infinite
+            !is_finite(x) ? \
+                x :
+            // signed-zero cases
+            GCLIM<T>::min() > abs(x) ? \
+                x :
+            // else
+                ceil_check_internal(x) );
+}
+
+}
+
+/**
+ * Compile-time ceil function
+ *
+ * @param x a real-valued input.
+ * @return computes the ceiling-value of the input.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+ceil(const T x)
+noexcept
+{
+    return internal::ceil_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..a3bab74
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/copysign.hpp
@@ -0,0 +1,41 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_copysign_HPP
+#define _gcem_copysign_HPP
+
+/**
+ * Compile-time copy sign function
+ *
+ * @param x a real-valued input
+ * @param y a real-valued input
+ * @return replace the signbit of \c x with the signbit of \c y.
+ */
+
+template <typename T1, typename T2>
+constexpr
+T1
+copysign(const T1 x, const T2 y)
+noexcept
+{
+    return( signbit(x) != signbit(y) ? -x : x );
+}
+
+#endif
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
new file mode 100644
index 0000000..0e98012
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cos.hpp
@@ -0,0 +1,83 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time cosine function using tan(x/2)
+ */
+
+#ifndef _gcem_cos_HPP
+#define _gcem_cos_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+cos_compute(const T x)
+noexcept
+{
+    return( T(1) - x*x)/(T(1) + x*x );
+}
+
+template<typename T>
+constexpr
+T
+cos_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // indistinguishable from 0
+            GCLIM<T>::min() > abs(x) ? 
+                T(1) :
+            // special cases: pi/2 and pi
+            GCLIM<T>::min() > abs(x - T(GCEM_HALF_PI)) ? \
+                T(0) :
+            GCLIM<T>::min() > abs(x + T(GCEM_HALF_PI)) ? \
+                T(0) :
+            GCLIM<T>::min() > abs(x - T(GCEM_PI)) ? \
+                - T(1) :
+            GCLIM<T>::min() > abs(x + T(GCEM_PI)) ? \
+                - T(1) :
+            // else
+                cos_compute( tan(x/T(2)) ) );
+}
+
+}
+
+/**
+ * Compile-time cosine function
+ *
+ * @param x a real-valued input.
+ * @return the cosine function using \f[ \cos(x) = \frac{1-\tan^2(x/2)}{1+\tan^2(x/2)} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+cos(const T x)
+noexcept
+{
+    return internal::cos_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..b3cebb8
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/cosh.hpp
@@ -0,0 +1,65 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time hyperbolic cosine function
+ */
+
+#ifndef _gcem_cosh_HPP
+#define _gcem_cosh_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+cosh_compute(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // indistinguishable from zero
+            GCLIM<T>::min() > abs(x) ? \
+                T(1) : 
+            // else
+                (exp(x) + exp(-x)) / T(2) );
+}
+
+}
+
+/**
+ * Compile-time hyperbolic cosine function
+ *
+ * @param x a real-valued input.
+ * @return the hyperbolic cosine function using \f[ \cosh(x) = \frac{\exp(x) + \exp(-x)}{2} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+cosh(const T x)
+noexcept
+{
+    return internal::cosh_compute( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..afec09e
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf.hpp
@@ -0,0 +1,143 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time error function
+ */
+
+#ifndef _gcem_erf_HPP
+#define _gcem_erf_HPP
+
+namespace internal
+{
+
+// see
+// http://functions.wolfram.com/GammaBetaErf/Erf/10/01/0007/
+
+template<typename T>
+constexpr
+T
+erf_cf_large_recur(const T x, const int depth)
+noexcept
+{
+    return( depth < GCEM_ERF_MAX_ITER ? \
+            // if
+                x + 2*depth/erf_cf_large_recur(x,depth+1) :
+            // else
+                x );
+}
+
+template<typename T>
+constexpr
+T
+erf_cf_large_main(const T x)
+noexcept
+{
+    return( T(1) - T(2) * ( exp(-x*x) / T(GCEM_SQRT_PI) ) \
+                / erf_cf_large_recur(T(2)*x,1) );
+}
+
+// see
+// http://functions.wolfram.com/GammaBetaErf/Erf/10/01/0005/
+
+template<typename T>
+constexpr
+T
+erf_cf_small_recur(const T xx, const int depth)
+noexcept
+{
+    return( depth < GCEM_ERF_MAX_ITER ? \
+            // if
+                (2*depth - T(1)) - 2*xx \
+                    + 4*depth*xx / erf_cf_small_recur(xx,depth+1) :
+            // else
+                (2*depth - T(1)) - 2*xx );
+}
+
+template<typename T>
+constexpr
+T
+erf_cf_small_main(const T x)
+noexcept
+{
+    return( T(2) * x * ( exp(-x*x) / T(GCEM_SQRT_PI) ) \
+                / erf_cf_small_recur(x*x,1) );
+}
+
+//
+
+template<typename T>
+constexpr
+T
+erf_begin(const T x)
+noexcept
+{
+    return( x > T(2.1) ? \
+            // if
+                erf_cf_large_main(x) :
+            // else
+                erf_cf_small_main(x) );
+}
+
+template<typename T>
+constexpr
+T
+erf_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // +/-Inf
+            is_posinf(x) ? \
+                T(1) :
+            is_neginf(x) ? \
+                - T(1) :
+            // indistinguishable from zero
+            GCLIM<T>::min() > abs(x) ? \
+                T(0) :
+            // else
+                x < T(0) ? \
+                    - erf_begin(-x) : 
+                      erf_begin( x) );
+}
+
+}
+
+/**
+ * Compile-time Gaussian error function
+ *
+ * @param x a real-valued input.
+ * @return computes the Gaussian error function
+ * \f[ \text{erf}(x) = \frac{2}{\sqrt{\pi}} \int_0^x \exp( - t^2) dt \f]
+ * using a continued fraction representation:
+ * \f[ \text{erf}(x) = \frac{2x}{\sqrt{\pi}} \exp(-x^2) \dfrac{1}{1 - 2x^2 + \dfrac{4x^2}{3 - 2x^2 + \dfrac{8x^2}{5 - 2x^2 + \dfrac{12x^2}{7 - 2x^2 + \ddots}}}} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+erf(const T x)
+noexcept
+{
+    return internal::erf_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..a93f8db
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/erf_inv.hpp
@@ -0,0 +1,264 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/* 
+ * compile-time inverse error function
+ *
+ * Initial approximation based on:
+ * 'Approximating the erfinv function' by Mike Giles
+ */
+
+#ifndef _gcem_erf_inv_HPP
+#define _gcem_erf_inv_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr T erf_inv_decision(const T value, const T p, const T direc, const int iter_count) noexcept;
+
+//
+// initial value
+
+// two cases: (1) a < 5; and (2) otherwise
+
+template<typename T>
+constexpr
+T
+erf_inv_initial_val_coef_2(const T a, const T p_term, const int order)
+noexcept
+{
+    return( order == 1 ? T(-0.000200214257L) :
+            order == 2 ? T( 0.000100950558L) + a*p_term :
+            order == 3 ? T( 0.00134934322L)  + a*p_term :
+            order == 4 ? T(-0.003673428440L) + a*p_term :
+            order == 5 ? T( 0.005739507730L) + a*p_term :
+            order == 6 ? T(-0.00762246130L)  + a*p_term :
+            order == 7 ? T( 0.009438870470L) + a*p_term :
+            order == 8 ? T( 1.001674060000L) + a*p_term :
+            order == 9 ? T( 2.83297682000L)  + a*p_term :
+                         p_term );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_initial_val_case_2(const T a, const T p_term, const int order)
+noexcept
+{
+    return( order == 9 ? \
+            // if
+                erf_inv_initial_val_coef_2(a,p_term,order) :
+            // else
+                erf_inv_initial_val_case_2(a,erf_inv_initial_val_coef_2(a,p_term,order),order+1) );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_initial_val_coef_1(const T a, const T p_term, const int order)
+noexcept
+{
+    return( order == 1 ? T( 2.81022636e-08L) : 
+            order == 2 ? T( 3.43273939e-07L) + a*p_term :
+            order == 3 ? T(-3.5233877e-06L)  + a*p_term :
+            order == 4 ? T(-4.39150654e-06L) + a*p_term :
+            order == 5 ? T( 0.00021858087L)  + a*p_term :
+            order == 6 ? T(-0.00125372503L)  + a*p_term :
+            order == 7 ? T(-0.004177681640L) + a*p_term :
+            order == 8 ? T( 0.24664072700L)  + a*p_term :
+            order == 9 ? T( 1.50140941000L)  + a*p_term :
+                         p_term );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_initial_val_case_1(const T a, const T p_term, const int order)
+noexcept
+{
+    return( order == 9 ? \
+            // if
+                erf_inv_initial_val_coef_1(a,p_term,order) :
+            // else
+                erf_inv_initial_val_case_1(a,erf_inv_initial_val_coef_1(a,p_term,order),order+1) );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_initial_val_int(const T a)
+noexcept
+{
+    return( a < T(5) ? \
+            // if
+                erf_inv_initial_val_case_1(a-T(2.5),T(0),1) :
+            // else
+                erf_inv_initial_val_case_2(sqrt(a)-T(3),T(0),1) );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_initial_val(const T x)
+noexcept
+{
+    return x*erf_inv_initial_val_int( -log( (T(1) - x)*(T(1) + x) ) );
+}
+
+//
+// Halley recursion
+
+template<typename T>
+constexpr
+T
+erf_inv_err_val(const T value, const T p)
+noexcept
+{   // err_val = f(x)
+    return( erf(value) - p );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_deriv_1(const T value)
+noexcept
+{   // derivative of the error function w.r.t. x
+    return( exp( -value*value ) );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_deriv_2(const T value, const T deriv_1)
+noexcept
+{   // second derivative of the error function w.r.t. x
+    return( deriv_1*( -T(2)*value ) );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_ratio_val_1(const T value, const T p, const T deriv_1)
+noexcept
+{
+    return( erf_inv_err_val(value,p) / deriv_1 );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_ratio_val_2(const T value, const T deriv_1)
+noexcept
+{
+    return( erf_inv_deriv_2(value,deriv_1) / deriv_1 );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_halley(const T ratio_val_1, const T ratio_val_2)
+noexcept
+{
+    return( ratio_val_1 / max( T(0.8), min( T(1.2), T(1) - T(0.5)*ratio_val_1*ratio_val_2 ) ) );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_recur(const T value, const T p, const T deriv_1, const int iter_count)
+noexcept
+{
+    return erf_inv_decision( value, p, 
+                             erf_inv_halley(erf_inv_ratio_val_1(value,p,deriv_1), 
+                                            erf_inv_ratio_val_2(value,deriv_1)),
+                             iter_count );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_decision(const T value, const T p, const T direc, const int iter_count)
+noexcept
+{
+    return( iter_count < GCEM_ERF_INV_MAX_ITER ? \
+            // if
+                erf_inv_recur(value-direc,p, erf_inv_deriv_1(value), iter_count+1) :
+            // else
+                value - direc );
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_recur_begin(const T initial_val, const T p)
+noexcept
+{
+    return erf_inv_recur(initial_val,p,erf_inv_deriv_1(initial_val),1);
+}
+
+template<typename T>
+constexpr
+T
+erf_inv_begin(const T p)
+noexcept
+{
+    return( // NaN check
+            is_nan(p) ? \
+                GCLIM<T>::quiet_NaN() :
+            // bad values
+            abs(p) > T(1) ? \
+                GCLIM<T>::quiet_NaN() :
+            // indistinguishable from 1
+            GCLIM<T>::min() > abs(T(1) - p) ? \
+                GCLIM<T>::infinity() :
+            // indistinguishable from - 1
+            GCLIM<T>::min() > abs(T(1) + p) ? \
+                - GCLIM<T>::infinity() :
+            // else
+            erf_inv_recur_begin(erf_inv_initial_val(p),p) );
+}
+
+}
+
+/**
+ * Compile-time inverse Gaussian error function
+ *
+ * @param p a real-valued input with values in the unit-interval.
+ * @return Computes the inverse Gaussian error function, a value \f$ x \f$ such that 
+ * \f[ f(x) := \text{erf}(x) - p \f]
+ * is equal to zero, for a given \c p. 
+ * GCE-Math finds this root using Halley's method:
+ * \f[ x_{n+1} = x_n - \frac{f(x_n)/f'(x_n)}{1 - 0.5 \frac{f(x_n)}{f'(x_n)} \frac{f''(x_n)}{f'(x_n)} } \f]
+ * where
+ * \f[ \frac{\partial}{\partial x} \text{erf}(x) = \exp(-x^2), \ \ \frac{\partial^2}{\partial x^2} \text{erf}(x) = -2x\exp(-x^2) \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+erf_inv(const T p)
+noexcept
+{
+    return internal::erf_inv_begin( static_cast<return_t<T>>(p) );
+}
+
+
+#endif
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
new file mode 100644
index 0000000..0c66829
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/exp.hpp
@@ -0,0 +1,106 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time exponential function
+ */
+
+#ifndef _gcem_exp_HPP
+#define _gcem_exp_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+exp_cf_recur(const T x, const int depth)
+noexcept
+{
+    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) : 
+             // else
+                T(1) );
+}
+
+template<typename T>
+constexpr
+T
+exp_cf(const T x)
+noexcept
+{
+    return( T(1)/exp_cf_recur(x,1) );
+}
+
+template<typename T>
+constexpr
+T
+exp_split(const T x)
+noexcept
+{
+    return( static_cast<T>(pow_integral(GCEM_E,find_whole(x))) * exp_cf(find_fraction(x)) );
+}
+
+template<typename T>
+constexpr
+T
+exp_check(const T x)
+noexcept
+{
+    return( is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            //
+            is_neginf(x) ? \
+                T(0) :
+            //
+            GCLIM<T>::min() > abs(x) ? \
+                T(1) : 
+            //
+            is_posinf(x) ? \
+                GCLIM<T>::infinity() :
+            //
+            abs(x) < T(2) ? \
+                exp_cf(x) : \
+                exp_split(x) );
+}
+
+}
+
+/**
+ * Compile-time exponential function
+ *
+ * @param x a real-valued input.
+ * @return \f$ \exp(x) \f$ using \f[ \exp(x) = \dfrac{1}{1-\dfrac{x}{1+x-\dfrac{\frac{1}{2}x}{1 + \frac{1}{2}x - \dfrac{\frac{1}{3}x}{1 + \frac{1}{3}x - \ddots}}}} \f] 
+ * The continued fraction argument is split into two parts: \f$ x = n + r \f$, where \f$ n \f$ is an integer and \f$ r \in [-0.5,0.5] \f$.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+exp(const T x)
+noexcept
+{
+    return internal::exp_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..11b2eb9
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/expm1.hpp
@@ -0,0 +1,76 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time exponential function
+ */
+
+#ifndef _gcem_expm1_HPP
+#define _gcem_expm1_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+expm1_compute(const T x)
+noexcept
+{
+    // return x * ( T(1) + x * ( T(1)/T(2) + x * ( T(1)/T(6) + x * ( T(1)/T(24) +  x/T(120) ) ) ) ); // O(x^6)
+    return x + x * ( x/T(2) + x * ( x/T(6) + x * ( x/T(24) +  x*x/T(120) ) ) ); // O(x^6)
+}
+
+template<typename T>
+constexpr
+T
+expm1_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            //
+            abs(x) > T(1e-04) ? \
+            // if
+                exp(x) - T(1) :
+            // else    
+                expm1_compute(x) );
+}
+
+}
+
+/**
+ * Compile-time exponential-minus-1 function
+ *
+ * @param x a real-valued input.
+ * @return \f$ \exp(x) - 1 \f$ using \f[ \exp(x) = \sum_{k=0}^\infty \dfrac{x^k}{k!} \f] 
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+expm1(const T x)
+noexcept
+{
+    return internal::expm1_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..539c3f3
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/factorial.hpp
@@ -0,0 +1,98 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time factorial function
+ */
+
+#ifndef _gcem_factorial_HPP
+#define _gcem_factorial_HPP
+
+namespace internal
+{
+
+// T should be int, long int, unsigned int, etc.
+
+template<typename T>
+constexpr
+T
+factorial_table(const T x)
+noexcept
+{   // table for x! when x = {2,...,16}
+    return( x == T(2)  ? T(2)     : x == T(3)  ? T(6)      :
+            x == T(4)  ? T(24)    : x == T(5)  ? T(120)    :
+            x == T(6)  ? T(720)   : x == T(7)  ? T(5040)   :
+            x == T(8)  ? T(40320) : x == T(9)  ? T(362880) :
+            //
+            x == T(10) ? T(3628800)       : 
+            x == T(11) ? T(39916800)      :
+            x == T(12) ? T(479001600)     :
+            x == T(13) ? T(6227020800)    : 
+            x == T(14) ? T(87178291200)   : 
+            x == T(15) ? T(1307674368000) : 
+                         T(20922789888000) );
+}
+
+template<typename T, typename std::enable_if<std::is_integral<T>::value>::type* = nullptr>
+constexpr
+T
+factorial_recur(const T x)
+noexcept
+{
+    return( x == T(0) ? T(1) :
+            x == T(1) ? x :
+            //
+            x < T(17) ? \
+                // if
+                factorial_table(x) :
+                // else
+                x*factorial_recur(x-1) );
+}
+
+template<typename T, typename std::enable_if<!std::is_integral<T>::value>::type* = nullptr>
+constexpr
+T
+factorial_recur(const T x)
+noexcept
+{
+    return tgamma(x + 1);
+}
+
+}
+
+/**
+ * Compile-time factorial function
+ *
+ * @param x a real-valued input.
+ * @return Computes the factorial value \f$ x! \f$. 
+ * When \c x is an integral type (\c int, <tt>long int</tt>, etc.), a simple recursion method is used, along with table values.
+ * When \c x is real-valued, <tt>factorial(x) = tgamma(x+1)</tt>.
+ */
+
+template<typename T>
+constexpr
+T
+factorial(const T x)
+noexcept
+{
+    return internal::factorial_recur(x);
+}
+
+#endif
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
new file mode 100644
index 0000000..710adce
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_exponent.hpp
@@ -0,0 +1,47 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time find_exponent function
+ */
+
+#ifndef _gcem_find_exponent_HPP
+#define _gcem_find_exponent_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+llint_t
+find_exponent(const T x, const llint_t exponent)
+noexcept
+{
+    return( x < T(1)  ? \
+                find_exponent(x*T(10),exponent - llint_t(1)) :
+            x > T(10) ? \
+                find_exponent(x/T(10),exponent + llint_t(1)) :
+            // else
+                exponent );
+}
+
+}
+
+#endif
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
new file mode 100644
index 0000000..d9769e6
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_fraction.hpp
@@ -0,0 +1,46 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * find the fraction part of x = n + r, where -0.5 <= r <= 0.5
+ */
+
+#ifndef _gcem_find_fraction_HPP
+#define _gcem_find_fraction_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+find_fraction(const T x)
+noexcept
+{
+    return( abs(x - internal::floor_check(x)) >= T(0.5) ? \
+            // if 
+                x - internal::floor_check(x) - sgn(x) : 
+            //else 
+                x - internal::floor_check(x) );
+}
+
+}
+
+#endif
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
new file mode 100644
index 0000000..bd5e0b9
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/find_whole.hpp
@@ -0,0 +1,46 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * find the whole number part of x = n + r, where -0.5 <= r <= 0.5
+ */
+
+#ifndef _gcem_find_whole_HPP
+#define _gcem_find_whole_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+llint_t
+find_whole(const T x)
+noexcept
+{
+    return( abs(x - internal::floor_check(x)) >= T(0.5) ? \
+            // if 
+                static_cast<llint_t>(internal::floor_check(x) + sgn(x)) :
+            // else 
+                static_cast<llint_t>(internal::floor_check(x)) );
+}
+
+}
+
+#endif
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
new file mode 100644
index 0000000..c60ff6a
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/floor.hpp
@@ -0,0 +1,130 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_floor_HPP
+#define _gcem_floor_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+int
+floor_resid(const T x, const T x_whole)
+noexcept
+{
+    return( (x < T(0)) && (x < x_whole) );
+}
+
+template<typename T>
+constexpr
+T
+floor_int(const T x, const T x_whole)
+noexcept
+{
+    return( x_whole - static_cast<T>(floor_resid(x,x_whole)) );
+}
+
+template<typename T>
+constexpr
+T
+floor_check_internal(const T x)
+noexcept
+{
+    return x;
+}
+
+template<>
+constexpr
+float
+floor_check_internal<float>(const float x)
+noexcept
+{
+    return( abs(x) >= 8388608.f ? \
+            // if
+                x : \
+            // else
+                floor_int(x, float(static_cast<int>(x))) );
+}
+
+template<>
+constexpr
+double
+floor_check_internal<double>(const double x)
+noexcept
+{
+    return( abs(x) >= 4503599627370496. ? \
+            // if
+                x : \
+            // else
+                floor_int(x, double(static_cast<llint_t>(x))) );
+}
+
+template<>
+constexpr
+long double
+floor_check_internal<long double>(const long double x)
+noexcept
+{
+    return( abs(x) >= 9223372036854775808.l ? \
+            // if
+                x : \
+            // else
+                floor_int(x, ((long double)static_cast<ullint_t>(abs(x))) * sgn(x)) );
+}
+
+template<typename T>
+constexpr
+T
+floor_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // +/- infinite
+            !is_finite(x) ? \
+                x :
+            // signed-zero cases
+            GCLIM<T>::min() > abs(x) ? \
+                x :
+            // else
+                floor_check_internal(x) );
+}
+
+}
+
+/**
+ * Compile-time floor function
+ *
+ * @param x a real-valued input.
+ * @return computes the floor-value of the input.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+floor(const T x)
+noexcept
+{
+    return internal::floor_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..c804ce6
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/fmod.hpp
@@ -0,0 +1,70 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_fmod_HPP
+#define _gcem_fmod_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+fmod_check(const T x, const T y)
+noexcept
+{
+    return( // NaN check
+            any_nan(x, y) ? \
+                GCLIM<T>::quiet_NaN() :
+            // +/- infinite
+            !all_finite(x, y) ? \
+                GCLIM<T>::quiet_NaN() :
+            // else
+                x - trunc(x/y)*y );
+}
+
+template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
+constexpr
+TC
+fmod_type_check(const T1 x, const T2 y)
+noexcept
+{
+    return fmod_check(static_cast<TC>(x),static_cast<TC>(y));
+}
+
+}
+
+/**
+ * Compile-time remainder of division function
+ * @param x a real-valued input.
+ * @param y a real-valued input.
+ * @return computes the floating-point remainder of \f$ x / y \f$ (rounded towards zero) using \f[ \text{fmod}(x,y) = x - \text{trunc}(x/y) \times y \f] 
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_return_t<T1,T2>
+fmod(const T1 x, const T2 y)
+noexcept
+{
+    return internal::fmod_type_check(x,y);
+}
+
+#endif
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
new file mode 100644
index 0000000..4a10bbe
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcd.hpp
@@ -0,0 +1,82 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_gcd_HPP
+#define _gcem_gcd_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+gcd_recur(const T a, const T b)
+noexcept
+{
+    return( b == T(0) ? a : gcd_recur(b, a % b) );
+}
+
+template<typename T, typename std::enable_if<std::is_integral<T>::value>::type* = nullptr>
+constexpr
+T
+gcd_int_check(const T a, const T b)
+noexcept
+{
+    return gcd_recur(a,b);
+}
+
+template<typename T, typename std::enable_if<!std::is_integral<T>::value>::type* = nullptr>
+constexpr
+T
+gcd_int_check(const T a, const T b)
+noexcept
+{
+    return gcd_recur( static_cast<ullint_t>(a), static_cast<ullint_t>(b) );
+}
+
+template<typename T1, typename T2, typename TC = common_t<T1,T2>>
+constexpr
+TC
+gcd_type_check(const T1 a, const T2 b)
+noexcept
+{
+    return gcd_int_check( static_cast<TC>(abs(a)), static_cast<TC>(abs(b)) );
+}
+
+}
+
+/**
+ * Compile-time greatest common divisor (GCD) function
+ *
+ * @param a integral-valued input.
+ * @param b integral-valued input.
+ * @return the greatest common divisor between integers \c a and \c b using a Euclidean algorithm.
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_t<T1,T2>
+gcd(const T1 a, const T2 b)
+noexcept
+{
+    return internal::gcd_type_check(a,b);
+}
+
+#endif
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
new file mode 100644
index 0000000..cd2747c
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/gcem_options.hpp
@@ -0,0 +1,213 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#include <cstddef>      // size_t
+#include <limits>
+#include <type_traits>
+
+// undef some functions from math.h
+// see issue #29
+
+#ifdef abs
+    #undef abs
+#endif
+
+#ifdef min
+    #undef min
+#endif
+
+#ifdef max
+    #undef max
+#endif
+
+#ifdef round
+    #undef round
+#endif
+
+#ifdef signbit
+    #undef signbit
+#endif
+
+//
+// version
+
+#ifndef GCEM_VERSION_MAJOR
+    #define GCEM_VERSION_MAJOR 1
+#endif
+
+#ifndef GCEM_VERSION_MINOR
+    #define GCEM_VERSION_MINOR 16
+#endif
+
+#ifndef GCEM_VERSION_PATCH
+    #define GCEM_VERSION_PATCH 0
+#endif
+
+//
+// types
+
+namespace gcem
+{
+    using uint_t = unsigned int;
+    using ullint_t = unsigned long long int;
+
+    using llint_t = long long int;
+
+    template<class T>
+    using GCLIM = std::numeric_limits<T>;
+
+    template<typename T>
+    using return_t = typename std::conditional<std::is_integral<T>::value,double,T>::type;
+
+    template<typename ...T>
+    using common_t = typename std::common_type<T...>::type;
+
+    template<typename ...T>
+    using common_return_t = return_t<common_t<T...>>;
+}
+
+//
+// constants
+
+#ifndef GCEM_LOG_2
+    #define GCEM_LOG_2 0.6931471805599453094172321214581765680755L
+#endif
+
+#ifndef GCEM_LOG_10
+    #define GCEM_LOG_10 2.3025850929940456840179914546843642076011L
+#endif
+
+#ifndef GCEM_PI
+    #define GCEM_PI 3.1415926535897932384626433832795028841972L
+#endif
+
+#ifndef GCEM_LOG_PI
+    #define GCEM_LOG_PI 1.1447298858494001741434273513530587116473L
+#endif
+
+#ifndef GCEM_LOG_2PI
+    #define GCEM_LOG_2PI 1.8378770664093454835606594728112352797228L
+#endif
+
+#ifndef GCEM_LOG_SQRT_2PI
+    #define GCEM_LOG_SQRT_2PI 0.9189385332046727417803297364056176398614L
+#endif
+
+#ifndef GCEM_SQRT_2
+    #define GCEM_SQRT_2 1.4142135623730950488016887242096980785697L
+#endif
+
+#ifndef GCEM_HALF_PI
+    #define GCEM_HALF_PI 1.5707963267948966192313216916397514420986L
+#endif
+
+#ifndef GCEM_SQRT_PI
+    #define GCEM_SQRT_PI 1.7724538509055160272981674833411451827975L
+#endif
+
+#ifndef GCEM_SQRT_HALF_PI
+    #define GCEM_SQRT_HALF_PI 1.2533141373155002512078826424055226265035L
+#endif
+
+#ifndef GCEM_E
+    #define GCEM_E 2.7182818284590452353602874713526624977572L
+#endif
+
+//
+// convergence settings
+
+#ifndef GCEM_ERF_MAX_ITER
+    #define GCEM_ERF_MAX_ITER 60
+#endif
+
+#ifndef GCEM_ERF_INV_MAX_ITER
+    #define GCEM_ERF_INV_MAX_ITER 60
+#endif
+
+#ifndef GCEM_EXP_MAX_ITER_SMALL
+    #define GCEM_EXP_MAX_ITER_SMALL 25
+#endif
+
+// #ifndef GCEM_LOG_TOL
+//     #define GCEM_LOG_TOL 1E-14
+// #endif
+
+#ifndef GCEM_LOG_MAX_ITER_SMALL
+    #define GCEM_LOG_MAX_ITER_SMALL 25
+#endif
+
+#ifndef GCEM_LOG_MAX_ITER_BIG
+    #define GCEM_LOG_MAX_ITER_BIG 255
+#endif
+
+#ifndef GCEM_INCML_BETA_TOL
+    #define GCEM_INCML_BETA_TOL 1E-15
+#endif
+
+#ifndef GCEM_INCML_BETA_MAX_ITER
+    #define GCEM_INCML_BETA_MAX_ITER 205
+#endif
+
+#ifndef GCEM_INCML_BETA_INV_MAX_ITER
+    #define GCEM_INCML_BETA_INV_MAX_ITER 35
+#endif
+
+#ifndef GCEM_INCML_GAMMA_MAX_ITER
+    #define GCEM_INCML_GAMMA_MAX_ITER 55
+#endif
+
+#ifndef GCEM_INCML_GAMMA_INV_MAX_ITER
+    #define GCEM_INCML_GAMMA_INV_MAX_ITER 35
+#endif
+
+#ifndef GCEM_SQRT_MAX_ITER
+    #define GCEM_SQRT_MAX_ITER 100
+#endif
+
+#ifndef GCEM_INV_SQRT_MAX_ITER
+    #define GCEM_INV_SQRT_MAX_ITER 100
+#endif
+
+#ifndef GCEM_TAN_MAX_ITER
+    #define GCEM_TAN_MAX_ITER 35
+#endif
+
+#ifndef GCEM_TANH_MAX_ITER
+    #define GCEM_TANH_MAX_ITER 35
+#endif
+
+//
+// Macros
+
+#ifdef _MSC_VER
+    #ifndef GCEM_SIGNBIT
+        #define GCEM_SIGNBIT(x) _signbit(x)
+    #endif
+    #ifndef GCEM_COPYSIGN
+        #define GCEM_COPYSIGN(x,y) _copysign(x,y)
+    #endif
+#else
+    #ifndef GCEM_SIGNBIT
+        #define GCEM_SIGNBIT(x) __builtin_signbit(x)
+    #endif
+    #ifndef GCEM_COPYSIGN
+        #define GCEM_COPYSIGN(x,y) __builtin_copysign(x,y)
+    #endif  
+#endif
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
new file mode 100644
index 0000000..5a805ed
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/hypot.hpp
@@ -0,0 +1,90 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time Pythagorean addition function
+ */
+
+// see: https://en.wikipedia.org/wiki/Pythagorean_addition
+
+#ifndef _gcem_hypot_HPP
+#define _gcem_hypot_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+hypot_compute(const T x, const T ydx)
+noexcept
+{
+    return abs(x) * sqrt( T(1) + (ydx * ydx) );
+}
+
+template<typename T>
+constexpr
+T
+hypot_vals_check(const T x, const T y)
+noexcept
+{
+    return( any_nan(x, y) ? \
+                GCLIM<T>::quiet_NaN() :
+            //
+            any_inf(x,y) ? \
+                GCLIM<T>::infinity() :
+            // indistinguishable from zero or one
+            GCLIM<T>::min() > abs(x) ? \
+                abs(y) :
+            GCLIM<T>::min() > abs(y) ? \
+                abs(x) :
+            // else
+            hypot_compute(x, y/x) );
+}
+
+template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
+constexpr
+TC
+hypot_type_check(const T1 x, const T2 y)
+noexcept
+{
+    return hypot_vals_check(static_cast<TC>(x),static_cast<TC>(y));
+}
+
+}
+
+/**
+ * Compile-time Pythagorean addition function
+ *
+ * @param x a real-valued input.
+ * @param y a real-valued input.
+ * @return Computes \f$ x \oplus y = \sqrt{x^2 + y^2} \f$.
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_return_t<T1,T2>
+hypot(const T1 x, const T2 y)
+noexcept
+{
+    return internal::hypot_type_check(x,y);
+}
+
+#endif
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
new file mode 100644
index 0000000..5645dbe
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta.hpp
@@ -0,0 +1,194 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time incomplete beta function
+ *
+ * see eq. 18.5.17a in the Handbook of Continued Fractions for Special Functions
+ */
+
+#ifndef _gcem_incomplete_beta_HPP
+#define _gcem_incomplete_beta_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr T incomplete_beta_cf(const T a, const T b, const T z, const T c_j, const T d_j, const T f_j, const int depth) noexcept;
+
+//
+// coefficients; see eq. 18.5.17b
+
+template<typename T>
+constexpr
+T
+incomplete_beta_coef_even(const T a, const T b, const T z, const int k)
+noexcept
+{
+    return( -z*(a + k)*(a + b + k)/( (a + 2*k)*(a + 2*k + T(1)) ) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_coef_odd(const T a, const T b, const T z, const int k)
+noexcept
+{
+    return( z*k*(b - k)/((a + 2*k - T(1))*(a + 2*k)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_coef(const T a, const T b, const T z, const int depth)
+noexcept
+{
+    return( !is_odd(depth) ? incomplete_beta_coef_even(a,b,z,depth/2) :
+                             incomplete_beta_coef_odd(a,b,z,(depth+1)/2) );
+}
+
+//
+// update formulae for the modified Lentz method
+
+template<typename T>
+constexpr
+T
+incomplete_beta_c_update(const T a, const T b, const T z, const T c_j, const int depth)
+noexcept
+{
+    return( T(1) + incomplete_beta_coef(a,b,z,depth)/c_j );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_d_update(const T a, const T b, const T z, const T d_j, const int depth)
+noexcept
+{
+    return( T(1) / (T(1) + incomplete_beta_coef(a,b,z,depth)*d_j) );
+}
+
+//
+// convergence-type condition
+
+template<typename T>
+constexpr
+T
+incomplete_beta_decision(const T a, const T b, const T z, const T c_j, const T d_j, const T f_j, const int depth)
+noexcept
+{
+    return( // tolerance check
+                abs(c_j*d_j - T(1)) < GCEM_INCML_BETA_TOL ? f_j*c_j*d_j :
+            // max_iter check
+                depth < GCEM_INCML_BETA_MAX_ITER ? \
+                    // if
+                        incomplete_beta_cf(a,b,z,c_j,d_j,f_j*c_j*d_j,depth+1) :
+                    // else 
+                        f_j*c_j*d_j );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_cf(const T a, const T b, const T z, const T c_j, const T d_j, const T f_j, const int depth)
+noexcept
+{
+    return  incomplete_beta_decision(a,b,z,
+                incomplete_beta_c_update(a,b,z,c_j,depth),
+                incomplete_beta_d_update(a,b,z,d_j,depth),
+                f_j,depth);
+}
+
+//
+// x^a (1-x)^{b} / (a beta(a,b)) * cf
+
+template<typename T>
+constexpr
+T
+incomplete_beta_begin(const T a, const T b, const T z)
+noexcept
+{
+    return  ( (exp(a*log(z) + b*log(T(1)-z) - lbeta(a,b)) / a) * \
+                incomplete_beta_cf(a,b,z,T(1), 
+                    incomplete_beta_d_update(a,b,z,T(1),0),
+                    incomplete_beta_d_update(a,b,z,T(1),0),1)
+            );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_check(const T a, const T b, const T z)
+noexcept
+{
+    return( // NaN check
+            any_nan(a, b, z) ? \
+                GCLIM<T>::quiet_NaN() :
+            // indistinguishable from zero
+            GCLIM<T>::min() > z ? \
+                T(0) :
+            // parameter check for performance
+            (a + T(1))/(a + b + T(2)) > z ? \
+                incomplete_beta_begin(a,b,z) :
+                T(1) - incomplete_beta_begin(b,a,T(1) - z) );
+}
+
+template<typename T1, typename T2, typename T3, typename TC = common_return_t<T1,T2,T3>>
+constexpr
+TC
+incomplete_beta_type_check(const T1 a, const T2 b, const T3 p)
+noexcept
+{
+    return incomplete_beta_check(static_cast<TC>(a),
+                                 static_cast<TC>(b),
+                                 static_cast<TC>(p));
+}
+
+}
+
+/**
+ * Compile-time regularized incomplete beta function
+ *
+ * @param a a real-valued, non-negative input.
+ * @param b a real-valued, non-negative input.
+ * @param z a real-valued, non-negative input.
+ *
+ * @return computes the regularized incomplete beta function,
+ * \f[ \frac{\text{B}(z;\alpha,\beta)}{\text{B}(\alpha,\beta)} = \frac{1}{\text{B}(\alpha,\beta)}\int_0^z t^{a-1} (1-t)^{\beta-1} dt \f]
+ * using a continued fraction representation, found in the Handbook of Continued Fractions for Special Functions, and a modified Lentz method.
+ * \f[ \frac{\text{B}(z;\alpha,\beta)}{\text{B}(\alpha,\beta)} = \frac{z^{\alpha} (1-t)^{\beta}}{\alpha \text{B}(\alpha,\beta)} \dfrac{a_1}{1 + \dfrac{a_2}{1 + \dfrac{a_3}{1 + \dfrac{a_4}{1 + \ddots}}}} \f]
+ * where \f$ a_1 = 1 \f$ and
+ * \f[ a_{2m+2} = - \frac{(\alpha + m)(\alpha + \beta + m)}{(\alpha + 2m)(\alpha + 2m + 1)}, \ m \geq 0 \f]
+ * \f[ a_{2m+1} = \frac{m(\beta - m)}{(\alpha + 2m - 1)(\alpha + 2m)}, \ m \geq 1 \f]
+ * The Lentz method works as follows: let \f$ f_j \f$ denote the value of the continued fraction up to the first \f$ j \f$ terms; \f$ f_j \f$ is updated as follows:
+ * \f[ c_j = 1 + a_j / c_{j-1}, \ \ d_j = 1 / (1 + a_j d_{j-1}) \f]
+ * \f[ f_j = c_j d_j f_{j-1} \f]
+ */
+
+template<typename T1, typename T2, typename T3>
+constexpr
+common_return_t<T1,T2,T3>
+incomplete_beta(const T1 a, const T2 b, const T3 z)
+noexcept
+{
+    return internal::incomplete_beta_type_check(a,b,z);
+}
+
+#endif
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
new file mode 100644
index 0000000..f7fdfa0
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_beta_inv.hpp
@@ -0,0 +1,352 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * inverse of the incomplete beta function
+ */
+
+#ifndef _gcem_incomplete_beta_inv_HPP
+#define _gcem_incomplete_beta_inv_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr T incomplete_beta_inv_decision(const T value, const T alpha_par, const T beta_par, const T p,
+                                         const T direc, const T lb_val, const int iter_count) noexcept;
+
+//
+// initial value for Halley
+
+//
+// a,b > 1 case
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_1_tval(const T p)
+noexcept
+{   // a > 1.0
+    return( p > T(0.5) ? \
+            // if
+                sqrt(-T(2)*log(T(1) - p)) :
+            // else
+                sqrt(-T(2)*log(p)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_1_int_begin(const T t_val)
+noexcept
+{   // internal for a > 1.0
+    return( t_val - ( T(2.515517) + T(0.802853)*t_val + T(0.010328)*t_val*t_val ) \
+                / ( T(1) + T(1.432788)*t_val + T(0.189269)*t_val*t_val + T(0.001308)*t_val*t_val*t_val ) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_1_int_ab1(const T alpha_par, const T beta_par)
+noexcept
+{
+    return( T(1)/(2*alpha_par - T(1)) + T(1)/(2*beta_par - T(1)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_1_int_ab2(const T alpha_par, const T beta_par)
+noexcept
+{
+    return( T(1)/(2*beta_par - T(1)) - T(1)/(2*alpha_par - T(1)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_1_int_h(const T ab_term_1)
+noexcept
+{
+    return( T(2) / ab_term_1 );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_1_int_w(const T value, const T ab_term_2, const T h_term)
+noexcept
+{
+    // return( value * sqrt(h_term + lambda)/h_term - ab_term_2*(lambda + 5.0/6.0 -2.0/(3.0*h_term)) );
+    return( value * sqrt(h_term + (value*value - T(3))/T(6))/h_term \
+                - ab_term_2*((value*value - T(3))/T(6) + T(5)/T(6) - T(2)/(T(3)*h_term)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_1_int_end(const T alpha_par, const T beta_par, const T w_term)
+noexcept
+{
+    return( alpha_par / (alpha_par + beta_par*exp(2*w_term)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_1(const T alpha_par, const T beta_par, const T t_val, const T sgn_term)
+noexcept
+{   // a > 1.0
+    return  incomplete_beta_inv_initial_val_1_int_end( alpha_par, beta_par,
+                incomplete_beta_inv_initial_val_1_int_w(
+                    sgn_term*incomplete_beta_inv_initial_val_1_int_begin(t_val),
+                    incomplete_beta_inv_initial_val_1_int_ab2(alpha_par,beta_par),
+                    incomplete_beta_inv_initial_val_1_int_h(
+                        incomplete_beta_inv_initial_val_1_int_ab1(alpha_par,beta_par)
+                    )
+                )
+            );
+}
+
+//
+// a,b else
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_2_s1(const T alpha_par, const T beta_par)
+noexcept
+{
+    return( pow(alpha_par/(alpha_par+beta_par),alpha_par) / alpha_par );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_2_s2(const T alpha_par, const T beta_par)
+noexcept
+{
+    return( pow(beta_par/(alpha_par+beta_par),beta_par) / beta_par );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val_2(const T alpha_par, const T beta_par, const T p, const T s_1, const T s_2)
+noexcept
+{
+    return( p <= s_1/(s_1 + s_2) ? pow(p*(s_1+s_2)*alpha_par,T(1)/alpha_par) :
+                                    T(1) - pow(p*(s_1+s_2)*beta_par,T(1)/beta_par) );
+}
+
+// initial value
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_initial_val(const T alpha_par, const T beta_par, const T p)
+noexcept
+{
+    return( (alpha_par > T(1) && beta_par > T(1)) ?
+            // if
+                incomplete_beta_inv_initial_val_1(alpha_par,beta_par,
+                    incomplete_beta_inv_initial_val_1_tval(p),
+                    p < T(0.5) ? T(1) : T(-1) ) :
+            // else
+                p > T(0.5) ?
+                    // if
+                       T(1) - incomplete_beta_inv_initial_val_2(beta_par,alpha_par,T(1) - p,
+                                    incomplete_beta_inv_initial_val_2_s1(beta_par,alpha_par),
+                                    incomplete_beta_inv_initial_val_2_s2(beta_par,alpha_par)) :
+                    // else
+                       incomplete_beta_inv_initial_val_2(alpha_par,beta_par,p,
+                            incomplete_beta_inv_initial_val_2_s1(alpha_par,beta_par),
+                            incomplete_beta_inv_initial_val_2_s2(alpha_par,beta_par))
+            );
+}
+
+//
+// Halley recursion
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_err_val(const T value, const T alpha_par, const T beta_par, const T p)
+noexcept
+{   // err_val = f(x)
+    return( incomplete_beta(alpha_par,beta_par,value) - p );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_deriv_1(const T value, const T alpha_par, const T beta_par, const T lb_val)
+noexcept
+{   // derivative of the incomplete beta function w.r.t. x
+    return( // indistinguishable from zero or one
+            GCLIM<T>::min() > abs(value) ? \
+                T(0) :
+            GCLIM<T>::min() > abs(T(1) - value) ? \
+                T(0) :
+            // else
+                exp( (alpha_par - T(1))*log(value) + (beta_par - T(1))*log(T(1) - value) - lb_val ) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_deriv_2(const T value, const T alpha_par, const T beta_par, const T deriv_1)
+noexcept
+{ // second derivative of the incomplete beta function w.r.t. x
+    return( deriv_1*((alpha_par - T(1))/value - (beta_par - T(1))/(T(1) - value)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_ratio_val_1(const T value, const T alpha_par, const T beta_par, const T p, const T deriv_1)
+noexcept
+{
+    return( incomplete_beta_inv_err_val(value,alpha_par,beta_par,p) / deriv_1 );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_ratio_val_2(const T value, const T alpha_par, const T beta_par, const T deriv_1)
+noexcept
+{
+    return( incomplete_beta_inv_deriv_2(value,alpha_par,beta_par,deriv_1) / deriv_1 );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_halley(const T ratio_val_1, const T ratio_val_2)
+noexcept
+{
+    return( ratio_val_1 / max( T(0.8), min( T(1.2), T(1) - T(0.5)*ratio_val_1*ratio_val_2 ) ) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_recur(const T value, const T alpha_par, const T beta_par, const T p, const T deriv_1,
+                          const T lb_val, const int iter_count)
+noexcept
+{
+    return( // derivative = 0
+            GCLIM<T>::min() > abs(deriv_1) ? \
+                incomplete_beta_inv_decision( value, alpha_par, beta_par, p, T(0), lb_val,
+                    GCEM_INCML_BETA_INV_MAX_ITER+1) :
+            // else
+            incomplete_beta_inv_decision( value, alpha_par, beta_par, p,
+               incomplete_beta_inv_halley(
+                   incomplete_beta_inv_ratio_val_1(value,alpha_par,beta_par,p,deriv_1),
+                   incomplete_beta_inv_ratio_val_2(value,alpha_par,beta_par,deriv_1)
+               ), lb_val, iter_count) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_decision(const T value, const T alpha_par, const T beta_par, const T p, const T direc,
+                             const T lb_val, const int iter_count)
+noexcept
+{
+    return( iter_count <= GCEM_INCML_BETA_INV_MAX_ITER ?
+            // if
+                incomplete_beta_inv_recur(value-direc,alpha_par,beta_par,p,
+                    incomplete_beta_inv_deriv_1(value,alpha_par,beta_par,lb_val),
+                    lb_val, iter_count+1) :
+            // else
+                value - direc );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_begin(const T initial_val, const T alpha_par, const T beta_par, const T p, const T lb_val)
+noexcept
+{
+    return incomplete_beta_inv_recur(initial_val,alpha_par,beta_par,p,
+               incomplete_beta_inv_deriv_1(initial_val,alpha_par,beta_par,lb_val),
+               lb_val,1);
+}
+
+template<typename T>
+constexpr
+T
+incomplete_beta_inv_check(const T alpha_par, const T beta_par, const T p)
+noexcept
+{
+    return( // NaN check
+            any_nan(alpha_par, beta_par, p) ? \
+                GCLIM<T>::quiet_NaN() :
+            // indistinguishable from zero or one
+            GCLIM<T>::min() > p ? \
+                T(0) :
+            GCLIM<T>::min() > abs(T(1) - p) ? \
+                T(1) :
+            // else
+                incomplete_beta_inv_begin(incomplete_beta_inv_initial_val(alpha_par,beta_par,p),
+                    alpha_par,beta_par,p,lbeta(alpha_par,beta_par)) );
+}
+
+template<typename T1, typename T2, typename T3, typename TC = common_t<T1,T2,T3>>
+constexpr
+TC
+incomplete_beta_inv_type_check(const T1 a, const T2 b, const T3 p)
+noexcept
+{
+    return incomplete_beta_inv_check(static_cast<TC>(a),
+                                     static_cast<TC>(b),
+                                     static_cast<TC>(p));
+}
+
+}
+
+/**
+ * Compile-time inverse incomplete beta function
+ *
+ * @param a a real-valued, non-negative input.
+ * @param b a real-valued, non-negative input.
+ * @param p a real-valued input with values in the unit-interval.
+ *
+ * @return Computes the inverse incomplete beta function, a value \f$ x \f$ such that 
+ * \f[ f(x) := \frac{\text{B}(x;\alpha,\beta)}{\text{B}(\alpha,\beta)} - p \f]
+ * equal to zero, for a given \c p.
+ * GCE-Math finds this root using Halley's method:
+ * \f[ x_{n+1} = x_n - \frac{f(x_n)/f'(x_n)}{1 - 0.5 \frac{f(x_n)}{f'(x_n)} \frac{f''(x_n)}{f'(x_n)} } \f]
+ * where
+ * \f[ \frac{\partial}{\partial x} \left(\frac{\text{B}(x;\alpha,\beta)}{\text{B}(\alpha,\beta)}\right) = \frac{1}{\text{B}(\alpha,\beta)} x^{\alpha-1} (1-x)^{\beta-1} \f]
+ * \f[ \frac{\partial^2}{\partial x^2} \left(\frac{\text{B}(x;\alpha,\beta)}{\text{B}(\alpha,\beta)}\right) = \frac{1}{\text{B}(\alpha,\beta)} x^{\alpha-1} (1-x)^{\beta-1} \left( \frac{\alpha-1}{x} - \frac{\beta-1}{1 - x} \right) \f]
+ */
+
+template<typename T1, typename T2, typename T3>
+constexpr
+common_t<T1,T2,T3>
+incomplete_beta_inv(const T1 a, const T2 b, const T3 p)
+noexcept
+{
+    return internal::incomplete_beta_inv_type_check(a,b,p);
+}
+
+#endif
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
new file mode 100644
index 0000000..38734a5
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma.hpp
@@ -0,0 +1,247 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/* 
+ * compile-time (regularized) incomplete gamma function
+ */
+
+#ifndef _gcem_incomplete_gamma_HPP
+#define _gcem_incomplete_gamma_HPP
+
+namespace internal
+{
+
+// 50 point Gauss-Legendre quadrature
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_quad_inp_vals(const T lb, const T ub, const int counter)
+noexcept
+{
+    return (ub-lb) * gauss_legendre_50_points[counter] / T(2) + (ub + lb) / T(2);
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_quad_weight_vals(const T lb, const T ub, const int counter)
+noexcept
+{
+    return (ub-lb) * gauss_legendre_50_weights[counter] / T(2);
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_quad_fn(const T x, const T a, const T lg_term)
+noexcept
+{
+    return exp( -x + (a-T(1))*log(x) - lg_term );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_quad_recur(const T lb, const T ub, const T a, const T lg_term, const int counter)
+noexcept
+{
+    return( counter < 49 ? \
+            // if 
+                incomplete_gamma_quad_fn(incomplete_gamma_quad_inp_vals(lb,ub,counter),a,lg_term) \
+                    * incomplete_gamma_quad_weight_vals(lb,ub,counter) \
+                    + incomplete_gamma_quad_recur(lb,ub,a,lg_term,counter+1) :
+            // else
+                incomplete_gamma_quad_fn(incomplete_gamma_quad_inp_vals(lb,ub,counter),a,lg_term) \
+                    * incomplete_gamma_quad_weight_vals(lb,ub,counter) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_quad_lb(const T a, const T z)
+noexcept
+{
+    return( a > T(1000) ? max(T(0),min(z,a) - 11*sqrt(a)) : // break integration into ranges
+            a > T(800)  ? max(T(0),min(z,a) - 11*sqrt(a)) :
+            a > T(500)  ? max(T(0),min(z,a) - 10*sqrt(a)) :
+            a > T(300)  ? max(T(0),min(z,a) - 10*sqrt(a)) : 
+            a > T(100)  ? max(T(0),min(z,a) -  9*sqrt(a)) :
+            a > T(90)   ? max(T(0),min(z,a) -  9*sqrt(a)) :
+            a > T(70)   ? max(T(0),min(z,a) -  8*sqrt(a)) :
+            a > T(50)   ? max(T(0),min(z,a) -  7*sqrt(a)) :
+            a > T(40)   ? max(T(0),min(z,a) -  6*sqrt(a)) :
+            a > T(30)   ? max(T(0),min(z,a) -  5*sqrt(a)) :
+            // else
+                max(T(0),min(z,a)-4*sqrt(a)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_quad_ub(const T a, const T z)
+noexcept
+{
+    return( a > T(1000) ? min(z, a + 10*sqrt(a)) :
+            a > T(800)  ? min(z, a + 10*sqrt(a)) :
+            a > T(500)  ? min(z, a + 9*sqrt(a))  :
+            a > T(300)  ? min(z, a + 9*sqrt(a))  : 
+            a > T(100)  ? min(z, a + 8*sqrt(a))  :
+            a > T(90)   ? min(z, a + 8*sqrt(a))  :
+            a > T(70)   ? min(z, a + 7*sqrt(a))  :
+            a > T(50)   ? min(z, a + 6*sqrt(a))  :
+            // else
+                min(z, a + 5*sqrt(a)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_quad(const T a, const T z)
+noexcept
+{
+    return incomplete_gamma_quad_recur(incomplete_gamma_quad_lb(a,z), incomplete_gamma_quad_ub(a,z), a,lgamma(a),0);
+}
+
+// reverse cf expansion
+// see: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_cf_2_recur(const T a, const T z, const int depth)
+noexcept
+{
+    return( depth < 100 ? \
+            // if
+                (1 + (depth-1)*2 - a + z) + depth*(a - depth)/incomplete_gamma_cf_2_recur(a,z,depth+1) :
+            // else
+                (1 + (depth-1)*2 - a + z) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_cf_2(const T a, const T z)
+noexcept
+{   // lower (regularized) incomplete gamma function
+    return( T(1.0) - exp(a*log(z) - z - lgamma(a)) / incomplete_gamma_cf_2_recur(a,z,1) );
+}
+
+// cf expansion
+// see: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_cf_1_coef(const T a, const T z, const int depth)
+noexcept
+{
+    return( is_odd(depth) ? - (a - 1 + T(depth+1)/T(2)) * z : T(depth)/T(2) * z );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_cf_1_recur(const T a, const T z, const int depth)
+noexcept
+{
+    return( depth < GCEM_INCML_GAMMA_MAX_ITER ? \
+            // if
+                (a + depth - 1) + incomplete_gamma_cf_1_coef(a,z,depth)/incomplete_gamma_cf_1_recur(a,z,depth+1) :
+            // else
+                (a + depth - 1) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_cf_1(const T a, const T z)
+noexcept
+{   // lower (regularized) incomplete gamma function
+    return( exp(a*log(z) - z - lgamma(a)) / incomplete_gamma_cf_1_recur(a,z,1) );
+}
+
+//
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_check(const T a, const T z)
+noexcept
+{
+    return( // NaN check
+            any_nan(a, z) ? \
+                GCLIM<T>::quiet_NaN() :
+            //
+            a < T(0) ? \
+                GCLIM<T>::quiet_NaN() :
+            //
+            GCLIM<T>::min() > z ? \
+                T(0) : 
+            //
+            GCLIM<T>::min() > a ? \
+                T(1) : 
+            // cf or quadrature
+            (a < T(10)) && (z - a < T(10)) ?
+                incomplete_gamma_cf_1(a,z) :
+            (a < T(10)) || (z/a > T(3)) ?
+                incomplete_gamma_cf_2(a,z) :
+            // else
+                incomplete_gamma_quad(a,z) );
+}
+
+template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
+constexpr
+TC
+incomplete_gamma_type_check(const T1 a, const T2 p)
+noexcept
+{
+    return incomplete_gamma_check(static_cast<TC>(a),
+                                  static_cast<TC>(p));
+}
+
+}
+
+/**
+ * Compile-time regularized lower incomplete gamma function
+ *
+ * @param a a real-valued, non-negative input.
+ * @param x a real-valued, non-negative input.
+ *
+ * @return the regularized lower incomplete gamma function evaluated at (\c a, \c x),
+ * \f[ \frac{\gamma(a,x)}{\Gamma(a)} = \frac{1}{\Gamma(a)} \int_0^x t^{a-1} \exp(-t) dt \f]
+ * When \c a is not too large, the value is computed using the continued fraction representation of the upper incomplete gamma function, \f$ \Gamma(a,x) \f$, using
+ * \f[ \Gamma(a,x) = \Gamma(a) - \dfrac{x^a\exp(-x)}{a - \dfrac{ax}{a + 1 + \dfrac{x}{a + 2 - \dfrac{(a+1)x}{a + 3 + \dfrac{2x}{a + 4 - \ddots}}}}} \f]
+ * where \f$ \gamma(a,x) \f$ and \f$ \Gamma(a,x) \f$ are connected via
+ * \f[ \frac{\gamma(a,x)}{\Gamma(a)} + \frac{\Gamma(a,x)}{\Gamma(a)} = 1 \f]
+ * When \f$ a > 10 \f$, a 50-point Gauss-Legendre quadrature scheme is employed.
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_return_t<T1,T2>
+incomplete_gamma(const T1 a, const T2 x)
+noexcept
+{
+    return internal::incomplete_gamma_type_check(a,x);
+}
+
+#endif
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
new file mode 100644
index 0000000..1e57fc1
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/incomplete_gamma_inv.hpp
@@ -0,0 +1,271 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/* 
+ * inverse of the incomplete gamma function
+ */
+
+#ifndef _gcem_incomplete_gamma_inv_HPP
+#define _gcem_incomplete_gamma_inv_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr T incomplete_gamma_inv_decision(const T value, const T a, const T p, const T direc, const T lg_val, const int iter_count) noexcept;
+
+//
+// initial value for Halley
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_t_val_1(const T p)
+noexcept
+{   // a > 1.0
+    return( p > T(0.5) ? sqrt(-2*log(T(1) - p)) : sqrt(-2*log(p)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_t_val_2(const T a)
+noexcept
+{   // a <= 1.0
+    return( T(1) - T(0.253) * a - T(0.12) * a*a );
+}
+
+//
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_initial_val_1_int_begin(const T t_val)
+noexcept
+{   // internal for a > 1.0
+    return( t_val - (T(2.515517L) + T(0.802853L)*t_val + T(0.010328L)*t_val*t_val) \
+                / (T(1) + T(1.432788L)*t_val + T(0.189269L)*t_val*t_val + T(0.001308L)*t_val*t_val*t_val) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_initial_val_1_int_end(const T value_inp, const T a)
+noexcept
+{   // internal for a > 1.0
+    return max( T(1E-04), a*pow(T(1) - T(1)/(9*a) - value_inp/(3*sqrt(a)), 3) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_initial_val_1(const T a, const T t_val, const T sgn_term)
+noexcept
+{   // a > 1.0
+    return incomplete_gamma_inv_initial_val_1_int_end(sgn_term*incomplete_gamma_inv_initial_val_1_int_begin(t_val), a);
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_initial_val_2(const T a, const T p, const T t_val)
+noexcept
+{   // a <= 1.0
+    return( p < t_val ? \
+             // if 
+                pow(p/t_val,T(1)/a) : 
+             // else
+                T(1) - log(T(1) - (p - t_val)/(T(1) - t_val)) );
+}
+
+// initial value
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_initial_val(const T a, const T p)
+noexcept
+{
+    return( a > T(1) ? \
+             // if
+                incomplete_gamma_inv_initial_val_1(a,
+                    incomplete_gamma_inv_t_val_1(p),
+                    p > T(0.5) ? T(-1) : T(1)) :
+             // else
+                incomplete_gamma_inv_initial_val_2(a,p,
+                    incomplete_gamma_inv_t_val_2(a)));
+}
+
+//
+// Halley recursion
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_err_val(const T value, const T a, const T p)
+noexcept
+{ // err_val = f(x)
+    return( incomplete_gamma(a,value) - p );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_deriv_1(const T value, const T a, const T lg_val)
+noexcept
+{ // derivative of the incomplete gamma function w.r.t. x
+    return( exp( - value + (a - T(1))*log(value) - lg_val ) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_deriv_2(const T value, const T a, const T deriv_1)
+noexcept
+{ // second derivative of the incomplete gamma function w.r.t. x
+    return( deriv_1*((a - T(1))/value - T(1)) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_ratio_val_1(const T value, const T a, const T p, const T deriv_1)
+noexcept
+{
+    return( incomplete_gamma_inv_err_val(value,a,p) / deriv_1 );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_ratio_val_2(const T value, const T a, const T deriv_1)
+noexcept
+{
+    return( incomplete_gamma_inv_deriv_2(value,a,deriv_1) / deriv_1 );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_halley(const T ratio_val_1, const T ratio_val_2)
+noexcept
+{
+    return( ratio_val_1 / max( T(0.8), min( T(1.2), T(1) - T(0.5)*ratio_val_1*ratio_val_2 ) ) );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_recur(const T value, const T a, const T p, const T deriv_1, const T lg_val, const int iter_count)
+noexcept
+{
+    return incomplete_gamma_inv_decision(value, a, p,
+                incomplete_gamma_inv_halley(incomplete_gamma_inv_ratio_val_1(value,a,p,deriv_1), 
+                incomplete_gamma_inv_ratio_val_2(value,a,deriv_1)),
+                lg_val, iter_count);
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_decision(const T value, const T a, const T p, const T direc, const T lg_val, const int iter_count)
+noexcept
+{
+    // return( abs(direc) > GCEM_INCML_GAMMA_INV_TOL ? incomplete_gamma_inv_recur(value - direc, a, p, incomplete_gamma_inv_deriv_1(value,a,lg_val), lg_val) : value - direc );
+    return( iter_count <= GCEM_INCML_GAMMA_INV_MAX_ITER ? \
+            // if
+                incomplete_gamma_inv_recur(value-direc,a,p,
+                    incomplete_gamma_inv_deriv_1(value,a,lg_val),
+                    lg_val,iter_count+1) :
+            // else 
+                value - direc );
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_begin(const T initial_val, const T a, const T p, const T lg_val)
+noexcept
+{
+    return incomplete_gamma_inv_recur(initial_val,a,p,
+                incomplete_gamma_inv_deriv_1(initial_val,a,lg_val), lg_val,1);
+}
+
+template<typename T>
+constexpr
+T
+incomplete_gamma_inv_check(const T a, const T p)
+noexcept
+{
+    return( // NaN check
+            any_nan(a, p) ? \
+                GCLIM<T>::quiet_NaN() :
+            //
+            GCLIM<T>::min() > p ? \
+                T(0) :
+            p > T(1) ? \
+                GCLIM<T>::quiet_NaN() :
+            GCLIM<T>::min() > abs(T(1) - p) ? \
+                GCLIM<T>::infinity() :
+            //
+            GCLIM<T>::min() > a ? \
+                T(0) :
+            // else
+                incomplete_gamma_inv_begin(incomplete_gamma_inv_initial_val(a,p),a,p,lgamma(a)) );
+}
+
+template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
+constexpr
+TC
+incomplete_gamma_inv_type_check(const T1 a, const T2 p)
+noexcept
+{
+    return incomplete_gamma_inv_check(static_cast<TC>(a),
+                                      static_cast<TC>(p));
+}
+
+}
+
+/**
+ * Compile-time inverse incomplete gamma function
+ *
+ * @param a a real-valued, non-negative input.
+ * @param p a real-valued input with values in the unit-interval.
+ *
+ * @return Computes the inverse incomplete gamma function, a value \f$ x \f$ such that 
+ * \f[ f(x) := \frac{\gamma(a,x)}{\Gamma(a)} - p \f]
+ * equal to zero, for a given \c p.
+ * GCE-Math finds this root using Halley's method:
+ * \f[ x_{n+1} = x_n - \frac{f(x_n)/f'(x_n)}{1 - 0.5 \frac{f(x_n)}{f'(x_n)} \frac{f''(x_n)}{f'(x_n)} } \f]
+ * where
+ * \f[ \frac{\partial}{\partial x} \left(\frac{\gamma(a,x)}{\Gamma(a)}\right) = \frac{1}{\Gamma(a)} x^{a-1} \exp(-x) \f]
+ * \f[ \frac{\partial^2}{\partial x^2} \left(\frac{\gamma(a,x)}{\Gamma(a)}\right) = \frac{1}{\Gamma(a)} x^{a-1} \exp(-x) \left( \frac{a-1}{x} - 1 \right) \f]
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_return_t<T1,T2>
+incomplete_gamma_inv(const T1 a, const T2 p)
+noexcept
+{
+    return internal::incomplete_gamma_inv_type_check(a,p);
+}
+
+#endif
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
new file mode 100644
index 0000000..0200f11
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/inv_sqrt.hpp
@@ -0,0 +1,88 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time inverse-square-root function
+ */
+
+#ifndef _gcem_inv_sqrt_HPP
+#define _gcem_inv_sqrt_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+inv_sqrt_recur(const T x, const T xn, const int count)
+noexcept
+{
+    return( abs( xn - T(1)/(x*xn) ) / (T(1) + xn) < GCLIM<T>::min() ? \
+            // if
+                xn :
+            count < GCEM_INV_SQRT_MAX_ITER ? \
+            // else
+                inv_sqrt_recur(x, T(0.5)*(xn + T(1)/(x*xn)), count+1) :
+                xn );
+}
+
+template<typename T>
+constexpr
+T
+inv_sqrt_check(const T x)
+noexcept
+{
+    return( is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            //
+            x < T(0) ? \
+                GCLIM<T>::quiet_NaN() :
+            //
+            is_posinf(x) ? \
+                T(0) :
+            // indistinguishable from zero or one
+            GCLIM<T>::min() > abs(x) ? \
+                GCLIM<T>::infinity() :
+            GCLIM<T>::min() > abs(T(1) - x) ? \
+                x :
+            // else
+            inv_sqrt_recur(x, x/T(2), 0) );
+}
+
+}
+
+
+/**
+ * Compile-time inverse-square-root function
+ *
+ * @param x a real-valued input.
+ * @return Computes \f$ 1 / \sqrt{x} \f$ using a Newton-Raphson approach.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+inv_sqrt(const T x)
+noexcept
+{
+    return internal::inv_sqrt_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..fa925bb
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_even.hpp
@@ -0,0 +1,41 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time check if integer is even
+ */
+
+#ifndef _gcem_is_even_HPP
+#define _gcem_is_even_HPP
+
+namespace internal
+{
+
+constexpr
+bool
+is_even(const llint_t x)
+noexcept
+{
+    return !is_odd(x);
+}
+
+}
+
+#endif
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
new file mode 100644
index 0000000..25f2e3c
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_finite.hpp
@@ -0,0 +1,78 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time check if a float is not NaN-valued or +/-Inf
+ */
+
+#ifndef _gcem_is_finite_HPP
+#define _gcem_is_finite_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+bool
+is_finite(const T x)
+noexcept
+{
+    return (!is_nan(x)) && (!is_inf(x));
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+any_finite(const T1 x, const T2 y)
+noexcept
+{
+    return( is_finite(x) || is_finite(y) );
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+all_finite(const T1 x, const T2 y)
+noexcept
+{
+    return( is_finite(x) && is_finite(y) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+any_finite(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+    return( is_finite(x) || is_finite(y) || is_finite(z) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+all_finite(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+    return( is_finite(x) && is_finite(y) && is_finite(z) );
+}
+
+}
+
+#endif
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
new file mode 100644
index 0000000..627c509
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_inf.hpp
@@ -0,0 +1,172 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time check if a float is +/-Inf
+ */
+
+#ifndef _gcem_is_inf_HPP
+#define _gcem_is_inf_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+bool
+is_neginf(const T x)
+noexcept
+{
+    return x == - GCLIM<T>::infinity();
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+any_neginf(const T1 x, const T2 y)
+noexcept
+{
+    return( is_neginf(x) || is_neginf(y) );
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+all_neginf(const T1 x, const T2 y)
+noexcept
+{
+    return( is_neginf(x) && is_neginf(y) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+any_neginf(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+    return( is_neginf(x) || is_neginf(y) || is_neginf(z) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+all_neginf(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+    return( is_neginf(x) && is_neginf(y) && is_neginf(z) );
+}
+
+//
+
+template<typename T>
+constexpr
+bool
+is_posinf(const T x)
+noexcept
+{
+    return x == GCLIM<T>::infinity();
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+any_posinf(const T1 x, const T2 y)
+noexcept
+{
+    return( is_posinf(x) || is_posinf(y) );
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+all_posinf(const T1 x, const T2 y)
+noexcept
+{
+    return( is_posinf(x) && is_posinf(y) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+any_posinf(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+    return( is_posinf(x) || is_posinf(y) || is_posinf(z) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+all_posinf(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+    return( is_posinf(x) && is_posinf(y) && is_posinf(z) );
+}
+
+//
+
+template<typename T>
+constexpr
+bool
+is_inf(const T x)
+noexcept
+{
+    return( is_neginf(x) || is_posinf(x) );
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+any_inf(const T1 x, const T2 y)
+noexcept
+{
+    return( is_inf(x) || is_inf(y) );
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+all_inf(const T1 x, const T2 y)
+noexcept
+{
+    return( is_inf(x) && is_inf(y) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+any_inf(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+    return( is_inf(x) || is_inf(y) || is_inf(z) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+all_inf(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+    return( is_inf(x) && is_inf(y) && is_inf(z) );
+}
+
+}
+
+#endif
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
new file mode 100644
index 0000000..a7a1af3
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_nan.hpp
@@ -0,0 +1,80 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time check if a float is NaN-valued
+ */
+
+#ifndef _gcem_is_nan_HPP
+#define _gcem_is_nan_HPP
+
+namespace internal
+{
+
+// future: consider using __builtin_isnan(__x)
+
+template<typename T>
+constexpr
+bool
+is_nan(const T x)
+noexcept
+{
+    return x != x;
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+any_nan(const T1 x, const T2 y)
+noexcept
+{
+    return( is_nan(x) || is_nan(y) );
+}
+
+template<typename T1, typename T2>
+constexpr
+bool
+all_nan(const T1 x, const T2 y)
+noexcept
+{
+    return( is_nan(x) && is_nan(y) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+any_nan(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+    return( is_nan(x) || is_nan(y) || is_nan(z) );
+}
+
+template<typename T1, typename T2, typename T3>
+constexpr
+bool
+all_nan(const T1 x, const T2 y, const T3 z)
+noexcept
+{
+    return( is_nan(x) && is_nan(y) && is_nan(z) );
+}
+
+}
+
+#endif
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
new file mode 100644
index 0000000..e6da720
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/is_odd.hpp
@@ -0,0 +1,42 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time check if integer is odd
+ */
+
+#ifndef _gcem_is_odd_HPP
+#define _gcem_is_odd_HPP
+
+namespace internal
+{
+
+constexpr
+bool
+is_odd(const llint_t x)
+noexcept
+{
+    // return( x % llint_t(2) == llint_t(0) ? false : true );
+    return (x & 1U) != 0;
+}
+
+}
+
+#endif
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
new file mode 100644
index 0000000..2213849
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lbeta.hpp
@@ -0,0 +1,42 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_lbeta_HPP
+#define _gcem_lbeta_HPP
+
+/**
+ * Compile-time log-beta function
+ *
+ * @param a a real-valued input.
+ * @param b a real-valued input.
+ * @return the log-beta function using \f[ \ln \text{B}(\alpha,\beta) := \ln \int_0^1 t^{\alpha - 1} (1-t)^{\beta - 1} dt = \ln \Gamma(\alpha) + \ln \Gamma(\beta) - \ln \Gamma(\alpha + \beta) \f]
+ * where \f$ \Gamma \f$ denotes the gamma function.
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_return_t<T1,T2>
+lbeta(const T1 a, const T2 b)
+noexcept
+{
+    return( (lgamma(a) + lgamma(b)) - lgamma(a+b) );
+}
+
+#endif
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
new file mode 100644
index 0000000..b0d8fb4
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lcm.hpp
@@ -0,0 +1,65 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_lcm_HPP
+#define _gcem_lcm_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+lcm_compute(const T a, const T b)
+noexcept
+{
+    return abs(a * (b / gcd(a,b)));
+}
+
+template<typename T1, typename T2, typename TC = common_t<T1,T2>>
+constexpr
+TC
+lcm_type_check(const T1 a, const T2 b)
+noexcept
+{
+    return lcm_compute(static_cast<TC>(a),static_cast<TC>(b));
+}
+
+}
+
+/**
+ * Compile-time least common multiple (LCM) function
+ *
+ * @param a integral-valued input.
+ * @param b integral-valued input.
+ * @return the least common multiple between integers \c a and \c b using the representation \f[ \text{lcm}(a,b) = \dfrac{| a b |}{\text{gcd}(a,b)} \f]
+ * where \f$ \text{gcd}(a,b) \f$ denotes the greatest common divisor between \f$ a \f$ and \f$ b \f$.
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_t<T1,T2>
+lcm(const T1 a, const T2 b)
+noexcept
+{
+    return internal::lcm_type_check(a,b);
+}
+
+#endif
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
new file mode 100644
index 0000000..5d78eb3
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lgamma.hpp
@@ -0,0 +1,135 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time log-gamma function
+ * 
+ * for coefficient values, see:
+ * http://my.fit.edu/~gabdo/gamma.txt
+ */
+
+#ifndef _gcem_lgamma_HPP
+#define _gcem_lgamma_HPP
+
+namespace internal
+{
+
+// P. Godfrey's coefficients:
+//
+//  0.99999999999999709182
+//  57.156235665862923517
+// -59.597960355475491248
+//  14.136097974741747174
+//  -0.49191381609762019978
+//    .33994649984811888699e-4
+//    .46523628927048575665e-4
+//   -.98374475304879564677e-4
+//    .15808870322491248884e-3
+//   -.21026444172410488319e-3
+//    .21743961811521264320e-3
+//   -.16431810653676389022e-3
+//    .84418223983852743293e-4
+//   -.26190838401581408670e-4
+//    .36899182659531622704e-5
+
+constexpr
+long double
+lgamma_coef_term(const long double x)
+noexcept
+{
+    return(     0.99999999999999709182L             + 57.156235665862923517L      / (x+1)  \
+             - 59.597960355475491248L      / (x+2)  + 14.136097974741747174L      / (x+3)  \
+             -  0.49191381609762019978L    / (x+4)  +   .33994649984811888699e-4L / (x+5)  \
+             +   .46523628927048575665e-4L / (x+6)  -   .98374475304879564677e-4L / (x+7)  \
+             +   .15808870322491248884e-3L / (x+8)  -   .21026444172410488319e-3L / (x+9)  \
+             +   .21743961811521264320e-3L / (x+10) -   .16431810653676389022e-3L / (x+11) \
+             +   .84418223983852743293e-4L / (x+12) -   .26190838401581408670e-4L / (x+13) \
+             +   .36899182659531622704e-5L / (x+14) );
+}
+
+template<typename T>
+constexpr
+T
+lgamma_term_2(const T x)
+noexcept
+{ //
+    return( T(GCEM_LOG_SQRT_2PI) + log(T(lgamma_coef_term(x))) );
+}
+
+template<typename T>
+constexpr
+T
+lgamma_term_1(const T x)
+noexcept
+{   // note: 607/128 + 0.5 = 5.2421875
+    return( (x + T(0.5))*log(x + T(5.2421875L)) - (x + T(5.2421875L)) );
+}
+
+template<typename T>
+constexpr
+T
+lgamma_begin(const T x)
+noexcept
+{   // returns lngamma(x+1)
+    return( lgamma_term_1(x) + lgamma_term_2(x) );
+}
+
+template<typename T>
+constexpr
+T
+lgamma_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // indistinguishable from one or <= zero
+            GCLIM<T>::min() > abs(x - T(1)) ? \
+                T(0) :
+            GCLIM<T>::min() > x ? \
+                GCLIM<T>::infinity() :
+            // else
+                lgamma_begin(x - T(1)) );
+}
+
+}
+
+/**
+ * Compile-time log-gamma function
+ *
+ * @param x a real-valued input.
+ * @return computes the log-gamma function
+ * \f[ \ln \Gamma(x) = \ln \int_0^\infty y^{x-1} \exp(-y) dy \f]
+ * using a polynomial form:
+ * \f[ \Gamma(x+1) \approx (x+g+0.5)^{x+0.5} \exp(-x-g-0.5) \sqrt{2 \pi} \left[ c_0 + \frac{c_1}{x+1} + \frac{c_2}{x+2} + \cdots + \frac{c_n}{x+n} \right] \f]
+ * where the value \f$ g \f$ and the coefficients \f$ (c_0, c_1, \ldots, c_n) \f$
+ * are taken from Paul Godfrey, whose note can be found here: http://my.fit.edu/~gabdo/gamma.txt
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+lgamma(const T x)
+noexcept
+{
+    return internal::lgamma_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..76bf833
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/lmgamma.hpp
@@ -0,0 +1,73 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/* 
+ * log multivariate gamma function
+ */
+
+#ifndef _gcem_lmgamma_HPP
+#define _gcem_lmgamma_HPP
+
+namespace internal
+{
+
+// see https://en.wikipedia.org/wiki/Multivariate_gamma_function
+
+template<typename T1, typename T2>
+constexpr
+T1
+lmgamma_recur(const T1 a, const T2 p)
+noexcept
+{
+    return( // NaN check
+            is_nan(a) ? \
+                GCLIM<T1>::quiet_NaN() :
+            //
+            p == T2(1) ? \
+                lgamma(a) :
+            p <  T2(1) ? \
+                GCLIM<T1>::quiet_NaN() :
+            // else
+                T1(GCEM_LOG_PI) * (p - T1(1))/T1(2) \
+                    + lgamma(a) + lmgamma_recur(a - T1(0.5),p-T2(1)) );
+}
+
+}
+
+/**
+ * Compile-time log multivariate gamma function
+ *
+ * @param a a real-valued input.
+ * @param p integral-valued input.
+ * @return computes log-multivariate gamma function via recursion
+ * \f[ \Gamma_p(a) = \pi^{(p-1)/2} \Gamma(a) \Gamma_{p-1}(a-0.5) \f]
+ * where \f$ \Gamma_1(a) = \Gamma(a) \f$.
+ */
+
+template<typename T1, typename T2>
+constexpr
+return_t<T1>
+lmgamma(const T1 a, const T2 p)
+noexcept
+{
+    return internal::lmgamma_recur(static_cast<return_t<T1>>(a),p);
+}
+
+#endif
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
new file mode 100644
index 0000000..0d83e97
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log.hpp
@@ -0,0 +1,162 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time natural logarithm function
+ */
+
+#ifndef _gcem_log_HPP
+#define _gcem_log_HPP
+
+namespace internal
+{
+
+// continued fraction seems to be a better approximation for small x
+// see http://functions.wolfram.com/ElementaryFunctions/Log/10/0005/
+
+template<typename T>
+constexpr
+T
+log_cf_main(const T xx, const int depth)
+noexcept
+{
+    return( depth < GCEM_LOG_MAX_ITER_SMALL ? \
+            // if 
+                T(2*depth - 1) - T(depth*depth)*xx/log_cf_main(xx,depth+1) :
+            // else 
+                T(2*depth - 1) );
+}
+
+template<typename T>
+constexpr
+T
+log_cf_begin(const T x)
+noexcept
+{ 
+    return( T(2)*x/log_cf_main(x*x,1) );
+}
+
+template<typename T>
+constexpr
+T
+log_main(const T x)
+noexcept
+{ 
+    return( log_cf_begin((x - T(1))/(x + T(1))) );
+}
+
+constexpr
+long double
+log_mantissa_integer(const int x)
+noexcept
+{
+    return( x == 2  ? 0.6931471805599453094172321214581765680755L :
+            x == 3  ? 1.0986122886681096913952452369225257046475L :
+            x == 4  ? 1.3862943611198906188344642429163531361510L :
+            x == 5  ? 1.6094379124341003746007593332261876395256L :
+            x == 6  ? 1.7917594692280550008124773583807022727230L :
+            x == 7  ? 1.9459101490553133051053527434431797296371L :
+            x == 8  ? 2.0794415416798359282516963643745297042265L :
+            x == 9  ? 2.1972245773362193827904904738450514092950L :
+            x == 10 ? 2.3025850929940456840179914546843642076011L :
+                      0.0L );
+}
+
+template<typename T>
+constexpr
+T
+log_mantissa(const T x)
+noexcept
+{   // divide by the integer part of x, which will be in [1,10], then adjust using tables
+    return( log_main(x/T(static_cast<int>(x))) + T(log_mantissa_integer(static_cast<int>(x))) );
+}
+
+template<typename T>
+constexpr
+T
+log_breakup(const T x)
+noexcept
+{   // x = a*b, where b = 10^c
+    return( log_mantissa(mantissa(x)) + T(GCEM_LOG_10)*T(find_exponent(x,0)) );
+}
+
+template<typename T>
+constexpr
+T
+log_check(const T x)
+noexcept
+{
+    return( is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // x < 0
+            x < T(0) ? \
+                GCLIM<T>::quiet_NaN() :
+            // x ~= 0
+            GCLIM<T>::min() > x ? \
+                - GCLIM<T>::infinity() :
+            // indistinguishable from 1
+            GCLIM<T>::min() > abs(x - T(1)) ? \
+                T(0) : 
+            // 
+            x == GCLIM<T>::infinity() ? \
+                GCLIM<T>::infinity() :
+            // else
+                (x < T(0.5) || x > T(1.5)) ?
+                // if 
+                    log_breakup(x) :
+                // else
+                    log_main(x) );
+}
+
+template<typename T>
+constexpr
+return_t<T>
+log_integral_check(const T x)
+noexcept
+{
+    return( std::is_integral<T>::value ? \
+                x == T(0) ? \
+                    - GCLIM<return_t<T>>::infinity() :
+                x > T(1) ? \
+                    log_check( static_cast<return_t<T>>(x) ) :
+                    static_cast<return_t<T>>(0) :
+            log_check( static_cast<return_t<T>>(x) ) );
+}
+
+}
+
+/**
+ * Compile-time natural logarithm function
+ *
+ * @param x a real-valued input.
+ * @return \f$ \log_e(x) \f$ using \f[ \log\left(\frac{1+x}{1-x}\right) = \dfrac{2x}{1-\dfrac{x^2}{3-\dfrac{4x^2}{5 - \dfrac{9x^3}{7 - \ddots}}}}, \ \ x \in [-1,1] \f] 
+ * The continued fraction argument is split into two parts: \f$ x = a \times 10^c \f$, where \f$ c \f$ is an integer.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+log(const T x)
+noexcept
+{
+    return internal::log_integral_check( x );
+}
+
+#endif
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
new file mode 100644
index 0000000..4a3c37d
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log10.hpp
@@ -0,0 +1,59 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time common logarithm function
+ */
+
+#ifndef _gcem_log10_HPP
+#define _gcem_log10_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+return_t<T>
+log10_check(const T x)
+noexcept
+{
+    // log_10(x) = ln(x) / ln(10)
+    return return_t<T>(log(x) / GCEM_LOG_10);
+}
+
+}
+
+/**
+ * Compile-time common logarithm function
+ *
+ * @param x a real-valued input.
+ * @return \f$ \log_{10}(x) \f$ using \f[ \log_{10}(x) = \frac{\log_e(x)}{\log_e(10)} \f] 
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+log10(const T x)
+noexcept
+{
+    return internal::log10_check( x );
+}
+
+#endif
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
new file mode 100644
index 0000000..3883b22
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log1p.hpp
@@ -0,0 +1,80 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time natural logarithm(x+1) function
+ */
+
+#ifndef _gcem_log1p_HPP
+#define _gcem_log1p_HPP
+
+namespace internal
+{
+
+// see:
+// http://functions.wolfram.com/ElementaryFunctions/Log/06/01/04/01/0003/
+
+
+template<typename T>
+constexpr
+T
+log1p_compute(const T x)
+noexcept
+{
+    // return x * ( T(1) + x * ( -T(1)/T(2) +  x * ( T(1)/T(3) +  x * ( -T(1)/T(4) + x/T(5) ) ) ) ); // O(x^6)
+    return x + x * ( - x/T(2) +  x * ( x/T(3) +  x * ( -x/T(4) + x*x/T(5) ) ) ); // O(x^6)
+}
+
+template<typename T>
+constexpr
+T
+log1p_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            //
+            abs(x) > T(1e-04) ? \
+            // if
+                log(T(1) + x) :
+            // else    
+                log1p_compute(x) );
+}
+
+}
+
+/**
+ * Compile-time natural-logarithm-plus-1 function
+ *
+ * @param x a real-valued input.
+ * @return \f$ \log_e(x+1) \f$ using \f[ \log(x+1) = \sum_{k=1}^\infty \dfrac{(-1)^{k-1}x^k}{k}, \ \ |x| < 1 \f] 
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+log1p(const T x)
+noexcept
+{
+    return internal::log1p_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..56b7f8e
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log2.hpp
@@ -0,0 +1,59 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time binary logarithm function
+ */
+
+#ifndef _gcem_log2_HPP
+#define _gcem_log2_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+return_t<T>
+log2_check(const T x)
+noexcept
+{
+    // log_2(x) = ln(x) / ln(2)
+    return return_t<T>(log(x) / GCEM_LOG_2);
+}
+
+}
+
+/**
+ * Compile-time binary logarithm function
+ *
+ * @param x a real-valued input.
+ * @return \f$ \log_2(x) \f$ using \f[ \log_{2}(x) = \frac{\log_e(x)}{\log_e(2)} \f] 
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+log2(const T x)
+noexcept
+{
+    return internal::log2_check( x );
+}
+
+#endif
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
new file mode 100644
index 0000000..7aa9a2b
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/log_binomial_coef.hpp
@@ -0,0 +1,65 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_log_binomial_coef_HPP
+#define _gcem_log_binomial_coef_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+log_binomial_coef_compute(const T n, const T k)
+noexcept
+{
+    return( lgamma(n+1) - (lgamma(k+1) + lgamma(n-k+1)) );
+}
+
+template<typename T1, typename T2, typename TC = common_return_t<T1,T2>>
+constexpr
+TC
+log_binomial_coef_type_check(const T1 n, const T2 k)
+noexcept
+{
+    return log_binomial_coef_compute(static_cast<TC>(n),static_cast<TC>(k));
+}
+
+}
+
+/**
+ * Compile-time log binomial coefficient
+ *
+ * @param n integral-valued input.
+ * @param k integral-valued input.
+ * @return computes the log Binomial coefficient
+ * \f[ \ln \frac{n!}{k!(n-k)!} = \ln \Gamma(n+1) - [ \ln \Gamma(k+1) + \ln \Gamma(n-k+1) ] \f]
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_return_t<T1,T2>
+log_binomial_coef(const T1 n, const T2 k)
+noexcept
+{
+    return internal::log_binomial_coef_type_check(n,k);
+}
+
+#endif
\ No newline at end of file
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
new file mode 100644
index 0000000..df6152b
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/mantissa.hpp
@@ -0,0 +1,47 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time mantissa function
+ */
+
+#ifndef _gcem_mantissa_HPP
+#define _gcem_mantissa_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+mantissa(const T x)
+noexcept
+{
+    return( x < T(1) ? \
+                mantissa(x*T(10)) : 
+            x > T(10) ? \
+                mantissa(x/T(10)) :
+            // else
+                x );
+}
+
+}
+
+#endif
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
new file mode 100644
index 0000000..4aed84f
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp
@@ -0,0 +1,41 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_max_HPP
+#define _gcem_max_HPP
+
+/**
+ * Compile-time pairwise maximum function
+ *
+ * @param x a real-valued input.
+ * @param y a real-valued input.
+ * @return Computes the maximum between \c x and \c y, where \c x and \c y have the same type (e.g., \c int, \c double, etc.)
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_t<T1,T2>
+max(const T1 x, const T2 y)
+noexcept
+{
+    return( y < x ? x : y );
+}
+
+#endif
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
new file mode 100644
index 0000000..d593dbc
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp
@@ -0,0 +1,41 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_min_HPP
+#define _gcem_min_HPP
+
+/**
+ * Compile-time pairwise minimum function
+ *
+ * @param x a real-valued input.
+ * @param y a real-valued input.
+ * @return Computes the minimum between \c x and \c y, where \c x and \c y have the same type (e.g., \c int, \c double, etc.)
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_t<T1,T2>
+min(const T1 x, const T2 y)
+noexcept
+{
+    return( y > x ? x : y );
+}
+
+#endif
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
new file mode 100644
index 0000000..db33f87
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/neg_zero.hpp
@@ -0,0 +1,37 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * extract signbit for signed zeros
+ */
+
+namespace internal
+{
+
+template<typename T>
+constexpr 
+bool 
+neg_zero(const T x)
+noexcept
+{
+    return( (x == T(0.0)) && (copysign(T(1.0), x) == T(-1.0)) );
+}
+
+}
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
new file mode 100644
index 0000000..166a8c1
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow.hpp
@@ -0,0 +1,82 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time power function
+ */
+
+#ifndef _gcem_pow_HPP
+#define _gcem_pow_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+pow_dbl(const T base, const T exp_term)
+noexcept
+{
+    return exp(exp_term*log(base));
+}
+
+template<typename T1, typename T2, typename TC = common_t<T1,T2>, 
+         typename std::enable_if<!std::is_integral<T2>::value>::type* = nullptr>
+constexpr
+TC
+pow_check(const T1 base, const T2 exp_term)
+noexcept
+{
+    return( base < T1(0) ? \
+                GCLIM<TC>::quiet_NaN() :
+            //
+            pow_dbl(static_cast<TC>(base),static_cast<TC>(exp_term)) );
+}
+
+template<typename T1, typename T2, typename TC = common_t<T1,T2>, 
+         typename std::enable_if<std::is_integral<T2>::value>::type* = nullptr>
+constexpr
+TC
+pow_check(const T1 base, const T2 exp_term)
+noexcept
+{
+    return pow_integral(base,exp_term);
+}
+
+}
+
+/**
+ * Compile-time power function
+ *
+ * @param base a real-valued input. 
+ * @param exp_term a real-valued input.
+ * @return Computes \c base raised to the power \c exp_term. In the case where \c exp_term is integral-valued, recursion by squaring is used, otherwise \f$ \text{base}^{\text{exp\_term}} = e^{\text{exp\_term} \log(\text{base})} \f$
+ */
+
+template<typename T1, typename T2>
+constexpr
+common_t<T1,T2>
+pow(const T1 base, const T2 exp_term)
+noexcept
+{
+    return internal::pow_check(base,exp_term);
+}
+
+#endif
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
new file mode 100644
index 0000000..3a902ca
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/pow_integral.hpp
@@ -0,0 +1,128 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time power function
+ */
+
+#ifndef _gcem_pow_integral_HPP
+#define _gcem_pow_integral_HPP
+
+namespace internal
+{
+
+template<typename T1, typename T2>
+constexpr T1 pow_integral_compute(const T1 base, const T2 exp_term) noexcept;
+
+// integral-valued powers using method described in 
+// https://en.wikipedia.org/wiki/Exponentiation_by_squaring
+
+template<typename T1, typename T2>
+constexpr
+T1
+pow_integral_compute_recur(const T1 base, const T1 val, const T2 exp_term)
+noexcept
+{
+    return( exp_term > T2(1) ? \
+                (is_odd(exp_term) ? \
+                    pow_integral_compute_recur(base*base,val*base,exp_term/2) :
+                    pow_integral_compute_recur(base*base,val,exp_term/2)) :
+                (exp_term == T2(1) ? val*base : val) );
+}
+
+template<typename T1, typename T2, typename std::enable_if<std::is_signed<T2>::value>::type* = nullptr>
+constexpr
+T1
+pow_integral_sgn_check(const T1 base, const T2 exp_term)
+noexcept
+{
+    return( exp_term < T2(0) ? \
+            //
+                T1(1) / pow_integral_compute(base, - exp_term) : 
+            //
+                pow_integral_compute_recur(base,T1(1),exp_term) );
+}
+
+template<typename T1, typename T2, typename std::enable_if<!std::is_signed<T2>::value>::type* = nullptr>
+constexpr
+T1
+pow_integral_sgn_check(const T1 base, const T2 exp_term)
+noexcept
+{
+    return( pow_integral_compute_recur(base,T1(1),exp_term) );
+}
+
+template<typename T1, typename T2>
+constexpr
+T1
+pow_integral_compute(const T1 base, const T2 exp_term)
+noexcept
+{
+    return( exp_term == T2(3) ? \
+                base*base*base :
+            exp_term == T2(2) ? \
+                base*base :
+            exp_term == T2(1) ? \
+                base :
+            exp_term == T2(0) ? \
+                T1(1) :
+            // check for overflow
+            exp_term == GCLIM<T2>::min() ? \
+                T1(0) :
+            exp_term == GCLIM<T2>::max() ? \
+                GCLIM<T1>::infinity() :
+            // else
+            pow_integral_sgn_check(base,exp_term) );
+}
+
+template<typename T1, typename T2, typename std::enable_if<std::is_integral<T2>::value>::type* = nullptr>
+constexpr
+T1
+pow_integral_type_check(const T1 base, const T2 exp_term)
+noexcept
+{
+    return pow_integral_compute(base,exp_term);
+}
+
+template<typename T1, typename T2, typename std::enable_if<!std::is_integral<T2>::value>::type* = nullptr>
+constexpr
+T1
+pow_integral_type_check(const T1 base, const T2 exp_term)
+noexcept
+{
+    // return GCLIM<return_t<T1>>::quiet_NaN();
+    return pow_integral_compute(base,static_cast<llint_t>(exp_term));
+}
+
+//
+// main function
+
+template<typename T1, typename T2>
+constexpr
+T1
+pow_integral(const T1 base, const T2 exp_term)
+noexcept
+{
+    return internal::pow_integral_type_check(base,exp_term);
+}
+
+}
+
+#endif
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
new file mode 100644
index 0000000..e609b89
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_30.hpp
@@ -0,0 +1,91 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * Gauss-Legendre quadrature: 30 points
+ */
+
+static const long double gauss_legendre_30_points[30] = \
+{
+    -0.05147184255531769583302521316672L,
+     0.05147184255531769583302521316672L,
+    -0.15386991360858354696379467274326L,
+     0.15386991360858354696379467274326L,
+    -0.25463692616788984643980512981781L,
+     0.25463692616788984643980512981781L,
+    -0.35270472553087811347103720708937L,
+     0.35270472553087811347103720708937L,
+    -0.44703376953808917678060990032285L,
+     0.44703376953808917678060990032285L,
+    -0.53662414814201989926416979331107L,
+     0.53662414814201989926416979331107L,
+    -0.62052618298924286114047755643119L,
+     0.62052618298924286114047755643119L,
+    -0.69785049479331579693229238802664L,
+     0.69785049479331579693229238802664L,
+    -0.76777743210482619491797734097450L,
+     0.76777743210482619491797734097450L,
+    -0.82956576238276839744289811973250L,
+     0.82956576238276839744289811973250L,
+    -0.88256053579205268154311646253023L,
+     0.88256053579205268154311646253023L,
+    -0.92620004742927432587932427708047L,
+     0.92620004742927432587932427708047L,
+    -0.96002186496830751221687102558180L,
+     0.96002186496830751221687102558180L,
+    -0.98366812327974720997003258160566L,
+     0.98366812327974720997003258160566L,
+    -0.99689348407464954027163005091870L,
+     0.99689348407464954027163005091870L\
+};
+
+static const long double gauss_legendre_30_weights[30] = \
+{
+    0.10285265289355884034128563670542L,
+    0.10285265289355884034128563670542L,
+    0.10176238974840550459642895216855L,
+    0.10176238974840550459642895216855L,
+    0.09959342058679526706278028210357L,
+    0.09959342058679526706278028210357L,
+    0.09636873717464425963946862635181L,
+    0.09636873717464425963946862635181L,
+    0.09212252223778612871763270708762L,
+    0.09212252223778612871763270708762L,
+    0.08689978720108297980238753071513L,
+    0.08689978720108297980238753071513L,
+    0.08075589522942021535469493846053L,
+    0.08075589522942021535469493846053L,
+    0.07375597473770520626824385002219L,
+    0.07375597473770520626824385002219L,
+    0.06597422988218049512812851511596L,
+    0.06597422988218049512812851511596L,
+    0.05749315621761906648172168940206L,
+    0.05749315621761906648172168940206L,
+    0.04840267283059405290293814042281L,
+    0.04840267283059405290293814042281L,
+    0.03879919256962704959680193644635L,
+    0.03879919256962704959680193644635L,
+    0.02878470788332336934971917961129L,
+    0.02878470788332336934971917961129L,
+    0.01846646831109095914230213191205L,
+    0.01846646831109095914230213191205L,
+    0.00796819249616660561546588347467L,
+    0.00796819249616660561546588347467L\
+};
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
new file mode 100644
index 0000000..44281f9
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/quadrature/gauss_legendre_50.hpp
@@ -0,0 +1,131 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * Gauss-Legendre quadrature: 50 points
+ */
+
+static const long double gauss_legendre_50_points[50] = \
+{
+    -0.03109833832718887611232898966595L,
+     0.03109833832718887611232898966595L,
+    -0.09317470156008614085445037763960L,
+     0.09317470156008614085445037763960L,
+    -0.15489058999814590207162862094111L,
+     0.15489058999814590207162862094111L,
+    -0.21600723687604175684728453261710L,
+     0.21600723687604175684728453261710L,
+    -0.27628819377953199032764527852113L,
+     0.27628819377953199032764527852113L,
+    -0.33550024541943735683698825729107L,
+     0.33550024541943735683698825729107L,
+    -0.39341431189756512739422925382382L,
+     0.39341431189756512739422925382382L,
+    -0.44980633497403878914713146777838L,
+     0.44980633497403878914713146777838L,
+    -0.50445814490746420165145913184914L,
+     0.50445814490746420165145913184914L,
+    -0.55715830451465005431552290962580L,
+     0.55715830451465005431552290962580L,
+    -0.60770292718495023918038179639183L,
+     0.60770292718495023918038179639183L,
+    -0.65589646568543936078162486400368L,
+     0.65589646568543936078162486400368L,
+    -0.70155246870682225108954625788366L,
+     0.70155246870682225108954625788366L,
+    -0.74449430222606853826053625268219L,
+     0.74449430222606853826053625268219L,
+    -0.78455583290039926390530519634099L,
+     0.78455583290039926390530519634099L,
+    -0.82158207085933594835625411087394L,
+     0.82158207085933594835625411087394L,
+    -0.85542976942994608461136264393476L,
+     0.85542976942994608461136264393476L,
+    -0.88596797952361304863754098246675L,
+     0.88596797952361304863754098246675L,
+    -0.91307855665579189308973564277166L,
+     0.91307855665579189308973564277166L,
+    -0.93665661894487793378087494727250L,
+     0.93665661894487793378087494727250L,
+    -0.95661095524280794299774564415662L,
+     0.95661095524280794299774564415662L,
+    -0.97286438510669207371334410460625L,
+     0.97286438510669207371334410460625L,
+    -0.98535408404800588230900962563249L,
+     0.98535408404800588230900962563249L,
+    -0.99403196943209071258510820042069L,
+     0.99403196943209071258510820042069L,
+    -0.99886640442007105018545944497422L,
+     0.99886640442007105018545944497422L\
+};
+
+static const long double gauss_legendre_50_weights[50] = \
+{
+    0.06217661665534726232103310736061L,
+    0.06217661665534726232103310736061L,
+    0.06193606742068324338408750978083L,
+    0.06193606742068324338408750978083L,
+    0.06145589959031666375640678608392L,
+    0.06145589959031666375640678608392L,
+    0.06073797084177021603175001538481L,
+    0.06073797084177021603175001538481L,
+    0.05978505870426545750957640531259L,
+    0.05978505870426545750957640531259L,
+    0.05860084981322244583512243663085L,
+    0.05860084981322244583512243663085L,
+    0.05718992564772838372302931506599L,
+    0.05718992564772838372302931506599L,
+    0.05555774480621251762356742561227L,
+    0.05555774480621251762356742561227L,
+    0.05371062188899624652345879725566L,
+    0.05371062188899624652345879725566L,
+    0.05165570306958113848990529584010L,
+    0.05165570306958113848990529584010L,
+    0.04940093844946631492124358075143L,
+    0.04940093844946631492124358075143L,
+    0.04695505130394843296563301363499L,
+    0.04695505130394843296563301363499L,
+    0.04432750433880327549202228683039L,
+    0.04432750433880327549202228683039L,
+    0.04152846309014769742241197896407L,
+    0.04152846309014769742241197896407L,
+    0.03856875661258767524477015023639L,
+    0.03856875661258767524477015023639L,
+    0.03545983561514615416073461100098L,
+    0.03545983561514615416073461100098L,
+    0.03221372822357801664816582732300L,
+    0.03221372822357801664816582732300L,
+    0.02884299358053519802990637311323L,
+    0.02884299358053519802990637311323L,
+    0.02536067357001239044019487838544L,
+    0.02536067357001239044019487838544L,
+    0.02178024317012479298159206906269L,
+    0.02178024317012479298159206906269L,
+    0.01811556071348939035125994342235L,
+    0.01811556071348939035125994342235L,
+    0.01438082276148557441937890892732L,
+    0.01438082276148557441937890892732L,
+    0.01059054838365096926356968149924L,
+    0.01059054838365096926356968149924L,
+    0.00675979919574540150277887817799L,
+    0.00675979919574540150277887817799L,
+    0.00290862255315514095840072434286L,
+    0.00290862255315514095840072434286L\
+};
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
new file mode 100644
index 0000000..43d7a5e
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/round.hpp
@@ -0,0 +1,125 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_round_HPP
+#define _gcem_round_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+round_int(const T x)
+noexcept
+{
+    return( abs(x - internal::floor_check(x)) >= T(0.5) ? \
+            // if 
+                internal::floor_check(x) + sgn(x) : \
+            // else 
+                internal::floor_check(x) );
+}
+
+template<typename T>
+constexpr
+T
+round_check_internal(const T x)
+noexcept
+{
+    return x;
+}
+
+template<>
+constexpr
+float
+round_check_internal<float>(const float x)
+noexcept
+{
+    return( abs(x) >= 8388608.f ? \
+            // if
+                x : \
+            //else
+                round_int(x) );
+}
+
+template<>
+constexpr
+double
+round_check_internal<double>(const double x)
+noexcept
+{
+    return( abs(x) >= 4503599627370496. ? \
+            // if
+                x : \
+            // else
+                round_int(x) );
+}
+
+template<>
+constexpr
+long double
+round_check_internal<long double>(const long double x)
+noexcept
+{
+    return( abs(x) >= 9223372036854775808.l ? \
+            // if
+                x : \
+            // else
+                round_int(x) );
+}
+
+template<typename T>
+constexpr
+T
+round_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // +/- infinite
+            !is_finite(x) ? \
+                x :
+            // signed-zero cases
+            GCLIM<T>::min() > abs(x) ? \
+                x :
+            // else
+                sgn(x) * round_check_internal(abs(x)) );
+}
+
+}
+
+/**
+ * Compile-time round function
+ *
+ * @param x a real-valued input.
+ * @return computes the rounding value of the input.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+round(const T x)
+noexcept
+{
+    return internal::round_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..605a35a
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sgn.hpp
@@ -0,0 +1,45 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_sgn_HPP
+#define _gcem_sgn_HPP
+
+/**
+ * Compile-time sign function
+ *
+ * @param x a real-valued input
+ * @return a value \f$ y \f$ such that \f[ y = \begin{cases} 1 \ &\text{ if } x > 0 \\ 0 \ &\text{ if } x = 0 \\ -1 \ &\text{ if } x < 0 \end{cases} \f]
+ */
+
+template<typename T>
+constexpr
+int
+sgn(const T x)
+noexcept
+{
+    return( // positive
+            x > T(0) ?  1 :
+            // negative
+            x < T(0) ? -1 :
+            // else
+                0 );
+}
+
+#endif
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
new file mode 100644
index 0000000..e207a5a
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/signbit.hpp
@@ -0,0 +1,44 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_signbit_HPP
+#define _gcem_signbit_HPP
+
+/**
+ * Compile-time sign bit detection function
+ *
+ * @param x a real-valued input
+ * @return return true if \c x is negative, otherwise return false.
+ */
+
+template <typename T>
+constexpr 
+bool
+signbit(const T x)
+noexcept
+{
+#ifdef _MSC_VER
+    return( (x == T(0)) ? (_fpclass(x) == _FPCLASS_NZ) : (x < T(0)) );
+#else
+    return GCEM_SIGNBIT(x);
+#endif
+}
+
+#endif
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
new file mode 100644
index 0000000..128cd32
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sin.hpp
@@ -0,0 +1,85 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time sine function using tan(x/2)
+ * 
+ * see eq. 5.4.8 in Numerical Recipes
+ */
+
+#ifndef _gcem_sin_HPP
+#define _gcem_sin_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+sin_compute(const T x)
+noexcept
+{
+    return T(2)*x/(T(1) + x*x);
+}
+
+template<typename T>
+constexpr
+T
+sin_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // indistinguishable from zero
+            GCLIM<T>::min() > abs(x) ? \
+                T(0) :
+            // special cases: pi/2 and pi
+            GCLIM<T>::min() > abs(x - T(GCEM_HALF_PI)) ? \
+                T(1) :
+            GCLIM<T>::min() > abs(x + T(GCEM_HALF_PI)) ? \
+                - T(1) :
+            GCLIM<T>::min() > abs(x - T(GCEM_PI)) ? \
+                T(0) :
+            GCLIM<T>::min() > abs(x + T(GCEM_PI)) ? \
+                - T(0) :
+            // else
+                sin_compute( tan(x/T(2)) ) );
+}
+
+}
+
+/**
+ * Compile-time sine function
+ *
+ * @param x a real-valued input.
+ * @return the sine function using \f[ \sin(x) = \frac{2\tan(x/2)}{1+\tan^2(x/2)} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+sin(const T x)
+noexcept
+{
+    return internal::sin_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..5355301
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sinh.hpp
@@ -0,0 +1,65 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time hyperbolic sine function
+ */
+
+#ifndef _gcem_sinh_HPP
+#define _gcem_sinh_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+sinh_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // indistinguishable from zero
+            GCLIM<T>::min() > abs(x) ? \
+                T(0) :
+            // else
+                (exp(x) - exp(-x))/T(2) );
+}
+
+}
+
+/**
+ * Compile-time hyperbolic sine function
+ *
+ * @param x a real-valued input.
+ * @return the hyperbolic sine function using \f[ \sinh(x) = \frac{\exp(x) - \exp(-x)}{2} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+sinh(const T x)
+noexcept
+{
+    return internal::sinh_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..0fd559d
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/sqrt.hpp
@@ -0,0 +1,90 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time square-root function
+ */
+
+#ifndef _gcem_sqrt_HPP
+#define _gcem_sqrt_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+sqrt_recur(const T x, const T xn, const int count)
+noexcept
+{
+    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 );
+}
+
+template<typename T>
+constexpr
+T
+sqrt_check(const T x, const T m_val)
+noexcept
+{
+    return( is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            //
+            x < T(0) ? \
+                GCLIM<T>::quiet_NaN() :
+            //
+            is_posinf(x) ? \
+                x :
+            // indistinguishable from zero or one
+            GCLIM<T>::min() > abs(x) ? \
+                T(0) :
+            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) );
+}
+
+}
+
+
+/**
+ * Compile-time square-root function
+ *
+ * @param x a real-valued input.
+ * @return Computes \f$ \sqrt{x} \f$ using a Newton-Raphson approach.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+sqrt(const T x)
+noexcept
+{
+    return internal::sqrt_check( static_cast<return_t<T>>(x), return_t<T>(1) );
+}
+
+#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
new file mode 100644
index 0000000..e53f5c8
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tan.hpp
@@ -0,0 +1,140 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time tangent function
+ */
+
+#ifndef _gcem_tan_HPP
+#define _gcem_tan_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+tan_series_exp_long(const T z)
+noexcept
+{   // this is based on a fourth-order expansion of tan(z) using Bernoulli numbers
+    return( - 1/z + (z/3 + (pow_integral(z,3)/45 + (2*pow_integral(z,5)/945 + pow_integral(z,7)/4725))) );
+}
+
+template<typename T>
+constexpr
+T
+tan_series_exp(const T x)
+noexcept
+{
+    return( GCLIM<T>::min() > abs(x - T(GCEM_HALF_PI)) ? \
+            // the value tan(pi/2) is somewhat of a convention;
+            // technically the function is not defined at EXACTLY pi/2,
+            // but this is floating point pi/2
+                T(1.633124e+16) :
+            // otherwise we use an expansion around pi/2
+                tan_series_exp_long(x - T(GCEM_HALF_PI))
+            );
+}
+
+template<typename T>
+constexpr
+T
+tan_cf_recur(const T xx, const int depth, const int max_depth)
+noexcept
+{
+    return( depth < max_depth ? \
+            // if
+                T(2*depth - 1) - xx/tan_cf_recur(xx,depth+1,max_depth) :
+            // else
+                T(2*depth - 1) );
+}
+
+template<typename T>
+constexpr
+T
+tan_cf_main(const T x)
+noexcept
+{
+    return( (x > T(1.55) && x < T(1.60)) ? \
+                tan_series_exp(x) : // deals with a singularity at tan(pi/2)
+            //
+            x > T(1.4) ? \
+                x/tan_cf_recur(x*x,1,45) :
+            x > T(1)   ? \
+                x/tan_cf_recur(x*x,1,35) :
+            // else
+                x/tan_cf_recur(x*x,1,25) );
+}
+
+template<typename T>
+constexpr
+T
+tan_begin(const T x, const int count = 0)
+noexcept
+{   // tan(x) = tan(x + pi)
+    return( x > T(GCEM_PI) ? \
+            // if
+                count > 1 ? GCLIM<T>::quiet_NaN() : // protect against undefined behavior
+                tan_begin( x - T(GCEM_PI) * internal::floor_check(x/T(GCEM_PI)), count+1 ) :
+            // else 
+                tan_cf_main(x) );
+}
+
+template<typename T>
+constexpr
+T
+tan_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // indistinguishable from zero 
+            GCLIM<T>::min() > abs(x) ? \
+                T(0) :
+            // else
+                x < T(0) ? \
+                    - tan_begin(-x) : 
+                      tan_begin( x) );
+}
+
+}
+
+/**
+ * Compile-time tangent function
+ *
+ * @param x a real-valued input.
+ * @return the tangent function using
+ * \f[ \tan(x) = \dfrac{x}{1 - \dfrac{x^2}{3 - \dfrac{x^2}{5 - \ddots}}} \f]
+ * To deal with a singularity at \f$ \pi / 2 \f$, the following expansion is employed:
+ * \f[ \tan(x) = - \frac{1}{x-\pi/2} - \sum_{k=1}^\infty \frac{(-1)^k 2^{2k} B_{2k}}{(2k)!} (x - \pi/2)^{2k - 1} \f]
+ * where \f$ B_n \f$ is the n-th Bernoulli number.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+tan(const T x)
+noexcept
+{
+    return internal::tan_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..109d751
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tanh.hpp
@@ -0,0 +1,89 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * compile-time hyperbolic tangent function
+ */
+
+#ifndef _gcem_tanh_HPP
+#define _gcem_tanh_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+tanh_cf(const T xx, const int depth)
+noexcept
+{
+    return( depth < GCEM_TANH_MAX_ITER ? \
+            // if
+                (2*depth - 1) + xx/tanh_cf(xx,depth+1) :
+            // else
+                T(2*depth - 1) );
+}
+
+template<typename T>
+constexpr
+T
+tanh_begin(const T x)
+noexcept
+{
+    return( x/tanh_cf(x*x,1) );
+}
+
+template<typename T>
+constexpr
+T
+tanh_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // indistinguishable from zero
+             GCLIM<T>::min() > abs(x) ? \
+                T(0) :
+             // else
+                x < T(0) ? \
+                    - tanh_begin(-x) : 
+                      tanh_begin( x) );
+}
+
+}
+
+/**
+ * Compile-time hyperbolic tangent function
+ *
+ * @param x a real-valued input.
+ * @return the hyperbolic tangent function using \f[ \tanh(x) = \dfrac{x}{1 + \dfrac{x^2}{3 + \dfrac{x^2}{5 + \dfrac{x^2}{7 + \ddots}}}} \f]
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+tanh(const T x)
+noexcept
+{
+    return internal::tanh_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..5a9ae97
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/tgamma.hpp
@@ -0,0 +1,80 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+/*
+ * the ('true') gamma function
+ */
+
+#ifndef _gcem_tgamma_HPP
+#define _gcem_tgamma_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+tgamma_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // indistinguishable from one or zero
+            GCLIM<T>::min() > abs(x - T(1)) ? \
+                T(1) :
+            GCLIM<T>::min() > abs(x) ? \
+                GCLIM<T>::infinity() :
+            // negative numbers
+            x < T(0) ? \
+                // check for integer
+                GCLIM<T>::min() > abs(x - find_whole(x)) ? \
+                    GCLIM<T>::quiet_NaN() :
+                // else
+                tgamma_check(x+T(1)) / x :
+
+            // else
+                exp(lgamma(x)) );
+}
+
+}
+
+/**
+ * Compile-time gamma function
+ *
+ * @param x a real-valued input.
+ * @return computes the `true' gamma function
+ * \f[ \Gamma(x) = \int_0^\infty y^{x-1} \exp(-y) dy \f]
+ * using a polynomial form:
+ * \f[ \Gamma(x+1) \approx (x+g+0.5)^{x+0.5} \exp(-x-g-0.5) \sqrt{2 \pi} \left[ c_0 + \frac{c_1}{x+1} + \frac{c_2}{x+2} + \cdots + \frac{c_n}{x+n} \right] \f]
+ * where the value \f$ g \f$ and the coefficients \f$ (c_0, c_1, \ldots, c_n) \f$
+ * are taken from Paul Godfrey, whose note can be found here: http://my.fit.edu/~gabdo/gamma.txt
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+tgamma(const T x)
+noexcept
+{
+    return internal::tgamma_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
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
new file mode 100644
index 0000000..4e19ef9
--- /dev/null
+++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/trunc.hpp
@@ -0,0 +1,121 @@
+/*################################################################################
+  ##
+  ##   Copyright (C) 2016-2022 Keith O'Hara
+  ##
+  ##   This file is part of the GCE-Math C++ library.
+  ##
+  ##   Licensed under the Apache License, Version 2.0 (the "License");
+  ##   you may not use this file except in compliance with the License.
+  ##   You may obtain a copy of the License at
+  ##
+  ##       http://www.apache.org/licenses/LICENSE-2.0
+  ##
+  ##   Unless required by applicable law or agreed to in writing, software
+  ##   distributed under the License is distributed on an "AS IS" BASIS,
+  ##   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  ##   See the License for the specific language governing permissions and
+  ##   limitations under the License.
+  ##
+  ################################################################################*/
+
+#ifndef _gcem_trunc_HPP
+#define _gcem_trunc_HPP
+
+namespace internal
+{
+
+template<typename T>
+constexpr
+T
+trunc_int(const T x)
+noexcept
+{
+    return( T(static_cast<llint_t>(x)) );
+}
+
+template<typename T>
+constexpr
+T
+trunc_check_internal(const T x)
+noexcept
+{
+    return x;
+}
+
+template<>
+constexpr
+float
+trunc_check_internal<float>(const float x)
+noexcept
+{
+    return( abs(x) >= 8388608.f ? \
+            // if
+                x : \
+            // else
+                trunc_int(x) );
+}
+
+template<>
+constexpr
+double
+trunc_check_internal<double>(const double x)
+noexcept
+{
+    return( abs(x) >= 4503599627370496. ? \
+            // if
+                x : \
+            // else
+                trunc_int(x) );
+}
+
+template<>
+constexpr
+long double
+trunc_check_internal<long double>(const long double x)
+noexcept
+{
+    return( abs(x) >= 9223372036854775808.l ? \
+            // if
+                x : \
+            // else
+                ((long double)static_cast<ullint_t>(abs(x))) * sgn(x) );
+}
+
+template<typename T>
+constexpr
+T
+trunc_check(const T x)
+noexcept
+{
+    return( // NaN check
+            is_nan(x) ? \
+                GCLIM<T>::quiet_NaN() :
+            // +/- infinite
+            !is_finite(x) ? \
+                x :
+            // signed-zero cases
+            GCLIM<T>::min() > abs(x) ? \
+                x :
+            // else
+                trunc_check_internal(x) );
+}
+
+}
+
+/**
+ * Compile-time trunc function
+ *
+ * @param x a real-valued input.
+ * @return computes the trunc-value of the input, essentially returning the integer part of the input.
+ */
+
+template<typename T>
+constexpr
+return_t<T>
+trunc(const T x)
+noexcept
+{
+    return internal::trunc_check( static_cast<return_t<T>>(x) );
+}
+
+#endif
diff --git a/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java
new file mode 100644
index 0000000..2927847
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java
@@ -0,0 +1,33 @@
+// 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 edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class ComputerVisionUtilTest {
+  @Test
+  void testObjectToRobotPose() {
+    var robot = new Pose3d(1.0, 2.0, 0.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(30.0)));
+    var cameraToObject =
+        new Transform3d(
+            new Translation3d(1.0, 1.0, 1.0),
+            new Rotation3d(0.0, Units.degreesToRadians(-20.0), Units.degreesToRadians(45.0)));
+    var robotToCamera =
+        new Transform3d(
+            new Translation3d(1.0, 0.0, 2.0),
+            new Rotation3d(0.0, 0.0, Units.degreesToRadians(25.0)));
+    Pose3d object = robot.plus(robotToCamera).plus(cameraToObject);
+
+    assertEquals(
+        robot, ComputerVisionUtil.objectToRobotPose(object, cameraToObject, robotToCamera));
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java b/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
index 66daf3d..3050867 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
@@ -10,7 +10,6 @@
 import org.ejml.simple.SimpleMatrix;
 import org.junit.jupiter.api.Test;
 
-@SuppressWarnings({"ParameterName", "LocalVariableName"})
 class DrakeTest {
   public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) {
     for (int i = 0; i < A.numRows(); i++) {
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 bb116ce..a3de9cc 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
@@ -10,7 +10,7 @@
 
 class MathUtilTest {
   @Test
-  void testApplyDeadband() {
+  void testApplyDeadbandUnityScale() {
     // < 0
     assertEquals(-1.0, MathUtil.applyDeadband(-1.0, 0.02));
     assertEquals((-0.03 + 0.02) / (1.0 - 0.02), MathUtil.applyDeadband(-0.03, 0.02));
@@ -28,6 +28,27 @@
   }
 
   @Test
+  void testApplyDeadbandArbitraryScale() {
+    // < 0
+    assertEquals(-2.5, MathUtil.applyDeadband(-2.5, 0.02, 2.5));
+    assertEquals(0.0, MathUtil.applyDeadband(-0.02, 0.02, 2.5));
+    assertEquals(0.0, MathUtil.applyDeadband(-0.01, 0.02, 2.5));
+
+    // == 0
+    assertEquals(0.0, MathUtil.applyDeadband(0.0, 0.02, 2.5));
+
+    // > 0
+    assertEquals(0.0, MathUtil.applyDeadband(0.01, 0.02, 2.5));
+    assertEquals(0.0, MathUtil.applyDeadband(0.02, 0.02, 2.5));
+    assertEquals(2.5, MathUtil.applyDeadband(2.5, 0.02, 2.5));
+  }
+
+  @Test
+  void testApplyDeadbandLargeMaxMagnitude() {
+    assertEquals(80.0, MathUtil.applyDeadband(100.0, 20, Double.POSITIVE_INFINITY));
+  }
+
+  @Test
   void testInputModulus() {
     // These tests check error wrapping. That is, the result of wrapping the
     // result of an angle reference minus the measurement.
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 8cd1e5a..054c29a 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
@@ -50,7 +50,6 @@
   }
 
   @Test
-  @SuppressWarnings("LocalVariableName")
   void testIsStabilizable() {
     Matrix<N2, N2> A;
     Matrix<N2, N1> B = VecBuilder.fill(0, 1);
@@ -77,7 +76,6 @@
   }
 
   @Test
-  @SuppressWarnings("LocalVariableName")
   void testIsDetectable() {
     Matrix<N2, N2> A;
     Matrix<N1, N2> C = Matrix.mat(Nat.N1(), Nat.N2()).fill(0, 1);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java
new file mode 100644
index 0000000..5488b68
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class VectorTest {
+  @Test
+  void testVectorDot() {
+    var vec1 = VecBuilder.fill(1.0, 2.0, 3.0);
+    var vec2 = VecBuilder.fill(4.0, 5.0, 6.0);
+
+    assertEquals(32.0, vec1.dot(vec2));
+
+    var vec3 = VecBuilder.fill(-1.0, 2.0, -3.0);
+    var vec4 = VecBuilder.fill(4.0, -5.0, 6.0);
+
+    assertEquals(-32.0, vec3.dot(vec4));
+  }
+
+  @Test
+  void testVectorNorm() {
+    assertEquals(Math.sqrt(14.0), VecBuilder.fill(1.0, 2.0, 3.0).norm());
+    assertEquals(Math.sqrt(14.0), VecBuilder.fill(1.0, -2.0, 3.0).norm());
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java
index f64608e..fcd9de1 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java
@@ -14,7 +14,6 @@
 import org.junit.jupiter.api.Test;
 
 class ControlAffinePlantInversionFeedforwardTest {
-  @SuppressWarnings("LocalVariableName")
   @Test
   void testCalculate() {
     ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
@@ -25,7 +24,6 @@
         48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);
   }
 
-  @SuppressWarnings("LocalVariableName")
   @Test
   void testCalculateState() {
     ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
@@ -36,7 +34,6 @@
         48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);
   }
 
-  @SuppressWarnings("ParameterName")
   protected Matrix<N2, N1> getDynamics(Matrix<N2, N1> x, Matrix<N1, N1> u) {
     return Matrix.mat(Nat.N2(), Nat.N2())
         .fill(1.000, 0, 0, 1.000)
@@ -44,7 +41,6 @@
         .plus(VecBuilder.fill(0, 1).times(u));
   }
 
-  @SuppressWarnings("ParameterName")
   protected Matrix<N2, N1> getStateDynamics(Matrix<N2, N1> x) {
     return Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x);
   }
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
new file mode 100644
index 0000000..b23ee1e
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
@@ -0,0 +1,303 @@
+// 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.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveAccelerationLimiterTest {
+  @Test
+  void testLowLimits() {
+    final double trackwidth = 0.9;
+    final double dt = 0.005;
+    final double maxA = 2.0;
+    final double maxAlpha = 2.0;
+
+    var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+    var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, maxA, maxAlpha);
+
+    var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+    var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+
+    // Ensure voltage exceeds acceleration before limiting
+    {
+      final var accels =
+          plant
+              .getA()
+              .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;
+      assertTrue(Math.abs(a) > maxA);
+    }
+    {
+      final var accels =
+          plant
+              .getA()
+              .times(xAccelLimiter)
+              .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0)));
+      final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
+      assertTrue(Math.abs(alpha) > maxAlpha);
+    }
+
+    // Forward
+    var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      final var voltages =
+          accelLimiter.calculate(
+              xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+      xAccelLimiter =
+          plant.calculateX(
+              xAccelLimiter,
+              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
+              dt);
+
+      final var accels =
+          plant
+              .getA()
+              .times(xAccelLimiter)
+              .plus(
+                  plant
+                      .getB()
+                      .times(
+                          new MatBuilder<>(Nat.N2(), Nat.N1())
+                              .fill(voltages.left, voltages.right)));
+      final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+      final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
+      assertTrue(Math.abs(a) <= maxA);
+      assertTrue(Math.abs(alpha) <= maxAlpha);
+    }
+
+    // Backward
+    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      final var voltages =
+          accelLimiter.calculate(
+              xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+      xAccelLimiter =
+          plant.calculateX(
+              xAccelLimiter,
+              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
+              dt);
+
+      final var accels =
+          plant
+              .getA()
+              .times(xAccelLimiter)
+              .plus(
+                  plant
+                      .getB()
+                      .times(
+                          new MatBuilder<>(Nat.N2(), Nat.N1())
+                              .fill(voltages.left, voltages.right)));
+      final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+      final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
+      assertTrue(Math.abs(a) <= maxA);
+      assertTrue(Math.abs(alpha) <= maxAlpha);
+    }
+
+    // Rotate CCW
+    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      final var voltages =
+          accelLimiter.calculate(
+              xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+      xAccelLimiter =
+          plant.calculateX(
+              xAccelLimiter,
+              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
+              dt);
+
+      final var accels =
+          plant
+              .getA()
+              .times(xAccelLimiter)
+              .plus(
+                  plant
+                      .getB()
+                      .times(
+                          new MatBuilder<>(Nat.N2(), Nat.N1())
+                              .fill(voltages.left, voltages.right)));
+      final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+      final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
+      assertTrue(Math.abs(a) <= maxA);
+      assertTrue(Math.abs(alpha) <= maxAlpha);
+    }
+  }
+
+  @Test
+  void testHighLimits() {
+    final double trackwidth = 0.9;
+    final double dt = 0.005;
+
+    var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+    // Limits are so high, they don't get hit, so states of constrained and
+    // unconstrained systems should match
+    var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, 1e3, 1e3);
+
+    var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+    var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+
+    // Forward
+    var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      final var voltages =
+          accelLimiter.calculate(
+              xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+      xAccelLimiter =
+          plant.calculateX(
+              xAccelLimiter,
+              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
+              dt);
+
+      assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5);
+      assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5);
+    }
+
+    // Backward
+    x.fill(0.0);
+    xAccelLimiter.fill(0.0);
+    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      final var voltages =
+          accelLimiter.calculate(
+              xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+      xAccelLimiter =
+          plant.calculateX(
+              xAccelLimiter,
+              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
+              dt);
+
+      assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5);
+      assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5);
+    }
+
+    // Rotate CCW
+    x.fill(0.0);
+    xAccelLimiter.fill(0.0);
+    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      final var voltages =
+          accelLimiter.calculate(
+              xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+      xAccelLimiter =
+          plant.calculateX(
+              xAccelLimiter,
+              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
+              dt);
+
+      assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5);
+      assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5);
+    }
+  }
+
+  @Test
+  void testSeperateMinMaxLowLimits() {
+    final double trackwidth = 0.9;
+    final double dt = 0.005;
+    final double minA = -1.0;
+    final double maxA = 2.0;
+    final double maxAlpha = 2.0;
+
+    var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+    var accelLimiter =
+        new DifferentialDriveAccelerationLimiter(plant, trackwidth, minA, maxA, maxAlpha);
+
+    var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+    var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+
+    // Ensure voltage exceeds acceleration before limiting
+    {
+      final var accels =
+          plant
+              .getA()
+              .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);
+    }
+
+    // a should always be within [minA, maxA]
+    // Forward
+    var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      final var voltages =
+          accelLimiter.calculate(
+              xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+      xAccelLimiter =
+          plant.calculateX(
+              xAccelLimiter,
+              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
+              dt);
+
+      final var accels =
+          plant
+              .getA()
+              .times(xAccelLimiter)
+              .plus(
+                  plant
+                      .getB()
+                      .times(
+                          new MatBuilder<>(Nat.N2(), Nat.N1())
+                              .fill(voltages.left, voltages.right)));
+      final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+      assertTrue(minA <= a && a <= maxA);
+    }
+
+    // Backward
+    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      final var voltages =
+          accelLimiter.calculate(
+              xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
+      xAccelLimiter =
+          plant.calculateX(
+              xAccelLimiter,
+              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
+              dt);
+
+      final var accels =
+          plant
+              .getA()
+              .times(xAccelLimiter)
+              .plus(
+                  plant
+                      .getB()
+                      .times(
+                          new MatBuilder<>(Nat.N2(), Nat.N1())
+                              .fill(voltages.left, voltages.right)));
+      final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
+      assertTrue(minA <= a && a <= maxA);
+    }
+  }
+
+  @Test
+  void testMinAccelGreaterThanMaxAccel() {
+    var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+    assertDoesNotThrow(() -> new DifferentialDriveAccelerationLimiter(plant, 1, -1, 1, 1e3));
+    assertThrows(
+        IllegalArgumentException.class,
+        () -> new DifferentialDriveAccelerationLimiter(plant, 1, 1, -1, 1e3));
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveFeedforwardTest.java
new file mode 100644
index 0000000..fdf53ea
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveFeedforwardTest.java
@@ -0,0 +1,85 @@
+// 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.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveFeedforwardTest {
+  private static final double kVLinear = 1.0;
+  private static final double kALinear = 1.0;
+  private static final double kVAngular = 1.0;
+  private static final double kAAngular = 1.0;
+  private static final double trackwidth = 1.0;
+  private static final double dtSeconds = 0.02;
+
+  @Test
+  void testCalculateWithTrackwidth() {
+    DifferentialDriveFeedforward differentialDriveFeedforward =
+        new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
+    LinearSystem<N2, N2, N2> plant =
+        LinearSystemId.identifyDrivetrainSystem(
+            kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
+    for (int currentLeftVelocity = -4; currentLeftVelocity <= 4; currentLeftVelocity += 2) {
+      for (int currentRightVelocity = -4; currentRightVelocity <= 4; currentRightVelocity += 2) {
+        for (int nextLeftVelocity = -4; nextLeftVelocity <= 4; nextLeftVelocity += 2) {
+          for (int nextRightVelocity = -4; nextRightVelocity <= 4; nextRightVelocity += 2) {
+            DifferentialDriveWheelVoltages u =
+                differentialDriveFeedforward.calculate(
+                    currentLeftVelocity,
+                    nextLeftVelocity,
+                    currentRightVelocity,
+                    nextRightVelocity,
+                    dtSeconds);
+            Matrix<N2, N1> nextX =
+                plant.calculateX(
+                    VecBuilder.fill(currentLeftVelocity, currentRightVelocity),
+                    VecBuilder.fill(u.left, u.right),
+                    dtSeconds);
+            assertEquals(nextX.get(0, 0), nextLeftVelocity, 1e-6);
+            assertEquals(nextX.get(1, 0), nextRightVelocity, 1e-6);
+          }
+        }
+      }
+    }
+  }
+
+  @Test
+  void testCalculateWithoutTrackwidth() {
+    DifferentialDriveFeedforward differentialDriveFeedforward =
+        new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular);
+    LinearSystem<N2, N2, N2> plant =
+        LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular);
+    for (int currentLeftVelocity = -4; currentLeftVelocity <= 4; currentLeftVelocity += 2) {
+      for (int currentRightVelocity = -4; currentRightVelocity <= 4; currentRightVelocity += 2) {
+        for (int nextLeftVelocity = -4; nextLeftVelocity <= 4; nextLeftVelocity += 2) {
+          for (int nextRightVelocity = -4; nextRightVelocity <= 4; nextRightVelocity += 2) {
+            DifferentialDriveWheelVoltages u =
+                differentialDriveFeedforward.calculate(
+                    currentLeftVelocity,
+                    nextLeftVelocity,
+                    currentRightVelocity,
+                    nextRightVelocity,
+                    dtSeconds);
+            Matrix<N2, N1> nextX =
+                plant.calculateX(
+                    VecBuilder.fill(currentLeftVelocity, currentRightVelocity),
+                    VecBuilder.fill(u.left, u.right),
+                    dtSeconds);
+            assertEquals(nextX.get(0, 0), nextLeftVelocity, 1e-6);
+            assertEquals(nextX.get(1, 0), nextRightVelocity, 1e-6);
+          }
+        }
+      }
+    }
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java
new file mode 100644
index 0000000..674cba3
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java
@@ -0,0 +1,107 @@
+// 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 static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import org.junit.jupiter.api.Test;
+
+class ImplicitModelFollowerTest {
+  @Test
+  void testSameModel() {
+    final double dt = 0.005;
+
+    var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+    var imf = new ImplicitModelFollower<N2, N2, N2>(plant, plant);
+
+    var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+    var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+
+    // Forward
+    var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertEquals(x.get(0, 0), xImf.get(0, 0));
+      assertEquals(x.get(1, 0), xImf.get(1, 0));
+    }
+
+    // Backward
+    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertEquals(x.get(0, 0), xImf.get(0, 0));
+      assertEquals(x.get(1, 0), xImf.get(1, 0));
+    }
+
+    // Rotate CCW
+    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertEquals(x.get(0, 0), xImf.get(0, 0));
+      assertEquals(x.get(1, 0), xImf.get(1, 0));
+    }
+  }
+
+  @Test
+  void testSlowerRefModel() {
+    final double dt = 0.005;
+
+    var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
+
+    // Linear acceleration is slower, but angular acceleration is the same
+    var plantRef = LinearSystemId.identifyDrivetrainSystem(1.0, 2.0, 1.0, 1.0);
+
+    var imf = new ImplicitModelFollower<N2, N2, N2>(plant, plantRef);
+
+    var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+    var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
+
+    // Forward
+    var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertTrue(x.get(0, 0) >= xImf.get(0, 0));
+      assertTrue(x.get(1, 0) >= xImf.get(1, 0));
+    }
+
+    // Backward
+    x.fill(0.0);
+    xImf.fill(0.0);
+    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertTrue(x.get(0, 0) <= xImf.get(0, 0));
+      assertTrue(x.get(1, 0) <= xImf.get(1, 0));
+    }
+
+    // Rotate CCW
+    x.fill(0.0);
+    xImf.fill(0.0);
+    u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
+    for (double t = 0.0; t < 3.0; t += dt) {
+      x = plant.calculateX(x, u, dt);
+      xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
+
+      assertEquals(x.get(0, 0), xImf.get(0, 0), 1e-5);
+      assertEquals(x.get(1, 0), xImf.get(1, 0), 1e-5);
+    }
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java
new file mode 100644
index 0000000..efb9c5e
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java
@@ -0,0 +1,128 @@
+// 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.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MatBuilder;
+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.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.ArrayList;
+import org.junit.jupiter.api.Test;
+
+class LTVDifferentialDriveControllerTest {
+  private static final double kTolerance = 1 / 12.0;
+  private static final double kAngularTolerance = Math.toRadians(2);
+
+  /** States of the drivetrain system. */
+  static class State {
+    /// X position in global coordinate frame.
+    public static final int kX = 0;
+
+    /// Y position in global coordinate frame.
+    public static final int kY = 1;
+
+    /// Heading in global coordinate frame.
+    public static final int kHeading = 2;
+
+    /// Left encoder velocity.
+    public static final int kLeftVelocity = 3;
+
+    /// Right encoder velocity.
+    public static final int kRightVelocity = 4;
+  }
+
+  private static final double kLinearV = 3.02; // V/(m/s)
+  private static final double kLinearA = 0.642; // V/(m/s²)
+  private static final double kAngularV = 1.382; // V/(m/s)
+  private static final double kAngularA = 0.08495; // V/(m/s²)
+  private static final LinearSystem<N2, N2, N2> plant =
+      LinearSystemId.identifyDrivetrainSystem(kLinearV, kLinearA, kAngularV, kAngularA);
+  private static final double kTrackwidth = 0.9;
+
+  private static Matrix<N5, N1> dynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+    double v = (x.get(State.kLeftVelocity, 0) + x.get(State.kRightVelocity, 0)) / 2.0;
+
+    var xdot = new Matrix<>(Nat.N5(), Nat.N1());
+    xdot.set(0, 0, v * Math.cos(x.get(State.kHeading, 0)));
+    xdot.set(1, 0, v * Math.sin(x.get(State.kHeading, 0)));
+    xdot.set(2, 0, (x.get(State.kRightVelocity, 0) - x.get(State.kLeftVelocity, 0)) / kTrackwidth);
+    xdot.assignBlock(
+        3, 0, plant.getA().times(x.block(Nat.N2(), Nat.N1(), 3, 0)).plus(plant.getB().times(u)));
+    return xdot;
+  }
+
+  @Test
+  void testReachesReference() {
+    final double kDt = 0.02;
+
+    final var controller =
+        new LTVDifferentialDriveController(
+            plant,
+            kTrackwidth,
+            VecBuilder.fill(0.0625, 0.125, 2.5, 0.95, 0.95),
+            VecBuilder.fill(12.0, 12.0),
+            kDt);
+    var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
+
+    final var waypoints = new ArrayList<Pose2d>();
+    waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
+    waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
+    var config = new TrajectoryConfig(8.8, 0.1);
+    final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+    var x =
+        new MatBuilder<>(Nat.N5(), Nat.N1())
+            .fill(
+                robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), 0.0, 0.0);
+
+    final var totalTime = trajectory.getTotalTimeSeconds();
+    for (int i = 0; i < (totalTime / kDt); ++i) {
+      var state = trajectory.sample(kDt * i);
+      robotPose =
+          new Pose2d(
+              x.get(State.kX, 0), x.get(State.kY, 0), new Rotation2d(x.get(State.kHeading, 0)));
+      final var output =
+          controller.calculate(
+              robotPose, x.get(State.kLeftVelocity, 0), x.get(State.kRightVelocity, 0), state);
+
+      x =
+          NumericalIntegration.rkdp(
+              LTVDifferentialDriveControllerTest::dynamics,
+              x,
+              new MatBuilder<>(Nat.N2(), Nat.N1()).fill(output.left, output.right),
+              kDt);
+    }
+
+    final var states = trajectory.getStates();
+    final var endPose = states.get(states.size() - 1).poseMeters;
+
+    // Java lambdas require local variables referenced from a lambda expression
+    // must be final or effectively final.
+    final var finalRobotPose = robotPose;
+    assertAll(
+        () -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
+        () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
+        () ->
+            assertEquals(
+                0.0,
+                MathUtil.angleModulus(
+                    endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()),
+                kAngularTolerance));
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java
new file mode 100644
index 0000000..463e1ce
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java
@@ -0,0 +1,65 @@
+// 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.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MathUtil;
+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.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.ArrayList;
+import org.junit.jupiter.api.Test;
+
+class LTVUnicycleControllerTest {
+  private static final double kTolerance = 1 / 12.0;
+  private static final double kAngularTolerance = Math.toRadians(2);
+
+  @Test
+  void testReachesReference() {
+    final double kDt = 0.02;
+
+    final var controller =
+        new LTVUnicycleController(
+            VecBuilder.fill(0.0625, 0.125, 2.5), VecBuilder.fill(4.0, 4.0), kDt);
+    var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
+
+    final var waypoints = new ArrayList<Pose2d>();
+    waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
+    waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
+    var config = new TrajectoryConfig(8.8, 0.1);
+    final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+    final var totalTime = trajectory.getTotalTimeSeconds();
+    for (int i = 0; i < (totalTime / kDt); ++i) {
+      var state = trajectory.sample(kDt * i);
+
+      var output = controller.calculate(robotPose, state);
+      robotPose =
+          robotPose.exp(
+              new Twist2d(output.vxMetersPerSecond * kDt, 0, output.omegaRadiansPerSecond * kDt));
+    }
+
+    final var states = trajectory.getStates();
+    final var endPose = states.get(states.size() - 1).poseMeters;
+
+    // Java lambdas require local variables referenced from a lambda expression
+    // must be final or effectively final.
+    final var finalRobotPose = robotPose;
+    assertAll(
+        () -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
+        () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
+        () ->
+            assertEquals(
+                0.0,
+                MathUtil.angleModulus(
+                    endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()),
+                kAngularTolerance));
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java
index 98b0e6c..1aba6d7 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java
@@ -14,7 +14,6 @@
 import org.junit.jupiter.api.Test;
 
 class LinearPlantInversionFeedforwardTest {
-  @SuppressWarnings("LocalVariableName")
   @Test
   void testCalculate() {
     Matrix<N2, N2> A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
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 93b89c8..31213f6 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
@@ -17,7 +17,6 @@
 
 class LinearQuadraticRegulatorTest {
   @Test
-  @SuppressWarnings("LocalVariableName")
   void testLQROnElevator() {
     var motors = DCMotor.getVex775Pro(2);
 
@@ -38,7 +37,6 @@
   }
 
   @Test
-  @SuppressWarnings("LocalVariableName")
   void testFourMotorElevator() {
     var dt = 0.020;
 
@@ -55,7 +53,6 @@
   }
 
   @Test
-  @SuppressWarnings("LocalVariableName")
   void testLQROnArm() {
     var motors = DCMotor.getVex775Pro(2);
 
@@ -89,7 +86,6 @@
    * @param Aref Desired state matrix.
    * @param dtSeconds Discretization timestep in seconds.
    */
-  @SuppressWarnings({"LocalVariableName", "MethodTypeParameterName", "ParameterName"})
   <States extends Num, Inputs extends Num> Matrix<Inputs, States> getImplicitModelFollowingK(
       Matrix<States, States> A,
       Matrix<States, Inputs> B,
@@ -114,7 +110,6 @@
   }
 
   @Test
-  @SuppressWarnings("LocalVariableName")
   void testMatrixOverloadsWithSingleIntegrator() {
     var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0);
     var B = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
@@ -138,7 +133,6 @@
   }
 
   @Test
-  @SuppressWarnings("LocalVariableName")
   void testMatrixOverloadsWithDoubleIntegrator() {
     double Kv = 3.02;
     double Ka = 0.642;
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 5fa4939..7b8484d 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
@@ -40,7 +40,6 @@
   private final LinearSystemLoop<N2, N1, N1> m_loop =
       new LinearSystemLoop<>(m_plant, m_controller, m_observer, 12, 0.00505);
 
-  @SuppressWarnings("LocalVariableName")
   private static void updateTwoState(
       LinearSystem<N2, N1, N1> plant, LinearSystemLoop<N2, N1, N1> loop, double noise) {
     Matrix<N1, N1> y = plant.calculateY(loop.getXHat(), loop.getU()).plus(VecBuilder.fill(noise));
@@ -50,7 +49,6 @@
   }
 
   @Test
-  @SuppressWarnings("LocalVariableName")
   void testStateSpaceEnabled() {
     m_loop.reset(VecBuilder.fill(0, 0));
     Matrix<N2, N1> references = VecBuilder.fill(2.0, 0.0);
@@ -79,7 +77,6 @@
   }
 
   @Test
-  @SuppressWarnings("LocalVariableName")
   void testFlywheelEnabled() {
     LinearSystem<N1, N1, N1> plant =
         LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00289, 1.0);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
index 83fced2..fd4e428 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
@@ -13,7 +13,6 @@
 import org.junit.jupiter.api.Test;
 
 class SimpleMotorFeedforwardTest {
-  @SuppressWarnings("LocalVariableName")
   @Test
   void testCalculate() {
     double Ks = 0.5;
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 78908e4..c3906d4 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
@@ -6,34 +6,37 @@
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
-import edu.wpi.first.math.MatBuilder;
-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.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import java.util.List;
 import java.util.Random;
+import java.util.TreeMap;
+import java.util.function.Function;
 import org.junit.jupiter.api.Test;
 
 class DifferentialDrivePoseEstimatorTest {
-  @SuppressWarnings("LocalVariableName")
   @Test
   void testAccuracy() {
+    var kinematics = new DifferentialDriveKinematics(1);
+
     var estimator =
         new DifferentialDrivePoseEstimator(
+            kinematics,
             new Rotation2d(),
+            0,
+            0,
             new Pose2d(),
-            new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02),
-            new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.01, 0.01, 0.001),
-            new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1, 0.1, 0.01));
-
-    var traj =
+            VecBuilder.fill(0.02, 0.02, 0.01),
+            VecBuilder.fill(0.1, 0.1, 0.1));
+    var trajectory =
         TrajectoryGenerator.generateTrajectory(
             List.of(
                 new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
@@ -41,67 +44,165 @@
                 new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
                 new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
                 new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
-            new TrajectoryConfig(10, 5));
+            new TrajectoryConfig(2, 2));
 
+    testFollowTrajectory(
+        kinematics,
+        estimator,
+        trajectory,
+        state ->
+            new ChassisSpeeds(
+                state.velocityMetersPerSecond,
+                0,
+                state.velocityMetersPerSecond * state.curvatureRadPerMeter),
+        state -> state.poseMeters,
+        trajectory.getInitialPose(),
+        new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+        0.02,
+        0.1,
+        0.25,
+        true);
+  }
+
+  @Test
+  void testBadInitialPose() {
     var kinematics = new DifferentialDriveKinematics(1);
-    var rand = new Random(4915);
 
-    final double dt = 0.02;
+    var estimator =
+        new DifferentialDrivePoseEstimator(
+            kinematics,
+            new Rotation2d(),
+            0,
+            0,
+            new Pose2d(),
+            VecBuilder.fill(0.02, 0.02, 0.01),
+            VecBuilder.fill(0.1, 0.1, 0.1));
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(2, 2));
+
+    for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) {
+      for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) {
+        var pose_offset = Rotation2d.fromDegrees(offset_direction_degs);
+        var heading_offset = Rotation2d.fromDegrees(offset_heading_degs);
+
+        var initial_pose =
+            trajectory
+                .getInitialPose()
+                .plus(
+                    new Transform2d(
+                        new Translation2d(pose_offset.getCos(), pose_offset.getSin()),
+                        heading_offset));
+
+        testFollowTrajectory(
+            kinematics,
+            estimator,
+            trajectory,
+            state ->
+                new ChassisSpeeds(
+                    state.velocityMetersPerSecond,
+                    0,
+                    state.velocityMetersPerSecond * state.curvatureRadPerMeter),
+            state -> state.poseMeters,
+            initial_pose,
+            new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+            0.02,
+            0.1,
+            0.25,
+            false);
+      }
+    }
+  }
+
+  void testFollowTrajectory(
+      final DifferentialDriveKinematics kinematics,
+      final DifferentialDrivePoseEstimator estimator,
+      final Trajectory trajectory,
+      final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
+      final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
+      final Pose2d startingPose,
+      final Pose2d endingPose,
+      final double dt,
+      final double visionUpdateRate,
+      final double visionUpdateDelay,
+      final boolean checkError) {
+    double leftDistanceMeters = 0;
+    double rightDistanceMeters = 0;
+
+    estimator.resetPosition(
+        new Rotation2d(), leftDistanceMeters, rightDistanceMeters, startingPose);
+
+    var rand = new Random(3538);
+
     double t = 0.0;
 
-    final double visionUpdateRate = 0.1;
-    Pose2d lastVisionPose = null;
-    double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+    System.out.print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
 
-    double distanceLeft = 0.0;
-    double distanceRight = 0.0;
+    final TreeMap<Double, Pose2d> visionUpdateQueue = new TreeMap<>();
 
     double maxError = Double.NEGATIVE_INFINITY;
     double errorSum = 0;
-    Trajectory.State groundtruthState;
-    DifferentialDriveWheelSpeeds input;
-    while (t <= traj.getTotalTimeSeconds()) {
-      groundtruthState = traj.sample(t);
-      input =
-          kinematics.toWheelSpeeds(
-              new ChassisSpeeds(
-                  groundtruthState.velocityMetersPerSecond,
-                  0.0,
-                  // ds/dt * dtheta/ds = dtheta/dt
-                  groundtruthState.velocityMetersPerSecond
-                      * groundtruthState.curvatureRadPerMeter));
+    while (t <= trajectory.getTotalTimeSeconds()) {
+      var groundTruthState = trajectory.sample(t);
 
-      if (lastVisionUpdateTime + visionUpdateRate + rand.nextGaussian() * 0.4 < t) {
-        if (lastVisionPose != null) {
-          estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
-        }
-        var groundPose = groundtruthState.poseMeters;
-        lastVisionPose =
-            new Pose2d(
-                new Translation2d(
-                    groundPose.getTranslation().getX() + rand.nextGaussian() * 0.1,
-                    groundPose.getTranslation().getY() + rand.nextGaussian() * 0.1),
-                new Rotation2d(rand.nextGaussian() * 0.01).plus(groundPose.getRotation()));
-        lastVisionUpdateTime = t;
+      // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
+      // last vision measurement
+      if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) {
+        Pose2d newVisionPose =
+            visionMeasurementGenerator
+                .apply(groundTruthState)
+                .plus(
+                    new Transform2d(
+                        new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1),
+                        new Rotation2d(rand.nextGaussian() * 0.05)));
+
+        visionUpdateQueue.put(t, newVisionPose);
       }
 
-      input.leftMetersPerSecond += rand.nextGaussian() * 0.01;
-      input.rightMetersPerSecond += rand.nextGaussian() * 0.01;
+      // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds
+      // since it was measured
+      if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) {
+        var visionEntry = visionUpdateQueue.pollFirstEntry();
+        estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey());
+      }
 
-      distanceLeft += input.leftMetersPerSecond * dt;
-      distanceRight += input.rightMetersPerSecond * dt;
+      var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
 
-      var rotNoise = new Rotation2d(rand.nextGaussian() * 0.001);
+      var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+      leftDistanceMeters += wheelSpeeds.leftMetersPerSecond * dt;
+      rightDistanceMeters += wheelSpeeds.rightMetersPerSecond * dt;
+
       var xHat =
           estimator.updateWithTime(
               t,
-              groundtruthState.poseMeters.getRotation().plus(rotNoise),
-              input,
-              distanceLeft,
-              distanceRight);
+              groundTruthState
+                  .poseMeters
+                  .getRotation()
+                  .plus(new Rotation2d(rand.nextGaussian() * 0.05))
+                  .minus(trajectory.getInitialPose().getRotation()),
+              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());
+          groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
       if (error > maxError) {
         maxError = error;
       }
@@ -110,7 +211,20 @@
       t += dt;
     }
 
-    assertEquals(0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.035, "Incorrect mean error");
-    assertEquals(0.0, maxError, 0.055, "Incorrect max error");
+    assertEquals(
+        endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X");
+    assertEquals(
+        endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y");
+    assertEquals(
+        endingPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().getRadians(),
+        0.15,
+        "Incorrect Final Theta");
+
+    if (checkError) {
+      assertEquals(
+          0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error");
+      assertEquals(0.0, maxError, 0.2, "Incorrect max error");
+    }
   }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
index 5b07ebc..112ef90 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
@@ -26,7 +26,6 @@
 import java.util.List;
 import org.junit.jupiter.api.Test;
 
-@SuppressWarnings("ParameterName")
 class ExtendedKalmanFilterTest {
   private static Matrix<N5, N1> getDynamics(final Matrix<N5, N1> x, final Matrix<N2, N1> u) {
     final var motors = DCMotor.getCIM(2);
@@ -68,7 +67,6 @@
     return VecBuilder.fill(x.get(0, 0), x.get(1, 0), x.get(2, 0), x.get(3, 0), x.get(4, 0));
   }
 
-  @SuppressWarnings("LocalVariableName")
   @Test
   void testInit() {
     double dtSeconds = 0.00505;
@@ -99,7 +97,6 @@
         });
   }
 
-  @SuppressWarnings("LocalVariableName")
   @Test
   void testConvergence() {
     double dtSeconds = 0.00505;
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
index 4d709be..848fe93 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
@@ -34,7 +34,6 @@
     createElevator();
   }
 
-  @SuppressWarnings("LocalVariableName")
   private static void createElevator() {
     var motors = DCMotor.getVex775Pro(2);
 
@@ -61,7 +60,6 @@
           new Matrix<>(Nat.N3(), Nat.N3())); // D
 
   @Test
-  @SuppressWarnings("LocalVariableName")
   void testElevatorKalmanFilter() {
     var Q = VecBuilder.fill(0.05, 1.0);
     var R = VecBuilder.fill(0.0001);
@@ -136,7 +134,6 @@
   }
 
   @Test
-  @SuppressWarnings("LocalVariableName")
   void testSwerveKFMovingOverTrajectory() {
     var random = new Random();
 
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 d8756f0..02d2d52 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
@@ -9,87 +9,186 @@
 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.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
+import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import java.util.List;
 import java.util.Random;
+import java.util.TreeMap;
+import java.util.function.Function;
 import org.junit.jupiter.api.Test;
 
 class MecanumDrivePoseEstimatorTest {
   @Test
-  @SuppressWarnings("LocalVariableName")
-  void testAccuracy() {
+  void testAccuracyFacingTrajectory() {
     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(
-            new Rotation2d(),
-            new Pose2d(),
             kinematics,
+            new Rotation2d(),
+            wheelPositions,
+            new Pose2d(),
             VecBuilder.fill(0.1, 0.1, 0.1),
-            VecBuilder.fill(0.05),
-            VecBuilder.fill(0.1, 0.1, 0.1));
+            VecBuilder.fill(0.45, 0.45, 0.1));
 
     var trajectory =
         TrajectoryGenerator.generateTrajectory(
             List.of(
-                new Pose2d(),
-                new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
-                new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
-                new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
-                new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
-                new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
-            new TrajectoryConfig(0.5, 2));
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(2, 2));
 
-    var rand = new Random(5190);
+    testFollowTrajectory(
+        kinematics,
+        estimator,
+        trajectory,
+        state ->
+            new ChassisSpeeds(
+                state.velocityMetersPerSecond,
+                0,
+                state.velocityMetersPerSecond * state.curvatureRadPerMeter),
+        state -> state.poseMeters,
+        trajectory.getInitialPose(),
+        new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+        0.02,
+        0.1,
+        0.25,
+        true);
+  }
 
-    final double dt = 0.02;
+  @Test
+  void testBadInitialPose() {
+    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(),
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.45, 0.45, 0.1));
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(2, 2));
+
+    for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) {
+      for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) {
+        var pose_offset = Rotation2d.fromDegrees(offset_direction_degs);
+        var heading_offset = Rotation2d.fromDegrees(offset_heading_degs);
+
+        var initial_pose =
+            trajectory
+                .getInitialPose()
+                .plus(
+                    new Transform2d(
+                        new Translation2d(pose_offset.getCos(), pose_offset.getSin()),
+                        heading_offset));
+
+        testFollowTrajectory(
+            kinematics,
+            estimator,
+            trajectory,
+            state ->
+                new ChassisSpeeds(
+                    state.velocityMetersPerSecond,
+                    0,
+                    state.velocityMetersPerSecond * state.curvatureRadPerMeter),
+            state -> state.poseMeters,
+            initial_pose,
+            new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+            0.02,
+            0.1,
+            0.25,
+            false);
+      }
+    }
+  }
+
+  void testFollowTrajectory(
+      final MecanumDriveKinematics kinematics,
+      final MecanumDrivePoseEstimator estimator,
+      final Trajectory trajectory,
+      final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
+      final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
+      final Pose2d startingPose,
+      final Pose2d endingPose,
+      final double dt,
+      final double visionUpdateRate,
+      final double visionUpdateDelay,
+      final boolean checkError) {
+    var wheelPositions = new MecanumDriveWheelPositions();
+
+    estimator.resetPosition(new Rotation2d(), wheelPositions, startingPose);
+
+    var rand = new Random(3538);
+
     double t = 0.0;
 
-    final double visionUpdateRate = 0.1;
-    Pose2d lastVisionPose = null;
-    double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+    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;
     double errorSum = 0;
     while (t <= trajectory.getTotalTimeSeconds()) {
       var groundTruthState = trajectory.sample(t);
 
-      if (lastVisionUpdateTime + visionUpdateRate < t) {
-        if (lastVisionPose != null) {
-          estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
-        }
+      // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
+      // last vision measurement
+      if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) {
+        Pose2d newVisionPose =
+            visionMeasurementGenerator
+                .apply(groundTruthState)
+                .plus(
+                    new Transform2d(
+                        new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1),
+                        new Rotation2d(rand.nextGaussian() * 0.05)));
 
-        lastVisionPose =
-            new Pose2d(
-                new Translation2d(
-                    groundTruthState.poseMeters.getTranslation().getX() + rand.nextGaussian() * 0.1,
-                    groundTruthState.poseMeters.getTranslation().getY()
-                        + rand.nextGaussian() * 0.1),
-                new Rotation2d(rand.nextGaussian() * 0.1)
-                    .plus(groundTruthState.poseMeters.getRotation()));
-
-        lastVisionUpdateTime = t;
+        visionUpdateQueue.put(t, newVisionPose);
       }
 
-      var wheelSpeeds =
-          kinematics.toWheelSpeeds(
-              new ChassisSpeeds(
-                  groundTruthState.velocityMetersPerSecond,
-                  0,
-                  groundTruthState.velocityMetersPerSecond
-                      * groundTruthState.curvatureRadPerMeter));
+      // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds
+      // since it was measured
+      if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) {
+        var visionEntry = visionUpdateQueue.pollFirstEntry();
+        estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey());
+      }
 
-      wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
-      wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
-      wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
-      wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
+      var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
+
+      var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+      wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
+      wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
+      wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
+      wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
 
       var xHat =
           estimator.updateWithTime(
@@ -97,8 +196,19 @@
               groundTruthState
                   .poseMeters
                   .getRotation()
-                  .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
-              wheelSpeeds);
+                  .plus(new Rotation2d(rand.nextGaussian() * 0.05))
+                  .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());
@@ -111,7 +221,19 @@
     }
 
     assertEquals(
-        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, "Incorrect mean error");
-    assertEquals(0.0, maxError, 0.42, "Incorrect max error");
+        endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X");
+    assertEquals(
+        endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y");
+    assertEquals(
+        endingPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().getRadians(),
+        0.15,
+        "Incorrect Final Theta");
+
+    if (checkError) {
+      assertEquals(
+          0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error");
+      assertEquals(0.0, maxError, 0.2, "Incorrect max error");
+    }
   }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
index 3c51d6c..2131c35 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
@@ -16,7 +16,7 @@
   void testZeroMeanPoints() {
     var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
     var points =
-        merweScaledSigmaPoints.sigmaPoints(
+        merweScaledSigmaPoints.squareRootSigmaPoints(
             VecBuilder.fill(0, 0), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1));
 
     assertTrue(
@@ -31,8 +31,8 @@
   void testNonzeroMeanPoints() {
     var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
     var points =
-        merweScaledSigmaPoints.sigmaPoints(
-            VecBuilder.fill(1, 2), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 10));
+        merweScaledSigmaPoints.squareRootSigmaPoints(
+            VecBuilder.fill(1, 2), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, Math.sqrt(10)));
 
     assertTrue(
         points.isEqual(
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 c6126c6..fde2c39 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
@@ -9,33 +9,43 @@
 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.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import java.util.List;
 import java.util.Random;
+import java.util.TreeMap;
+import java.util.function.Function;
 import org.junit.jupiter.api.Test;
 
 class SwerveDrivePoseEstimatorTest {
   @Test
-  @SuppressWarnings("LocalVariableName")
-  void testAccuracy() {
+  void testAccuracyFacingTrajectory() {
     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(
-            new Rotation2d(),
-            new Pose2d(),
             kinematics,
+            new Rotation2d(),
+            new SwerveModulePosition[] {fl, fr, bl, br},
+            new Pose2d(),
             VecBuilder.fill(0.1, 0.1, 0.1),
-            VecBuilder.fill(0.005),
-            VecBuilder.fill(0.1, 0.1, 0.1));
+            VecBuilder.fill(0.5, 0.5, 0.5));
 
     var trajectory =
         TrajectoryGenerator.generateTrajectory(
@@ -45,49 +55,158 @@
                 new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
                 new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
                 new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
-            new TrajectoryConfig(0.5, 2));
+            new TrajectoryConfig(2, 2));
 
-    var rand = new Random(4915);
+    testFollowTrajectory(
+        kinematics,
+        estimator,
+        trajectory,
+        state ->
+            new ChassisSpeeds(
+                state.velocityMetersPerSecond,
+                0,
+                state.velocityMetersPerSecond * state.curvatureRadPerMeter),
+        state -> state.poseMeters,
+        trajectory.getInitialPose(),
+        new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+        0.02,
+        0.1,
+        0.25,
+        true);
+  }
 
-    final double dt = 0.02;
+  @Test
+  void testBadInitialPose() {
+    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, -1, Rotation2d.fromRadians(-1)),
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.9, 0.9, 0.9));
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(2, 2));
+
+    for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) {
+      for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) {
+        var pose_offset = Rotation2d.fromDegrees(offset_direction_degs);
+        var heading_offset = Rotation2d.fromDegrees(offset_heading_degs);
+
+        var initial_pose =
+            trajectory
+                .getInitialPose()
+                .plus(
+                    new Transform2d(
+                        new Translation2d(pose_offset.getCos(), pose_offset.getSin()),
+                        heading_offset));
+
+        testFollowTrajectory(
+            kinematics,
+            estimator,
+            trajectory,
+            state ->
+                new ChassisSpeeds(
+                    state.velocityMetersPerSecond,
+                    0,
+                    state.velocityMetersPerSecond * state.curvatureRadPerMeter),
+            state -> state.poseMeters,
+            initial_pose,
+            new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+            0.02,
+            0.1,
+            1.0,
+            false);
+      }
+    }
+  }
+
+  void testFollowTrajectory(
+      final SwerveDriveKinematics kinematics,
+      final SwerveDrivePoseEstimator estimator,
+      final Trajectory trajectory,
+      final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
+      final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
+      final Pose2d startingPose,
+      final Pose2d endingPose,
+      final double dt,
+      final double visionUpdateRate,
+      final double visionUpdateDelay,
+      final boolean checkError) {
+    SwerveModulePosition[] positions = {
+      new SwerveModulePosition(),
+      new SwerveModulePosition(),
+      new SwerveModulePosition(),
+      new SwerveModulePosition()
+    };
+
+    estimator.resetPosition(new Rotation2d(), positions, startingPose);
+
+    var rand = new Random(3538);
+
     double t = 0.0;
 
-    final double visionUpdateRate = 0.1;
-    Pose2d lastVisionPose = null;
-    double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+    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;
     double errorSum = 0;
     while (t <= trajectory.getTotalTimeSeconds()) {
       var groundTruthState = trajectory.sample(t);
 
-      if (lastVisionUpdateTime + visionUpdateRate < t) {
-        if (lastVisionPose != null) {
-          estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
-        }
+      // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
+      // last vision measurement
+      if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) {
+        Pose2d newVisionPose =
+            visionMeasurementGenerator
+                .apply(groundTruthState)
+                .plus(
+                    new Transform2d(
+                        new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1),
+                        new Rotation2d(rand.nextGaussian() * 0.05)));
 
-        lastVisionPose =
-            new Pose2d(
-                new Translation2d(
-                    groundTruthState.poseMeters.getTranslation().getX() + rand.nextGaussian() * 0.1,
-                    groundTruthState.poseMeters.getTranslation().getY()
-                        + rand.nextGaussian() * 0.1),
-                new Rotation2d(rand.nextGaussian() * 0.1)
-                    .plus(groundTruthState.poseMeters.getRotation()));
-
-        lastVisionUpdateTime = t;
+        visionUpdateQueue.put(t, newVisionPose);
       }
 
-      var moduleStates =
-          kinematics.toSwerveModuleStates(
-              new ChassisSpeeds(
-                  groundTruthState.velocityMetersPerSecond,
-                  0.0,
-                  groundTruthState.velocityMetersPerSecond
-                      * groundTruthState.curvatureRadPerMeter));
-      for (var moduleState : moduleStates) {
-        moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
-        moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1;
+      // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds
+      // since it was measured
+      if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) {
+        var visionEntry = visionUpdateQueue.pollFirstEntry();
+        estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey());
+      }
+
+      var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
+
+      var moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds);
+
+      for (int i = 0; i < moduleStates.length; i++) {
+        positions[i].distanceMeters +=
+            moduleStates[i].speedMetersPerSecond * (1 - rand.nextGaussian() * 0.05) * dt;
+        positions[i].angle =
+            moduleStates[i].angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
       }
 
       var xHat =
@@ -96,8 +215,27 @@
               groundTruthState
                   .poseMeters
                   .getRotation()
-                  .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
-              moduleStates);
+                  .plus(new Rotation2d(rand.nextGaussian() * 0.05))
+                  .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());
@@ -110,7 +248,19 @@
     }
 
     assertEquals(
-        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, "Incorrect mean error");
-    assertEquals(0.0, maxError, 0.42, "Incorrect max error");
+        endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X");
+    assertEquals(
+        endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y");
+    assertEquals(
+        endingPose.getRotation().getRadians(),
+        estimator.getEstimatedPosition().getRotation().getRadians(),
+        0.15,
+        "Incorrect Final Theta");
+
+    if (checkError) {
+      assertEquals(
+          0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error");
+      assertEquals(0.0, maxError, 0.2, "Incorrect max error");
+    }
   }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
index 25def49..a8e4d3b 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
@@ -16,8 +16,8 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
-import edu.wpi.first.math.numbers.N4;
-import edu.wpi.first.math.numbers.N6;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N5;
 import edu.wpi.first.math.system.Discretization;
 import edu.wpi.first.math.system.NumericalIntegration;
 import edu.wpi.first.math.system.NumericalJacobian;
@@ -30,92 +30,109 @@
 import org.junit.jupiter.api.Test;
 
 class UnscentedKalmanFilterTest {
-  @SuppressWarnings({"LocalVariableName", "ParameterName"})
-  private static Matrix<N6, N1> getDynamics(Matrix<N6, N1> x, Matrix<N2, N1> u) {
+  private static Matrix<N5, N1> getDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
     var motors = DCMotor.getCIM(2);
 
-    var gHigh = 7.08;
-    var rb = 0.8382 / 2.0;
-    var r = 0.0746125;
-    var m = 63.503;
-    var J = 5.6;
+    // var gLow = 15.32;    // Low gear ratio
+    var gHigh = 7.08; // High gear ratio
+    var rb = 0.8382 / 2.0; // Robot radius
+    var r = 0.0746125; // Wheel radius
+    var m = 63.503; // Robot mass
+    var J = 5.6; // Robot moment of inertia
 
     var C1 =
         -Math.pow(gHigh, 2)
             * motors.KtNMPerAmp
             / (motors.KvRadPerSecPerVolt * motors.rOhms * r * r);
     var C2 = gHigh * motors.KtNMPerAmp / (motors.rOhms * r);
+    var k1 = 1.0 / m + Math.pow(rb, 2) / J;
+    var k2 = 1.0 / m - Math.pow(rb, 2) / J;
 
-    var c = x.get(2, 0);
-    var s = x.get(3, 0);
-    var vl = x.get(4, 0);
-    var vr = x.get(5, 0);
-
+    var vl = x.get(3, 0);
+    var vr = x.get(4, 0);
     var Vl = u.get(0, 0);
     var Vr = u.get(1, 0);
 
-    var k1 = 1.0 / m + rb * rb / J;
-    var k2 = 1.0 / m - rb * rb / J;
-
-    var xvel = (vl + vr) / 2;
-    var w = (vr - vl) / (2.0 * rb);
-
+    var v = 0.5 * (vl + vr);
     return VecBuilder.fill(
-        xvel * c,
-        xvel * s,
-        -s * w,
-        c * w,
-        k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)),
-        k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr)));
+        v * Math.cos(x.get(2, 0)),
+        v * Math.sin(x.get(2, 0)),
+        (vr - vl) / (2.0 * rb),
+        k1 * (C1 * vl + C2 * Vl) + k2 * (C1 * vr + C2 * Vr),
+        k2 * (C1 * vl + C2 * Vl) + k1 * (C1 * vr + C2 * Vr));
   }
 
-  @SuppressWarnings({"PMD.UnusedFormalParameter", "ParameterName"})
-  private static Matrix<N4, N1> getLocalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
-    return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0), x.get(5, 0));
+  @SuppressWarnings("PMD.UnusedFormalParameter")
+  private static Matrix<N3, N1> getLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+    return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
   }
 
-  @SuppressWarnings({"PMD.UnusedFormalParameter", "ParameterName"})
-  private static Matrix<N6, N1> getGlobalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
+  @SuppressWarnings("PMD.UnusedFormalParameter")
+  private static Matrix<N5, N1> getGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
     return x.copy();
   }
 
   @Test
-  @SuppressWarnings("LocalVariableName")
   void testInit() {
+    var dtSeconds = 0.005;
     assertDoesNotThrow(
         () -> {
-          UnscentedKalmanFilter<N6, N2, N4> observer =
+          UnscentedKalmanFilter<N5, N2, N3> observer =
               new UnscentedKalmanFilter<>(
-                  Nat.N6(),
-                  Nat.N4(),
+                  Nat.N5(),
+                  Nat.N3(),
                   UnscentedKalmanFilterTest::getDynamics,
                   UnscentedKalmanFilterTest::getLocalMeasurementModel,
-                  VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
-                  VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
-                  0.00505);
+                  VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
+                  VecBuilder.fill(0.0001, 0.01, 0.01),
+                  AngleStatistics.angleMean(2),
+                  AngleStatistics.angleMean(0),
+                  AngleStatistics.angleResidual(2),
+                  AngleStatistics.angleResidual(0),
+                  AngleStatistics.angleAdd(2),
+                  dtSeconds);
 
           var u = VecBuilder.fill(12.0, 12.0);
-          observer.predict(u, 0.00505);
+          observer.predict(u, dtSeconds);
 
           var localY = getLocalMeasurementModel(observer.getXhat(), u);
           observer.correct(u, localY);
+
+          var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
+          var R =
+              StateSpaceUtil.makeCovarianceMatrix(
+                  Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.01, 0.01));
+          observer.correct(
+              Nat.N5(),
+              u,
+              globalY,
+              UnscentedKalmanFilterTest::getGlobalMeasurementModel,
+              R,
+              AngleStatistics.angleMean(2),
+              AngleStatistics.angleResidual(2),
+              AngleStatistics.angleResidual(2),
+              AngleStatistics.angleAdd(2));
         });
   }
 
-  @SuppressWarnings("LocalVariableName")
   @Test
   void testConvergence() {
-    double dtSeconds = 0.00505;
+    double dtSeconds = 0.005;
     double rbMeters = 0.8382 / 2.0; // Robot radius
 
-    UnscentedKalmanFilter<N6, N2, N4> observer =
+    UnscentedKalmanFilter<N5, N2, N3> observer =
         new UnscentedKalmanFilter<>(
-            Nat.N6(),
-            Nat.N4(),
+            Nat.N5(),
+            Nat.N3(),
             UnscentedKalmanFilterTest::getDynamics,
             UnscentedKalmanFilterTest::getLocalMeasurementModel,
-            VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
-            VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
+            VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
+            VecBuilder.fill(0.0001, 0.5, 0.5),
+            AngleStatistics.angleMean(2),
+            AngleStatistics.angleMean(0),
+            AngleStatistics.angleResidual(2),
+            AngleStatistics.angleResidual(0),
+            AngleStatistics.angleAdd(2),
             dtSeconds);
 
     List<Pose2d> waypoints =
@@ -125,56 +142,52 @@
     var trajectory =
         TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));
 
-    Matrix<N6, N1> nextR;
+    Matrix<N5, N1> r = new Matrix<>(Nat.N5(), Nat.N1());
     Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
 
     var B =
         NumericalJacobian.numericalJacobianU(
-            Nat.N6(),
+            Nat.N5(),
             Nat.N2(),
             UnscentedKalmanFilterTest::getDynamics,
-            new Matrix<>(Nat.N6(), Nat.N1()),
-            u);
+            new Matrix<>(Nat.N5(), Nat.N1()),
+            new Matrix<>(Nat.N2(), Nat.N1()));
 
-    observer.setXhat(VecBuilder.fill(2.75, 22.521, 1.0, 0.0, 0.0, 0.0)); // TODO not hard code this
-
-    var ref = trajectory.sample(0.0);
-
-    Matrix<N6, N1> r =
+    observer.setXhat(
         VecBuilder.fill(
-            ref.poseMeters.getTranslation().getX(),
-            ref.poseMeters.getTranslation().getY(),
-            ref.poseMeters.getRotation().getCos(),
-            ref.poseMeters.getRotation().getSin(),
-            ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters)),
-            ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters)));
-    nextR = r.copy();
+            trajectory.getInitialPose().getTranslation().getX(),
+            trajectory.getInitialPose().getTranslation().getY(),
+            trajectory.getInitialPose().getRotation().getRadians(),
+            0.0,
+            0.0));
 
     var trueXhat = observer.getXhat();
 
     double totalTime = trajectory.getTotalTimeSeconds();
     for (int i = 0; i < (totalTime / dtSeconds); i++) {
-      ref = trajectory.sample(dtSeconds * i);
+      var ref = trajectory.sample(dtSeconds * i);
       double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters));
       double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters));
 
-      nextR.set(0, 0, ref.poseMeters.getTranslation().getX());
-      nextR.set(1, 0, ref.poseMeters.getTranslation().getY());
-      nextR.set(2, 0, ref.poseMeters.getRotation().getCos());
-      nextR.set(3, 0, ref.poseMeters.getRotation().getSin());
-      nextR.set(4, 0, vl);
-      nextR.set(5, 0, vr);
+      var nextR =
+          VecBuilder.fill(
+              ref.poseMeters.getTranslation().getX(),
+              ref.poseMeters.getTranslation().getY(),
+              ref.poseMeters.getRotation().getRadians(),
+              vl,
+              vr);
 
-      Matrix<N4, N1> localY = getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
-      var noiseStdDev = VecBuilder.fill(0.001, 0.001, 0.5, 0.5);
+      Matrix<N3, N1> localY = getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
+      var noiseStdDev = VecBuilder.fill(0.0001, 0.5, 0.5);
 
       observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev)));
 
       var rdot = nextR.minus(r).div(dtSeconds);
       u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
 
-      r = nextR;
       observer.predict(u, dtSeconds);
+
+      r = nextR;
       trueXhat =
           NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds);
     }
@@ -183,29 +196,31 @@
     observer.correct(u, localY);
 
     var globalY = getGlobalMeasurementModel(trueXhat, u);
-    var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.0001, 0.5, 0.5));
+    var R =
+        StateSpaceUtil.makeCovarianceMatrix(
+            Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
     observer.correct(
-        Nat.N6(),
+        Nat.N5(),
         u,
         globalY,
         UnscentedKalmanFilterTest::getGlobalMeasurementModel,
         R,
-        (sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)),
-        Matrix::minus,
-        Matrix::minus,
-        Matrix::plus);
+        AngleStatistics.angleMean(2),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleAdd(2));
 
     final var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
 
-    assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 0.25);
-    assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 0.25);
-    assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0);
-    assertEquals(0.0, observer.getXhat(3), 1.0);
-    assertEquals(0.0, observer.getXhat(4), 1.0);
+    assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 0.055);
+    assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 0.15);
+    assertEquals(
+        finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 0.000005);
+    assertEquals(0.0, observer.getXhat(3), 0.1);
+    assertEquals(0.0, observer.getXhat(4), 0.1);
   }
 
   @Test
-  @SuppressWarnings({"LocalVariableName", "ParameterName"})
   void testLinearUKF() {
     var dt = 0.020;
     var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
@@ -236,94 +251,22 @@
   }
 
   @Test
-  void testUnscentedTransform() {
-    // From FilterPy
-    var ret =
-        UnscentedKalmanFilter.unscentedTransform(
-            Nat.N4(),
-            Nat.N4(),
-            Matrix.mat(Nat.N4(), Nat.N9())
-                .fill(
-                    -0.9,
-                    -0.822540333075852,
-                    -0.8922540333075852,
-                    -0.9,
-                    -0.9,
-                    -0.9774596669241481,
-                    -0.9077459666924148,
-                    -0.9,
-                    -0.9,
-                    1.0,
-                    1.0,
-                    1.077459666924148,
-                    1.0,
-                    1.0,
-                    1.0,
-                    0.9225403330758519,
-                    1.0,
-                    1.0,
-                    -0.9,
-                    -0.9,
-                    -0.9,
-                    -0.822540333075852,
-                    -0.8922540333075852,
-                    -0.9,
-                    -0.9,
-                    -0.9774596669241481,
-                    -0.9077459666924148,
-                    1.0,
-                    1.0,
-                    1.0,
-                    1.0,
-                    1.077459666924148,
-                    1.0,
-                    1.0,
-                    1.0,
-                    0.9225403330758519),
-            VecBuilder.fill(
-                -132.33333333,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667),
-            VecBuilder.fill(
-                -129.34333333,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667),
-            (sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)),
-            Matrix::minus);
+  void testRoundTripP() {
+    var dtSeconds = 0.005;
 
-    assertTrue(VecBuilder.fill(-0.9, 1, -0.9, 1).isEqual(ret.getFirst(), 1E-5));
+    var observer =
+        new UnscentedKalmanFilter<>(
+            Nat.N2(),
+            Nat.N2(),
+            (x, u) -> x,
+            (x, u) -> x,
+            VecBuilder.fill(0.0, 0.0),
+            VecBuilder.fill(0.0, 0.0),
+            dtSeconds);
 
-    assertTrue(
-        Matrix.mat(Nat.N4(), Nat.N4())
-            .fill(
-                2.02000002e-01,
-                2.00000500e-02,
-                -2.69044710e-29,
-                -4.59511477e-29,
-                2.00000500e-02,
-                2.00001000e-01,
-                -2.98781068e-29,
-                -5.12759588e-29,
-                -2.73372625e-29,
-                -3.09882635e-29,
-                2.02000002e-01,
-                2.00000500e-02,
-                -4.67065917e-29,
-                -5.10705197e-29,
-                2.00000500e-02,
-                2.00001000e-01)
-            .isEqual(ret.getSecond(), 1E-5));
+    var P = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 1.0, 2.0);
+    observer.setP(P);
+
+    assertTrue(observer.getP().isEqual(P, 1e-9));
   }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
index f0b39c6..ec43cd5 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
@@ -141,7 +141,7 @@
         3,
         // f(x) = ln(x)
         (double x) -> Math.log(x),
-        // df/dx = 1 / x
+        // df/dx = 1/x
         (double x) -> 1.0 / x,
         h,
         1.0,
@@ -174,7 +174,7 @@
         5,
         // f(x) = ln(x)
         (double x) -> Math.log(x),
-        // d²f/dx² = -1 / x²
+        // d²f/dx² = -1/x²
         (double x) -> -1.0 / (x * x),
         h,
         1.0,
@@ -213,7 +213,7 @@
         2,
         // f(x) = ln(x)
         (double x) -> Math.log(x),
-        // df/dx = 1 / x
+        // df/dx = 1/x
         (double x) -> 1.0 / x,
         h,
         1.0,
@@ -246,7 +246,7 @@
         4,
         // f(x) = ln(x)
         (double x) -> Math.log(x),
-        // d²f/dx² = -1 / x²
+        // d²f/dx² = -1/x²
         (double x) -> -1.0 / (x * x),
         h,
         1.0,
@@ -282,7 +282,7 @@
       stencil[i] = -(samples - 1) / 2 + i;
     }
 
-    var filter = LinearFilter.finiteDifference(derivative, samples, stencil, h);
+    var filter = LinearFilter.finiteDifference(derivative, stencil, h);
 
     for (int i = (int) (min / h); i < (int) (max / h); ++i) {
       // Let filter initialize
diff --git a/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java
index e7ec644..6e94b3c 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java
@@ -38,4 +38,13 @@
     WPIUtilJNI.setMockTime(1000000L);
     assertEquals(limiter.calculate(0.5), 0.5);
   }
+
+  @Test
+  void slewRatePositiveNegativeTest() {
+    var limiter = new SlewRateLimiter(1, -0.5, 0);
+    WPIUtilJNI.setMockTime(1000000L);
+    assertEquals(limiter.calculate(2), 1);
+    WPIUtilJNI.setMockTime(2000000L);
+    assertEquals(limiter.calculate(0), 0.5);
+  }
 }
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
new file mode 100644
index 0000000..68babdd
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java
@@ -0,0 +1,251 @@
+// 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.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class CoordinateSystemTest {
+  private void checkPose3dConvert(
+      Pose3d poseFrom, Pose3d poseTo, CoordinateSystem coordFrom, CoordinateSystem coordTo) {
+    // "from" to "to"
+    assertEquals(
+        poseTo.getTranslation(),
+        CoordinateSystem.convert(poseFrom.getTranslation(), coordFrom, coordTo));
+    assertEquals(
+        poseTo.getRotation(), CoordinateSystem.convert(poseFrom.getRotation(), coordFrom, coordTo));
+    assertEquals(poseTo, CoordinateSystem.convert(poseFrom, coordFrom, coordTo));
+
+    // "to" to "from"
+    assertEquals(
+        poseFrom.getTranslation(),
+        CoordinateSystem.convert(poseTo.getTranslation(), coordTo, coordFrom));
+    assertEquals(
+        poseFrom.getRotation(), CoordinateSystem.convert(poseTo.getRotation(), coordTo, coordFrom));
+    assertEquals(poseFrom, CoordinateSystem.convert(poseTo, coordTo, coordFrom));
+  }
+
+  private void checkTransform3dConvert(
+      Transform3d transformFrom,
+      Transform3d transformTo,
+      CoordinateSystem coordFrom,
+      CoordinateSystem coordTo) {
+    // "from" to "to"
+    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));
+  }
+
+  @Test
+  void testPose3dEDNtoNWU() {
+    // No rotation from EDN to NWU
+    checkPose3dConvert(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d()),
+        new Pose3d(
+            3.0,
+            -1.0,
+            -2.0,
+            new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+
+    // 45° roll from EDN to NWU
+    checkPose3dConvert(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
+        new Pose3d(
+            3.0,
+            -1.0,
+            -2.0,
+            new Rotation3d(Units.degreesToRadians(-45.0), 0.0, Units.degreesToRadians(-90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+
+    // 45° pitch from EDN to NWU
+    checkPose3dConvert(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
+        new Pose3d(
+            3.0,
+            -1.0,
+            -2.0,
+            new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-135.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+
+    // 45° yaw from EDN to NWU
+    checkPose3dConvert(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
+        new Pose3d(
+            3.0,
+            -1.0,
+            -2.0,
+            new Rotation3d(
+                Units.degreesToRadians(-90.0),
+                Units.degreesToRadians(45.0),
+                Units.degreesToRadians(-90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+  }
+
+  @Test
+  void testPose3dEDNtoNED() {
+    // No rotation from EDN to NED
+    checkPose3dConvert(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d()),
+        new Pose3d(
+            3.0,
+            1.0,
+            2.0,
+            new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+
+    // 45° roll from EDN to NED
+    checkPose3dConvert(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
+        new Pose3d(
+            3.0,
+            1.0,
+            2.0,
+            new Rotation3d(Units.degreesToRadians(135.0), 0.0, Units.degreesToRadians(90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+
+    // 45° pitch from EDN to NED
+    checkPose3dConvert(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
+        new Pose3d(
+            3.0,
+            1.0,
+            2.0,
+            new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(135.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+
+    // 45° yaw from EDN to NED
+    checkPose3dConvert(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
+        new Pose3d(
+            3.0,
+            1.0,
+            2.0,
+            new Rotation3d(
+                Units.degreesToRadians(90.0),
+                Units.degreesToRadians(-45.0),
+                Units.degreesToRadians(90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+  }
+
+  @Test
+  void testTransform3dEDNtoNWU() {
+    // 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))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+
+    // 45° roll from EDN to NWU
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            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))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+
+    // 45° pitch from EDN to NWU
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            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))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+
+    // 45° yaw from EDN to NWU
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            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))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+  }
+
+  @Test
+  void testTransform3dEDNtoNED() {
+    // 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))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+
+    // 45° roll from EDN to NED
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            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))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+
+    // 45° pitch from EDN to NED
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            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))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+
+    // 45° yaw from EDN to NED
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            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))),
+        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 5a314b1..780c816 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
@@ -21,9 +21,9 @@
     var transformed = initial.plus(transformation);
 
     assertAll(
-        () -> assertEquals(transformed.getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon),
-        () -> assertEquals(transformed.getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon),
-        () -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon));
+        () -> assertEquals(1.0 + 5.0 / Math.sqrt(2.0), transformed.getX(), kEpsilon),
+        () -> assertEquals(2.0 + 5.0 / Math.sqrt(2.0), transformed.getY(), kEpsilon),
+        () -> assertEquals(50.0, transformed.getRotation().getDegrees(), kEpsilon));
   }
 
   @Test
@@ -34,9 +34,9 @@
     var finalRelativeToInitial = last.relativeTo(initial);
 
     assertAll(
-        () -> assertEquals(finalRelativeToInitial.getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
-        () -> assertEquals(finalRelativeToInitial.getY(), 0.0, kEpsilon),
-        () -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon));
+        () -> assertEquals(5.0 * Math.sqrt(2.0), finalRelativeToInitial.getX(), kEpsilon),
+        () -> assertEquals(0.0, finalRelativeToInitial.getY(), kEpsilon),
+        () -> assertEquals(0.0, finalRelativeToInitial.getRotation().getDegrees(), kEpsilon));
   }
 
   @Test
@@ -61,8 +61,8 @@
     final var transform = last.minus(initial);
 
     assertAll(
-        () -> assertEquals(transform.getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
-        () -> assertEquals(transform.getY(), 0.0, kEpsilon),
-        () -> assertEquals(transform.getRotation().getDegrees(), 0.0, kEpsilon));
+        () -> assertEquals(5.0 * Math.sqrt(2.0), transform.getX(), kEpsilon),
+        () -> assertEquals(0.0, transform.getY(), kEpsilon),
+        () -> assertEquals(0.0, transform.getRotation().getDegrees(), kEpsilon));
   }
 }
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
new file mode 100644
index 0000000..f13819f
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
@@ -0,0 +1,156 @@
+// 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.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.VecBuilder;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class Pose3dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testTransformByRotations() {
+    var initialPose =
+        new Pose3d(
+            new Translation3d(0.0, 0.0, 0.0),
+            new Rotation3d(
+                Units.degreesToRadians(0.0),
+                Units.degreesToRadians(0.0),
+                Units.degreesToRadians(0.0)));
+
+    var transform1 =
+        new Transform3d(
+            new Translation3d(0.0, 0.0, 0.0),
+            new Rotation3d(
+                Units.degreesToRadians(90.0),
+                Units.degreesToRadians(45.0),
+                Units.degreesToRadians(0.0)));
+
+    var transform2 =
+        new Transform3d(
+            new Translation3d(0.0, 0.0, 0.0),
+            new Rotation3d(
+                Units.degreesToRadians(-90.0),
+                Units.degreesToRadians(0.0),
+                Units.degreesToRadians(0.0)));
+
+    var transform3 =
+        new Transform3d(
+            new Translation3d(0.0, 0.0, 0.0),
+            new Rotation3d(
+                Units.degreesToRadians(0.0),
+                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
+    // with
+    // 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 =
+        initialPose.transformBy(transform1).transformBy(transform2).transformBy(transform3);
+
+    assertAll(
+        () ->
+            assertEquals(
+                finalPose.getRotation().getX(), initialPose.getRotation().getX(), kEpsilon),
+        () ->
+            assertEquals(
+                finalPose.getRotation().getY(), initialPose.getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                finalPose.getRotation().getZ(), initialPose.getRotation().getZ(), kEpsilon));
+  }
+
+  @Test
+  void testTransformBy() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var initial =
+        new Pose3d(
+            new Translation3d(1.0, 2.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+    var transformation =
+        new Transform3d(
+            new Translation3d(5.0, 0.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0)));
+
+    var transformed = initial.plus(transformation);
+
+    assertAll(
+        () -> assertEquals(1.0 + 5.0 / Math.sqrt(2.0), transformed.getX(), kEpsilon),
+        () -> assertEquals(2.0 + 5.0 / Math.sqrt(2.0), transformed.getY(), kEpsilon),
+        () ->
+            assertEquals(Units.degreesToRadians(50.0), transformed.getRotation().getZ(), kEpsilon));
+  }
+
+  @Test
+  void testRelativeTo() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var initial = new Pose3d(0.0, 0.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+    var last = new Pose3d(5.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+
+    var finalRelativeToInitial = last.relativeTo(initial);
+
+    assertAll(
+        () -> assertEquals(5.0 * Math.sqrt(2.0), finalRelativeToInitial.getX(), kEpsilon),
+        () -> assertEquals(0.0, finalRelativeToInitial.getY(), kEpsilon),
+        () -> assertEquals(0.0, finalRelativeToInitial.getRotation().getZ(), kEpsilon));
+  }
+
+  @Test
+  void testEquality() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var one = new Pose3d(0.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0)));
+    var two = new Pose3d(0.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0)));
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var one = new Pose3d(0.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0)));
+    var two = new Pose3d(0.0, 1.524, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(43.0)));
+    assertNotEquals(one, two);
+  }
+
+  @Test
+  void testMinus() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var initial = new Pose3d(0.0, 0.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+    var last = new Pose3d(5.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+
+    final var transform = last.minus(initial);
+
+    assertAll(
+        () -> assertEquals(5.0 * Math.sqrt(2.0), transform.getX(), kEpsilon),
+        () -> assertEquals(0.0, transform.getY(), kEpsilon),
+        () -> assertEquals(0.0, transform.getRotation().getZ(), kEpsilon));
+  }
+
+  @Test
+  void testToPose2d() {
+    var pose =
+        new Pose3d(
+            1.0,
+            2.0,
+            3.0,
+            new Rotation3d(
+                Units.degreesToRadians(20.0),
+                Units.degreesToRadians(30.0),
+                Units.degreesToRadians(40.0)));
+    var expected = new Pose2d(1.0, 2.0, new Rotation2d(Units.degreesToRadians(40.0)));
+
+    assertEquals(expected, pose.toPose2d());
+  }
+}
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
new file mode 100644
index 0000000..7c7e103
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java
@@ -0,0 +1,89 @@
+// 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.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class QuaternionTest {
+  @Test
+  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());
+
+    // 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());
+
+    // 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());
+
+    q3 = 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());
+  }
+
+  @Test
+  void testTimes() {
+    // 90° CCW rotations around each axis
+    double c = Math.cos(Units.degreesToRadians(90.0) / 2.0);
+    double s = Math.sin(Units.degreesToRadians(90.0) / 2.0);
+    var xRot = new Quaternion(c, s, 0.0, 0.0);
+    var yRot = new Quaternion(c, 0.0, s, 0.0);
+    var zRot = new Quaternion(c, 0.0, 0.0, s);
+
+    // 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);
+
+    // 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());
+  }
+
+  @Test
+  void testInverse() {
+    var q = new Quaternion(0.75, 0.3, 0.4, 0.5);
+    var inv = q.inverse();
+
+    assertEquals(q.getW(), inv.getW());
+    assertEquals(-q.getX(), inv.getX());
+    assertEquals(-q.getY(), inv.getY());
+    assertEquals(-q.getZ(), inv.getZ());
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
index cb3f0f3..6702f1d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
@@ -15,12 +15,12 @@
 
   @Test
   void testRadiansToDegrees() {
-    var rot1 = new Rotation2d(Math.PI / 3);
-    var rot2 = new Rotation2d(Math.PI / 4);
+    var rot1 = Rotation2d.fromRadians(Math.PI / 3);
+    var rot2 = Rotation2d.fromRadians(Math.PI / 4);
 
     assertAll(
-        () -> assertEquals(rot1.getDegrees(), 60.0, kEpsilon),
-        () -> assertEquals(rot2.getDegrees(), 45.0, kEpsilon));
+        () -> assertEquals(60.0, rot1.getDegrees(), kEpsilon),
+        () -> assertEquals(45.0, rot2.getDegrees(), kEpsilon));
   }
 
   @Test
@@ -29,8 +29,8 @@
     var rot2 = Rotation2d.fromDegrees(30.0);
 
     assertAll(
-        () -> assertEquals(rot1.getRadians(), Math.PI / 4, kEpsilon),
-        () -> assertEquals(rot2.getRadians(), Math.PI / 6, kEpsilon));
+        () -> assertEquals(Math.PI / 4.0, rot1.getRadians(), kEpsilon),
+        () -> assertEquals(Math.PI / 6.0, rot2.getRadians(), kEpsilon));
   }
 
   @Test
@@ -39,8 +39,8 @@
     var rotated = zero.rotateBy(Rotation2d.fromDegrees(90.0));
 
     assertAll(
-        () -> assertEquals(rotated.getRadians(), Math.PI / 2.0, kEpsilon),
-        () -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon));
+        () -> assertEquals(Math.PI / 2.0, rotated.getRadians(), kEpsilon),
+        () -> assertEquals(90.0, rotated.getDegrees(), kEpsilon));
   }
 
   @Test
@@ -48,7 +48,7 @@
     var rot = Rotation2d.fromDegrees(90.0);
     rot = rot.plus(Rotation2d.fromDegrees(30.0));
 
-    assertEquals(rot.getDegrees(), 120.0, kEpsilon);
+    assertEquals(120.0, rot.getDegrees(), kEpsilon);
   }
 
   @Test
@@ -56,7 +56,22 @@
     var rot1 = Rotation2d.fromDegrees(70.0);
     var rot2 = Rotation2d.fromDegrees(30.0);
 
-    assertEquals(rot1.minus(rot2).getDegrees(), 40.0, kEpsilon);
+    assertEquals(40.0, rot1.minus(rot2).getDegrees(), kEpsilon);
+  }
+
+  @Test
+  void testUnaryMinus() {
+    var rot = Rotation2d.fromDegrees(20.0);
+
+    assertEquals(-20.0, rot.unaryMinus().getDegrees(), kEpsilon);
+  }
+
+  @Test
+  void testMultiply() {
+    var rot = Rotation2d.fromDegrees(10.0);
+
+    assertEquals(30.0, rot.times(3.0).getDegrees(), kEpsilon);
+    assertEquals(410.0, rot.times(41.0).getDegrees(), kEpsilon);
   }
 
   @Test
@@ -65,9 +80,9 @@
     var rot2 = Rotation2d.fromDegrees(43.0);
     assertEquals(rot1, rot2);
 
-    var rot3 = Rotation2d.fromDegrees(-180.0);
-    var rot4 = Rotation2d.fromDegrees(180.0);
-    assertEquals(rot3, rot4);
+    rot1 = Rotation2d.fromDegrees(-180.0);
+    rot2 = Rotation2d.fromDegrees(180.0);
+    assertEquals(rot1, rot2);
   }
 
   @Test
@@ -76,4 +91,19 @@
     var rot2 = Rotation2d.fromDegrees(43.5);
     assertNotEquals(rot1, rot2);
   }
+
+  @Test
+  void testInterpolate() {
+    // 50 + (70 - 50) * 0.5 = 60
+    var rot1 = Rotation2d.fromDegrees(50);
+    var rot2 = Rotation2d.fromDegrees(70);
+    var interpolated = rot1.interpolate(rot2, 0.5);
+    assertEquals(60.0, interpolated.getDegrees(), kEpsilon);
+
+    // -160 minus half distance between 170 and -160 (15) = -175
+    rot1 = Rotation2d.fromDegrees(170);
+    rot2 = Rotation2d.fromDegrees(-160);
+    interpolated = rot1.interpolate(rot2, 0.5);
+    assertEquals(-175.0, interpolated.getDegrees());
+  }
 }
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
new file mode 100644
index 0000000..072a2e6
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
@@ -0,0 +1,363 @@
+// 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.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 static org.junit.jupiter.api.Assertions.assertThrows;
+
+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.util.Units;
+import org.junit.jupiter.api.Test;
+
+class Rotation3dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @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);
+    assertEquals(rot1, rot2);
+
+    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);
+    assertEquals(rot3, rot4);
+
+    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);
+    assertEquals(rot5, rot6);
+  }
+
+  @Test
+  void testInitRotationMatrix() {
+    // No rotation
+    final var R1 = Matrix.eye(Nat.N3());
+    final var rot1 = new Rotation3d(R1);
+    assertEquals(new Rotation3d(), rot1);
+
+    // 90 degree CCW rotation around z-axis
+    final var R2 = new Matrix<>(Nat.N3(), Nat.N3());
+    R2.assignBlock(0, 0, VecBuilder.fill(0.0, 1.0, 0.0));
+    R2.assignBlock(0, 1, VecBuilder.fill(-1.0, 0.0, 0.0));
+    R2.assignBlock(0, 2, VecBuilder.fill(0.0, 0.0, 1.0));
+    final var rot2 = new Rotation3d(R2);
+    final var expected2 = new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0));
+    assertEquals(expected2, rot2);
+
+    // Matrix that isn't orthogonal
+    final var R3 =
+        new MatBuilder<>(Nat.N3(), Nat.N3()).fill(1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0);
+    assertThrows(IllegalArgumentException.class, () -> new Rotation3d(R3));
+
+    // Matrix that's orthogonal but not special orthogonal
+    final var R4 = Matrix.eye(Nat.N3()).times(2.0);
+    assertThrows(IllegalArgumentException.class, () -> new Rotation3d(R4));
+  }
+
+  @Test
+  void testInitTwoVector() {
+    final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+    final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    // 90 degree CW rotation around y-axis
+    final var rot1 = new Rotation3d(xAxis, zAxis);
+    final var expected1 = new Rotation3d(yAxis, -Math.PI / 2.0);
+    assertEquals(expected1, rot1);
+
+    // 45 degree CCW rotation around z-axis
+    final var rot2 = new Rotation3d(xAxis, VecBuilder.fill(1.0, 1.0, 0.0));
+    final var expected2 = new Rotation3d(zAxis, Math.PI / 4.0);
+    assertEquals(expected2, rot2);
+
+    // 0 degree rotation of x-axes
+    final var rot3 = new Rotation3d(xAxis, xAxis);
+    assertEquals(new Rotation3d(), rot3);
+
+    // 0 degree rotation of y-axes
+    final var rot4 = new Rotation3d(yAxis, yAxis);
+    assertEquals(new Rotation3d(), rot4);
+
+    // 0 degree rotation of z-axes
+    final var rot5 = new Rotation3d(zAxis, zAxis);
+    assertEquals(new Rotation3d(), rot5);
+
+    // 180 degree rotation tests. For 180 degree rotations, any quaternion with
+    // an orthogonal rotation axis is acceptable. The rotation axis and initial
+    // vector are orthogonal if their dot product is zero.
+
+    // 180 degree rotation of x-axes
+    final var rot6 = new Rotation3d(xAxis, xAxis.times(-1.0));
+    final var q6 = rot6.getQuaternion();
+    assertEquals(0.0, q6.getW());
+    assertEquals(
+        0.0,
+        q6.getX() * xAxis.get(0, 0) + q6.getY() * xAxis.get(1, 0) + q6.getZ() * xAxis.get(2, 0));
+
+    // 180 degree rotation of y-axes
+    final var rot7 = new Rotation3d(yAxis, yAxis.times(-1.0));
+    final var q7 = rot7.getQuaternion();
+    assertEquals(0.0, q7.getW());
+    assertEquals(
+        0.0,
+        q7.getX() * yAxis.get(0, 0) + q7.getY() * yAxis.get(1, 0) + q7.getZ() * yAxis.get(2, 0));
+
+    // 180 degree rotation of z-axes
+    final var rot8 = new Rotation3d(zAxis, zAxis.times(-1.0));
+    final var q8 = rot8.getQuaternion();
+    assertEquals(0.0, q8.getW());
+    assertEquals(
+        0.0,
+        q8.getX() * zAxis.get(0, 0) + q8.getY() * zAxis.get(1, 0) + q8.getZ() * zAxis.get(2, 0));
+  }
+
+  @Test
+  void testRadiansToDegrees() {
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var rot1 = new Rotation3d(zAxis, Math.PI / 3);
+    assertAll(
+        () -> assertEquals(Units.degreesToRadians(0.0), rot1.getX(), kEpsilon),
+        () -> assertEquals(Units.degreesToRadians(0.0), rot1.getY(), kEpsilon),
+        () -> assertEquals(Units.degreesToRadians(60.0), rot1.getZ(), kEpsilon));
+
+    var rot2 = new Rotation3d(zAxis, Math.PI / 4);
+    assertAll(
+        () -> assertEquals(Units.degreesToRadians(0.0), rot2.getX(), kEpsilon),
+        () -> assertEquals(Units.degreesToRadians(0.0), rot2.getY(), kEpsilon),
+        () -> assertEquals(Units.degreesToRadians(45.0), rot2.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testRadiansAndDegrees() {
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(45.0));
+    assertAll(
+        () -> assertEquals(0.0, rot1.getX(), kEpsilon),
+        () -> assertEquals(0.0, rot1.getY(), kEpsilon),
+        () -> assertEquals(Math.PI / 4.0, rot1.getZ(), kEpsilon));
+
+    var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(30.0));
+    assertAll(
+        () -> assertEquals(0.0, rot2.getX(), kEpsilon),
+        () -> assertEquals(0.0, rot2.getY(), kEpsilon),
+        () -> assertEquals(Math.PI / 6.0, rot2.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testRotationLoop() {
+    var rot = new Rotation3d();
+
+    rot = rot.plus(new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0));
+    var expected = new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0);
+    assertEquals(expected, rot);
+
+    rot = rot.plus(new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0));
+    expected =
+        new Rotation3d(
+            VecBuilder.fill(1.0 / Math.sqrt(3), 1.0 / Math.sqrt(3), -1.0 / Math.sqrt(3)),
+            Units.degreesToRadians(120.0));
+    assertEquals(expected, rot);
+
+    rot = rot.plus(new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0)));
+    expected = new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0);
+    assertEquals(expected, rot);
+
+    rot = rot.plus(new Rotation3d(0.0, Units.degreesToRadians(-90.0), 0.0));
+    assertEquals(new Rotation3d(), rot);
+  }
+
+  @Test
+  void testRotateByFromZeroX() {
+    final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+
+    final var zero = new Rotation3d();
+    var rotated = zero.rotateBy(new Rotation3d(xAxis, Units.degreesToRadians(90.0)));
+
+    var expected = new Rotation3d(xAxis, Units.degreesToRadians(90.0));
+    assertEquals(expected, rotated);
+  }
+
+  @Test
+  void testRotateByFromZeroY() {
+    final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+
+    final var zero = new Rotation3d();
+    var rotated = zero.rotateBy(new Rotation3d(yAxis, Units.degreesToRadians(90.0)));
+
+    var expected = new Rotation3d(yAxis, Units.degreesToRadians(90.0));
+    assertEquals(expected, rotated);
+  }
+
+  @Test
+  void testRotateByFromZeroZ() {
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    final var zero = new Rotation3d();
+    var rotated = zero.rotateBy(new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
+
+    var expected = new Rotation3d(zAxis, Units.degreesToRadians(90.0));
+    assertEquals(expected, rotated);
+  }
+
+  @Test
+  void testRotateByNonZeroX() {
+    final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+
+    var rot = new Rotation3d(xAxis, Units.degreesToRadians(90.0));
+    rot = rot.plus(new Rotation3d(xAxis, Units.degreesToRadians(30.0)));
+
+    var expected = new Rotation3d(xAxis, Units.degreesToRadians(120.0));
+    assertEquals(expected, rot);
+  }
+
+  @Test
+  void testRotateByNonZeroY() {
+    final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+
+    var rot = new Rotation3d(yAxis, Units.degreesToRadians(90.0));
+    rot = rot.plus(new Rotation3d(yAxis, Units.degreesToRadians(30.0)));
+
+    var expected = new Rotation3d(yAxis, Units.degreesToRadians(120.0));
+    assertEquals(expected, rot);
+  }
+
+  @Test
+  void testRotateByNonZeroZ() {
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var rot = new Rotation3d(zAxis, Units.degreesToRadians(90.0));
+    rot = rot.plus(new Rotation3d(zAxis, Units.degreesToRadians(30.0)));
+
+    var expected = new Rotation3d(zAxis, Units.degreesToRadians(120.0));
+    assertEquals(expected, rot);
+  }
+
+  @Test
+  void testMinus() {
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(70.0));
+    var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(30.0));
+
+    assertEquals(rot1.minus(rot2).getZ(), Units.degreesToRadians(40.0), kEpsilon);
+  }
+
+  @Test
+  void testEquality() {
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(43.0));
+    var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(43.0));
+    assertEquals(rot1, rot2);
+
+    rot1 = new Rotation3d(zAxis, Units.degreesToRadians(-180.0));
+    rot2 = new Rotation3d(zAxis, Units.degreesToRadians(180.0));
+    assertEquals(rot1, rot2);
+  }
+
+  @Test
+  void testAxisAngle() {
+    final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+    final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var rot1 = new Rotation3d(xAxis, Units.degreesToRadians(90.0));
+    assertEquals(xAxis, rot1.getAxis());
+    assertEquals(Math.PI / 2.0, rot1.getAngle(), 1e-9);
+
+    var rot2 = new Rotation3d(yAxis, Units.degreesToRadians(45.0));
+    assertEquals(yAxis, rot2.getAxis());
+    assertEquals(Math.PI / 4.0, rot2.getAngle(), 1e-9);
+
+    var rot3 = new Rotation3d(zAxis, Units.degreesToRadians(60.0));
+    assertEquals(zAxis, rot3.getAxis());
+    assertEquals(Math.PI / 3.0, rot3.getAngle(), 1e-9);
+  }
+
+  @Test
+  void testToRotation2d() {
+    var rotation =
+        new Rotation3d(
+            Units.degreesToRadians(20.0),
+            Units.degreesToRadians(30.0),
+            Units.degreesToRadians(40.0));
+    var expected = new Rotation2d(Units.degreesToRadians(40.0));
+
+    assertEquals(expected, rotation.toRotation2d());
+  }
+
+  @Test
+  void testInequality() {
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(43.0));
+    var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(43.5));
+    assertNotEquals(rot1, rot2);
+  }
+
+  @Test
+  void testInterpolate() {
+    final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+    final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    // 50 + (70 - 50) * 0.5 = 60
+    var rot1 = new Rotation3d(xAxis, Units.degreesToRadians(50));
+    var rot2 = new Rotation3d(xAxis, Units.degreesToRadians(70));
+    var interpolated = rot1.interpolate(rot2, 0.5);
+    assertEquals(Units.degreesToRadians(60.0), interpolated.getX(), kEpsilon);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getZ(), kEpsilon);
+
+    // -160 minus half distance between 170 and -160 (15) = -175
+    rot1 = new Rotation3d(xAxis, Units.degreesToRadians(170));
+    rot2 = new Rotation3d(xAxis, Units.degreesToRadians(-160));
+    interpolated = rot1.interpolate(rot2, 0.5);
+    assertEquals(Units.degreesToRadians(-175.0), interpolated.getX());
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getZ());
+
+    // 50 + (70 - 50) * 0.5 = 60
+    rot1 = new Rotation3d(yAxis, Units.degreesToRadians(50));
+    rot2 = new Rotation3d(yAxis, Units.degreesToRadians(70));
+    interpolated = rot1.interpolate(rot2, 0.5);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon);
+    assertEquals(Units.degreesToRadians(60.0), interpolated.getY(), kEpsilon);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getZ(), kEpsilon);
+
+    // -160 minus half distance between 170 and -160 (165) = 5
+    rot1 = new Rotation3d(yAxis, Units.degreesToRadians(170));
+    rot2 = new Rotation3d(yAxis, Units.degreesToRadians(-160));
+    interpolated = rot1.interpolate(rot2, 0.5);
+    assertEquals(Units.degreesToRadians(180.0), interpolated.getX(), kEpsilon);
+    assertEquals(Units.degreesToRadians(-5.0), interpolated.getY(), kEpsilon);
+    assertEquals(Units.degreesToRadians(180.0), interpolated.getZ(), kEpsilon);
+
+    // 50 + (70 - 50) * 0.5 = 60
+    rot1 = new Rotation3d(zAxis, Units.degreesToRadians(50));
+    rot2 = new Rotation3d(zAxis, Units.degreesToRadians(70));
+    interpolated = rot1.interpolate(rot2, 0.5);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
+    assertEquals(Units.degreesToRadians(60.0), interpolated.getZ(), kEpsilon);
+
+    // -160 minus half distance between 170 and -160 (15) = -175
+    rot1 = new Rotation3d(zAxis, Units.degreesToRadians(170));
+    rot2 = new Rotation3d(zAxis, Units.degreesToRadians(-160));
+    interpolated = rot1.interpolate(rot2, 0.5);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getX(), kEpsilon);
+    assertEquals(Units.degreesToRadians(0.0), interpolated.getY(), kEpsilon);
+    assertEquals(Units.degreesToRadians(-175.0), interpolated.getZ(), kEpsilon);
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
index 7265e25..93d8a0e 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
@@ -4,14 +4,11 @@
 
 package edu.wpi.first.math.geometry;
 
-import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
 import org.junit.jupiter.api.Test;
 
 class Transform2dTest {
-  private static final double kEpsilon = 1E-9;
-
   @Test
   void testInverse() {
     var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
@@ -20,14 +17,7 @@
     var transformed = initial.plus(transform);
     var untransformed = transformed.plus(transform.inverse());
 
-    assertAll(
-        () -> assertEquals(initial.getX(), untransformed.getX(), kEpsilon),
-        () -> assertEquals(initial.getY(), untransformed.getY(), kEpsilon),
-        () ->
-            assertEquals(
-                initial.getRotation().getDegrees(),
-                untransformed.getRotation().getDegrees(),
-                kEpsilon));
+    assertEquals(initial, untransformed);
   }
 
   @Test
@@ -39,13 +29,6 @@
     var transformedSeparate = initial.plus(transform1).plus(transform2);
     var transformedCombined = initial.plus(transform1.plus(transform2));
 
-    assertAll(
-        () -> assertEquals(transformedSeparate.getX(), transformedCombined.getX(), kEpsilon),
-        () -> assertEquals(transformedSeparate.getY(), transformedCombined.getY(), kEpsilon),
-        () ->
-            assertEquals(
-                transformedSeparate.getRotation().getDegrees(),
-                transformedCombined.getRotation().getDegrees(),
-                kEpsilon));
+    assertEquals(transformedSeparate, transformedCombined);
   }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java
new file mode 100644
index 0000000..51d5f07
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java
@@ -0,0 +1,50 @@
+// 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.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class Transform3dTest {
+  @Test
+  void testInverse() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var initial =
+        new Pose3d(
+            new Translation3d(1.0, 2.0, 3.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+    var transform =
+        new Transform3d(
+            new Translation3d(5.0, 4.0, 3.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0)));
+
+    var transformed = initial.plus(transform);
+    var untransformed = transformed.plus(transform.inverse());
+
+    assertEquals(initial, untransformed);
+  }
+
+  @Test
+  void testComposition() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var initial =
+        new Pose3d(
+            new Translation3d(1.0, 2.0, 3.0), new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+    var transform1 =
+        new Transform3d(
+            new Translation3d(5.0, 0.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0)));
+    var transform2 =
+        new Transform3d(
+            new Translation3d(0.0, 2.0, 0.0), new Rotation3d(zAxis, Units.degreesToRadians(5.0)));
+
+    var transformedSeparate = initial.plus(transform1).plus(transform2);
+    var transformedCombined = initial.plus(transform1.plus(transform2));
+
+    assertEquals(transformedSeparate, transformedCombined);
+  }
+}
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 2d8eeaa..c8f5690 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
@@ -21,8 +21,8 @@
     var sum = one.plus(two);
 
     assertAll(
-        () -> assertEquals(sum.getX(), 3.0, kEpsilon),
-        () -> assertEquals(sum.getY(), 8.0, kEpsilon));
+        () -> assertEquals(3.0, sum.getX(), kEpsilon),
+        () -> assertEquals(8.0, sum.getY(), kEpsilon));
   }
 
   @Test
@@ -33,8 +33,8 @@
     var difference = one.minus(two);
 
     assertAll(
-        () -> assertEquals(difference.getX(), -1.0, kEpsilon),
-        () -> assertEquals(difference.getY(), -2.0, kEpsilon));
+        () -> assertEquals(-1.0, difference.getX(), kEpsilon),
+        () -> assertEquals(-2.0, difference.getY(), kEpsilon));
   }
 
   @Test
@@ -43,8 +43,8 @@
     var rotated = another.rotateBy(Rotation2d.fromDegrees(90.0));
 
     assertAll(
-        () -> assertEquals(rotated.getX(), 0.0, kEpsilon),
-        () -> assertEquals(rotated.getY(), 3.0, kEpsilon));
+        () -> assertEquals(0.0, rotated.getX(), kEpsilon),
+        () -> assertEquals(3.0, rotated.getY(), kEpsilon));
   }
 
   @Test
@@ -53,8 +53,8 @@
     var mult = original.times(3);
 
     assertAll(
-        () -> assertEquals(mult.getX(), 9.0, kEpsilon),
-        () -> assertEquals(mult.getY(), 15.0, kEpsilon));
+        () -> assertEquals(9.0, mult.getX(), kEpsilon),
+        () -> assertEquals(15.0, mult.getY(), kEpsilon));
   }
 
   @Test
@@ -63,21 +63,21 @@
     var div = original.div(2);
 
     assertAll(
-        () -> assertEquals(div.getX(), 1.5, kEpsilon),
-        () -> assertEquals(div.getY(), 2.5, kEpsilon));
+        () -> assertEquals(1.5, div.getX(), kEpsilon),
+        () -> assertEquals(2.5, div.getY(), kEpsilon));
   }
 
   @Test
   void testNorm() {
     var one = new Translation2d(3.0, 5.0);
-    assertEquals(one.getNorm(), Math.hypot(3.0, 5.0), kEpsilon);
+    assertEquals(Math.hypot(3.0, 5.0), one.getNorm(), kEpsilon);
   }
 
   @Test
   void testDistance() {
     var one = new Translation2d(1, 1);
     var two = new Translation2d(6, 6);
-    assertEquals(one.getDistance(two), 5 * Math.sqrt(2), kEpsilon);
+    assertEquals(5.0 * Math.sqrt(2.0), one.getDistance(two), kEpsilon);
   }
 
   @Test
@@ -86,8 +86,8 @@
     var inverted = original.unaryMinus();
 
     assertAll(
-        () -> assertEquals(inverted.getX(), 4.5, kEpsilon),
-        () -> assertEquals(inverted.getY(), -7, kEpsilon));
+        () -> assertEquals(4.5, inverted.getX(), kEpsilon),
+        () -> assertEquals(-7.0, inverted.getY(), kEpsilon));
   }
 
   @Test
@@ -109,9 +109,9 @@
     var one = new Translation2d(Math.sqrt(2), Rotation2d.fromDegrees(45.0));
     var two = new Translation2d(2, Rotation2d.fromDegrees(60.0));
     assertAll(
-        () -> assertEquals(one.getX(), 1.0, kEpsilon),
-        () -> assertEquals(one.getY(), 1.0, kEpsilon),
-        () -> assertEquals(two.getX(), 1.0, kEpsilon),
-        () -> assertEquals(two.getY(), Math.sqrt(3), kEpsilon));
+        () -> assertEquals(1.0, one.getX(), kEpsilon),
+        () -> assertEquals(1.0, one.getY(), kEpsilon),
+        () -> assertEquals(1.0, two.getX(), kEpsilon),
+        () -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon));
   }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
new file mode 100644
index 0000000..762425b
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
@@ -0,0 +1,153 @@
+// 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.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.VecBuilder;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class Translation3dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testSum() {
+    var one = new Translation3d(1.0, 3.0, 5.0);
+    var two = new Translation3d(2.0, 5.0, 8.0);
+
+    var sum = one.plus(two);
+
+    assertAll(
+        () -> assertEquals(3.0, sum.getX(), kEpsilon),
+        () -> assertEquals(8.0, sum.getY(), kEpsilon),
+        () -> assertEquals(13.0, sum.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testDifference() {
+    var one = new Translation3d(1.0, 3.0, 5.0);
+    var two = new Translation3d(2.0, 5.0, 8.0);
+
+    var difference = one.minus(two);
+
+    assertAll(
+        () -> assertEquals(-1.0, difference.getX(), kEpsilon),
+        () -> assertEquals(-2.0, difference.getY(), kEpsilon),
+        () -> assertEquals(-3.0, difference.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testRotateBy() {
+    var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+    var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var translation = new Translation3d(1.0, 2.0, 3.0);
+
+    var rotated1 = translation.rotateBy(new Rotation3d(xAxis, Units.degreesToRadians(90.0)));
+    assertAll(
+        () -> assertEquals(1.0, rotated1.getX(), kEpsilon),
+        () -> assertEquals(-3.0, rotated1.getY(), kEpsilon),
+        () -> assertEquals(2.0, rotated1.getZ(), kEpsilon));
+
+    var rotated2 = translation.rotateBy(new Rotation3d(yAxis, Units.degreesToRadians(90.0)));
+    assertAll(
+        () -> assertEquals(3.0, rotated2.getX(), kEpsilon),
+        () -> assertEquals(2.0, rotated2.getY(), kEpsilon),
+        () -> assertEquals(-1.0, rotated2.getZ(), kEpsilon));
+
+    var rotated3 = translation.rotateBy(new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
+    assertAll(
+        () -> assertEquals(-2.0, rotated3.getX(), kEpsilon),
+        () -> assertEquals(1.0, rotated3.getY(), kEpsilon),
+        () -> assertEquals(3.0, rotated3.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testToTranslation2d() {
+    var translation = new Translation3d(1.0, 2.0, 3.0);
+    var expected = new Translation2d(1.0, 2.0);
+
+    assertEquals(expected, translation.toTranslation2d());
+  }
+
+  @Test
+  void testMultiplication() {
+    var original = new Translation3d(3.0, 5.0, 7.0);
+    var mult = original.times(3);
+
+    assertAll(
+        () -> assertEquals(9.0, mult.getX(), kEpsilon),
+        () -> assertEquals(15.0, mult.getY(), kEpsilon),
+        () -> assertEquals(21.0, mult.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testDivision() {
+    var original = new Translation3d(3.0, 5.0, 7.0);
+    var div = original.div(2);
+
+    assertAll(
+        () -> assertEquals(1.5, div.getX(), kEpsilon),
+        () -> assertEquals(2.5, div.getY(), kEpsilon),
+        () -> assertEquals(3.5, div.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testNorm() {
+    var one = new Translation3d(3.0, 5.0, 7.0);
+    assertEquals(Math.sqrt(83.0), one.getNorm(), kEpsilon);
+  }
+
+  @Test
+  void testDistance() {
+    var one = new Translation3d(1.0, 1.0, 1.0);
+    var two = new Translation3d(6.0, 6.0, 6.0);
+    assertEquals(5.0 * Math.sqrt(3.0), one.getDistance(two), kEpsilon);
+  }
+
+  @Test
+  void testUnaryMinus() {
+    var original = new Translation3d(-4.5, 7.0, 9.0);
+    var inverted = original.unaryMinus();
+
+    assertAll(
+        () -> assertEquals(4.5, inverted.getX(), kEpsilon),
+        () -> assertEquals(-7.0, inverted.getY(), kEpsilon),
+        () -> assertEquals(-9.0, inverted.getZ(), kEpsilon));
+  }
+
+  @Test
+  void testEquality() {
+    var one = new Translation3d(9, 5.5, 3.5);
+    var two = new Translation3d(9, 5.5, 3.5);
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = new Translation3d(9, 5.5, 3.5);
+    var two = new Translation3d(9, 5.7, 3.5);
+    assertNotEquals(one, two);
+  }
+
+  @Test
+  void testPolarConstructor() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var one = new Translation3d(Math.sqrt(2), new Rotation3d(zAxis, Units.degreesToRadians(45.0)));
+    var two = new Translation3d(2, new Rotation3d(zAxis, Units.degreesToRadians(60.0)));
+    assertAll(
+        () -> assertEquals(1.0, one.getX(), kEpsilon),
+        () -> assertEquals(1.0, one.getY(), kEpsilon),
+        () -> assertEquals(0.0, one.getZ(), kEpsilon),
+        () -> assertEquals(1.0, two.getX(), kEpsilon),
+        () -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon),
+        () -> assertEquals(0.0, two.getZ(), kEpsilon));
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java
index 69a4c86..4108a62 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java
@@ -4,35 +4,28 @@
 
 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 org.junit.jupiter.api.Test;
 
 class Twist2dTest {
-  private static final double kEpsilon = 1E-9;
-
   @Test
-  void testStraightLineTwist() {
+  void testStraight() {
     var straight = new Twist2d(5.0, 0.0, 0.0);
     var straightPose = new Pose2d().exp(straight);
 
-    assertAll(
-        () -> assertEquals(straightPose.getX(), 5.0, kEpsilon),
-        () -> assertEquals(straightPose.getY(), 0.0, kEpsilon),
-        () -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon));
+    var expected = new Pose2d(5.0, 0.0, new Rotation2d());
+    assertEquals(expected, straightPose);
   }
 
   @Test
-  void testQuarterCirleTwist() {
+  void testQuarterCirle() {
     var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0);
     var quarterCirclePose = new Pose2d().exp(quarterCircle);
 
-    assertAll(
-        () -> assertEquals(quarterCirclePose.getX(), 5.0, kEpsilon),
-        () -> assertEquals(quarterCirclePose.getY(), 5.0, kEpsilon),
-        () -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon));
+    var expected = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(90.0));
+    assertEquals(expected, quarterCirclePose);
   }
 
   @Test
@@ -40,10 +33,8 @@
     var diagonal = new Twist2d(2.0, 2.0, 0.0);
     var diagonalPose = new Pose2d().exp(diagonal);
 
-    assertAll(
-        () -> assertEquals(diagonalPose.getX(), 2.0, kEpsilon),
-        () -> assertEquals(diagonalPose.getY(), 2.0, kEpsilon),
-        () -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon));
+    var expected = new Pose2d(2.0, 2.0, new Rotation2d());
+    assertEquals(expected, diagonalPose);
   }
 
   @Test
@@ -67,9 +58,11 @@
 
     final var twist = start.log(end);
 
-    assertAll(
-        () -> assertEquals(twist.dx, 5.0 / 2.0 * Math.PI, kEpsilon),
-        () -> assertEquals(twist.dy, 0.0, kEpsilon),
-        () -> assertEquals(twist.dtheta, Math.PI / 2.0, kEpsilon));
+    var expected = new Twist2d(5.0 / 2.0 * Math.PI, 0.0, Math.PI / 2.0);
+    assertEquals(expected, twist);
+
+    // Make sure computed twist gives back original end pose
+    final var reapplied = start.exp(twist);
+    assertEquals(end, reapplied);
   }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist3dTest.java
new file mode 100644
index 0000000..bb3bbb7
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist3dTest.java
@@ -0,0 +1,124 @@
+// 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.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class Twist3dTest {
+  @Test
+  void testStraightX() {
+    var straight = new Twist3d(5.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+    var straightPose = new Pose3d().exp(straight);
+
+    var expected = new Pose3d(5.0, 0.0, 0.0, new Rotation3d());
+    assertEquals(expected, straightPose);
+  }
+
+  @Test
+  void testStraightY() {
+    var straight = new Twist3d(0.0, 5.0, 0.0, 0.0, 0.0, 0.0);
+    var straightPose = new Pose3d().exp(straight);
+
+    var expected = new Pose3d(0.0, 5.0, 0.0, new Rotation3d());
+    assertEquals(expected, straightPose);
+  }
+
+  @Test
+  void testStraightZ() {
+    var straight = new Twist3d(0.0, 0.0, 5.0, 0.0, 0.0, 0.0);
+    var straightPose = new Pose3d().exp(straight);
+
+    var expected = new Pose3d(0.0, 0.0, 5.0, new Rotation3d());
+    assertEquals(expected, straightPose);
+  }
+
+  @Test
+  void testQuarterCirle() {
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var quarterCircle = new Twist3d(5.0 / 2.0 * Math.PI, 0.0, 0.0, 0.0, 0.0, Math.PI / 2.0);
+    var quarterCirclePose = new Pose3d().exp(quarterCircle);
+
+    var expected = new Pose3d(5.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
+    assertEquals(expected, quarterCirclePose);
+  }
+
+  @Test
+  void testDiagonalNoDtheta() {
+    var diagonal = new Twist3d(2.0, 2.0, 0.0, 0.0, 0.0, 0.0);
+    var diagonalPose = new Pose3d().exp(diagonal);
+
+    var expected = new Pose3d(2.0, 2.0, 0.0, new Rotation3d());
+    assertEquals(expected, diagonalPose);
+  }
+
+  @Test
+  void testEquality() {
+    var one = new Twist3d(5, 1, 0, 0.0, 0.0, 3.0);
+    var two = new Twist3d(5, 1, 0, 0.0, 0.0, 3.0);
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = new Twist3d(5, 1, 0, 0.0, 0.0, 3.0);
+    var two = new Twist3d(5, 1.2, 0, 0.0, 0.0, 3.0);
+    assertNotEquals(one, two);
+  }
+
+  @Test
+  void testPose3dLogX() {
+    final var start = new Pose3d();
+    final var end =
+        new Pose3d(0.0, 5.0, 5.0, new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0));
+
+    final var twist = start.log(end);
+
+    var expected =
+        new Twist3d(0.0, 5.0 / 2.0 * Math.PI, 0.0, Units.degreesToRadians(90.0), 0.0, 0.0);
+    assertEquals(expected, twist);
+
+    // Make sure computed twist gives back original end pose
+    final var reapplied = start.exp(twist);
+    assertEquals(end, reapplied);
+  }
+
+  @Test
+  void testPose3dLogY() {
+    final var start = new Pose3d();
+    final var end =
+        new Pose3d(5.0, 0.0, 5.0, new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0));
+
+    final var twist = start.log(end);
+
+    var expected = new Twist3d(0.0, 0.0, 5.0 / 2.0 * Math.PI, 0.0, Math.PI / 2.0, 0.0);
+    assertEquals(expected, twist);
+
+    // Make sure computed twist gives back original end pose
+    final var reapplied = start.exp(twist);
+    assertEquals(end, reapplied);
+  }
+
+  @Test
+  void testPose3dLogZ() {
+    final var start = new Pose3d();
+    final var end =
+        new Pose3d(5.0, 5.0, 0.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0)));
+
+    final var twist = start.log(end);
+
+    var expected = new Twist3d(5.0 / 2.0 * Math.PI, 0.0, 0.0, 0.0, 0.0, Math.PI / 2.0);
+    assertEquals(expected, twist);
+
+    // Make sure computed twist gives back original end pose
+    final var reapplied = start.exp(twist);
+    assertEquals(end, reapplied);
+  }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java b/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java
index 7b0e334..e65d27f 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java
@@ -16,15 +16,15 @@
     TimeInterpolatableBuffer<Rotation2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
 
     buffer.addSample(0, new Rotation2d());
-    assertEquals(0, buffer.getSample(0).getRadians(), 0.001);
+    assertEquals(0, buffer.getSample(0).get().getRadians(), 0.001);
     buffer.addSample(1, new Rotation2d(1));
-    assertEquals(0.5, buffer.getSample(0.5).getRadians(), 0.001);
-    assertEquals(1.0, buffer.getSample(1.0).getRadians(), 0.001);
+    assertEquals(0.5, buffer.getSample(0.5).get().getRadians(), 0.001);
+    assertEquals(1.0, buffer.getSample(1.0).get().getRadians(), 0.001);
     buffer.addSample(3, new Rotation2d(2));
-    assertEquals(1.5, buffer.getSample(2).getRadians(), 0.001);
+    assertEquals(1.5, buffer.getSample(2).get().getRadians(), 0.001);
 
     buffer.addSample(10.5, new Rotation2d(2));
-    assertEquals(new Rotation2d(1), buffer.getSample(0));
+    assertEquals(new Rotation2d(1), buffer.getSample(0).get());
   }
 
   @Test
@@ -34,7 +34,7 @@
     // We expect to be at (1 - 1/Math.sqrt(2), 1/Math.sqrt(2), 45deg) at t=0.5
     buffer.addSample(0, new Pose2d(0, 0, Rotation2d.fromDegrees(90)));
     buffer.addSample(1, new Pose2d(1, 1, Rotation2d.fromDegrees(0)));
-    Pose2d sample = buffer.getSample(0.5);
+    Pose2d sample = buffer.getSample(0.5).get();
 
     assertEquals(1 - 1 / Math.sqrt(2), sample.getTranslation().getX(), 0.01);
     assertEquals(1 / Math.sqrt(2), sample.getTranslation().getY(), 0.01);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java
index f85e8fb..6d15f64 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java
@@ -14,11 +14,11 @@
 class DifferentialDriveOdometryTest {
   private static final double kEpsilon = 1E-9;
   private final DifferentialDriveOdometry m_odometry =
-      new DifferentialDriveOdometry(new Rotation2d());
+      new DifferentialDriveOdometry(new Rotation2d(), 0, 0);
 
   @Test
   void testOdometryWithEncoderDistances() {
-    m_odometry.resetPosition(new Pose2d(), Rotation2d.fromDegrees(45));
+    m_odometry.resetPosition(Rotation2d.fromDegrees(45), 0, 0, new Pose2d());
     var pose = m_odometry.update(Rotation2d.fromDegrees(135.0), 0.0, 5 * Math.PI);
 
     assertAll(
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
index b3546f5..18d3ec7 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
@@ -45,6 +45,17 @@
   }
 
   @Test
+  void testStraightLineForwardKinematicsKinematicsWithDeltas() {
+    var wheelDeltas = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
+    var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+    assertAll(
+        () -> assertEquals(3.536, twist.dx, 0.1),
+        () -> assertEquals(0, twist.dy, 0.1),
+        () -> assertEquals(0, twist.dtheta, 0.1));
+  }
+
+  @Test
   void testStrafeInverseKinematics() {
     ChassisSpeeds speeds = new ChassisSpeeds(0, 4, 0);
     var moduleStates = m_kinematics.toWheelSpeeds(speeds);
@@ -68,6 +79,17 @@
   }
 
   @Test
+  void testStrafeForwardKinematicsKinematicsWithDeltas() {
+    var wheelDeltas = new MecanumDriveWheelPositions(-2.828427, 2.828427, 2.828427, -2.828427);
+    var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+    assertAll(
+        () -> assertEquals(0, twist.dx, 0.1),
+        () -> assertEquals(2.8284, twist.dy, 0.1),
+        () -> assertEquals(0, twist.dtheta, 0.1));
+  }
+
+  @Test
   void testRotationInverseKinematics() {
     ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
     var moduleStates = m_kinematics.toWheelSpeeds(speeds);
@@ -91,6 +113,17 @@
   }
 
   @Test
+  void testRotationForwardKinematicsKinematicsWithDeltas() {
+    var wheelDeltas = new MecanumDriveWheelPositions(-150.79645, 150.79645, -150.79645, 150.79645);
+    var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+    assertAll(
+        () -> assertEquals(0, twist.dx, 0.1),
+        () -> assertEquals(0, twist.dy, 0.1),
+        () -> assertEquals(2 * Math.PI, twist.dtheta, 0.1));
+  }
+
+  @Test
   void testMixedTranslationRotationInverseKinematics() {
     ChassisSpeeds speeds = new ChassisSpeeds(2, 3, 1);
     var moduleStates = m_kinematics.toWheelSpeeds(speeds);
@@ -114,6 +147,17 @@
   }
 
   @Test
+  void testMixedTranslationRotationForwardKinematicsKinematicsWithDeltas() {
+    var wheelDeltas = new MecanumDriveWheelPositions(-17.677670, 20.51, -13.44, 16.26);
+    var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+    assertAll(
+        () -> assertEquals(1.413, twist.dx, 0.1),
+        () -> assertEquals(2.122, twist.dy, 0.1),
+        () -> assertEquals(0.707, twist.dtheta, 0.1));
+  }
+
+  @Test
   void testOffCenterRotationInverseKinematics() {
     ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 1);
     var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
@@ -137,6 +181,17 @@
   }
 
   @Test
+  void testOffCenterRotationForwardKinematicsKinematicsWithDeltas() {
+    var wheelDeltas = new MecanumDriveWheelPositions(0, 16.971, -16.971, 33.941);
+    var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+    assertAll(
+        () -> assertEquals(8.48525, twist.dx, 0.1),
+        () -> assertEquals(-8.48525, twist.dy, 0.1),
+        () -> assertEquals(0.707, twist.dtheta, 0.1));
+  }
+
+  @Test
   void testOffCenterTranslationRotationInverseKinematics() {
     ChassisSpeeds speeds = new ChassisSpeeds(5, 2, 1);
     var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
@@ -160,6 +215,17 @@
   }
 
   @Test
+  void testOffCenterRotationTranslationForwardKinematicsKinematicsWithDeltas() {
+    var wheelDeltas = new MecanumDriveWheelPositions(2.12, 21.92, -12.02, 36.06);
+    var twist = m_kinematics.toTwist2d(wheelDeltas);
+
+    assertAll(
+        () -> assertEquals(12.02, twist.dx, 0.1),
+        () -> assertEquals(-7.07, twist.dy, 0.1),
+        () -> assertEquals(0.707, twist.dtheta, 0.1));
+  }
+
+  @Test
   void testDesaturate() {
     var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7);
     wheelSpeeds.desaturate(5.5);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
index 3f43109..cf6a439 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
@@ -10,6 +10,10 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import java.util.Random;
 import org.junit.jupiter.api.Test;
 
 class MecanumDriveOdometryTest {
@@ -21,15 +25,19 @@
   private final MecanumDriveKinematics m_kinematics =
       new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
 
+  private final MecanumDriveWheelPositions zero = new MecanumDriveWheelPositions();
+
   private final MecanumDriveOdometry m_odometry =
-      new MecanumDriveOdometry(m_kinematics, new Rotation2d());
+      new MecanumDriveOdometry(m_kinematics, new Rotation2d(), zero);
 
   @Test
   void testMultipleConsecutiveUpdates() {
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+    var wheelPositions = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
 
-    m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
-    var secondPose = m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
+    m_odometry.resetPosition(new Rotation2d(), wheelPositions, new Pose2d());
+
+    m_odometry.update(new Rotation2d(), wheelPositions);
+    var secondPose = m_odometry.update(new Rotation2d(), wheelPositions);
 
     assertAll(
         () -> assertEquals(secondPose.getX(), 0.0, 0.01),
@@ -39,11 +47,12 @@
 
   @Test
   void testTwoIterations() {
-    // 5 units/sec  in the x axis (forward)
-    final var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+    // 5 units/sec  in the x-axis (forward)
+    final var wheelPositions = new MecanumDriveWheelPositions(0.3536, 0.3536, 0.3536, 0.3536);
+    m_odometry.resetPosition(new Rotation2d(), new MecanumDriveWheelPositions(), new Pose2d());
 
-    m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
-    var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
+    m_odometry.update(new Rotation2d(), new MecanumDriveWheelPositions());
+    var pose = m_odometry.update(new Rotation2d(), wheelPositions);
 
     assertAll(
         () -> assertEquals(0.3536, pose.getX(), 0.01),
@@ -55,10 +64,11 @@
   void test90degreeTurn() {
     // This is a 90 degree turn about the point between front left and rear left wheels
     // fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946
-    final var wheelSpeeds = new MecanumDriveWheelSpeeds(-13.328, 39.986, -13.329, 39.986);
+    final var wheelPositions = new MecanumDriveWheelPositions(-13.328, 39.986, -13.329, 39.986);
+    m_odometry.resetPosition(new Rotation2d(), new MecanumDriveWheelPositions(), new Pose2d());
 
-    m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
-    final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
+    m_odometry.update(new Rotation2d(), new MecanumDriveWheelPositions());
+    final var pose = m_odometry.update(Rotation2d.fromDegrees(90.0), wheelPositions);
 
     assertAll(
         () -> assertEquals(8.4855, pose.getX(), 0.01),
@@ -70,14 +80,188 @@
   void testGyroAngleReset() {
     var gyro = Rotation2d.fromDegrees(90.0);
     var fieldAngle = Rotation2d.fromDegrees(0.0);
-    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
-    var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
-    m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds());
-    var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
+    m_odometry.resetPosition(
+        gyro, new MecanumDriveWheelPositions(), new Pose2d(new Translation2d(), fieldAngle));
+    var speeds = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
+    m_odometry.update(gyro, new MecanumDriveWheelPositions());
+    var pose = m_odometry.update(gyro, speeds);
 
     assertAll(
         () -> assertEquals(3.536, pose.getX(), 0.1),
         () -> assertEquals(0.0, pose.getY(), 0.1),
         () -> assertEquals(0.0, pose.getRotation().getRadians(), 0.1));
   }
+
+  @Test
+  void testAccuracyFacingTrajectory() {
+    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 odometry =
+        new MecanumDriveOdometry(kinematics, new Rotation2d(), wheelPositions, new Pose2d());
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(),
+                new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
+                new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
+                new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
+                new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
+                new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
+            new TrajectoryConfig(0.5, 2));
+
+    var rand = new Random(5190);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    double odometryDistanceTravelled = 0;
+    double trajectoryDistanceTravelled = 0;
+    while (t <= trajectory.getTotalTimeSeconds()) {
+      var groundTruthState = trajectory.sample(t);
+
+      trajectoryDistanceTravelled +=
+          groundTruthState.velocityMetersPerSecond * dt
+              + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+
+      var wheelSpeeds =
+          kinematics.toWheelSpeeds(
+              new ChassisSpeeds(
+                  groundTruthState.velocityMetersPerSecond,
+                  0,
+                  groundTruthState.velocityMetersPerSecond
+                      * groundTruthState.curvatureRadPerMeter));
+
+      wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
+
+      wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
+      wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
+      wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
+      wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
+
+      var lastPose = odometry.getPoseMeters();
+
+      var xHat =
+          odometry.update(
+              groundTruthState
+                  .poseMeters
+                  .getRotation()
+                  .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
+              wheelPositions);
+
+      odometryDistanceTravelled += lastPose.getTranslation().getDistance(xHat.getTranslation());
+
+      double error =
+          groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error");
+    assertEquals(0.0, maxError, 0.3, "Incorrect max error");
+    assertEquals(
+        1.0,
+        odometryDistanceTravelled / trajectoryDistanceTravelled,
+        0.05,
+        "Incorrect distance travelled");
+  }
+
+  @Test
+  void testAccuracyFacingXAxis() {
+    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 odometry =
+        new MecanumDriveOdometry(kinematics, new Rotation2d(), wheelPositions, new Pose2d());
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(),
+                new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
+                new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
+                new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
+                new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
+                new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
+            new TrajectoryConfig(0.5, 2));
+
+    var rand = new Random(5190);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    double odometryDistanceTravelled = 0;
+    double trajectoryDistanceTravelled = 0;
+    while (t <= trajectory.getTotalTimeSeconds()) {
+      var groundTruthState = trajectory.sample(t);
+
+      trajectoryDistanceTravelled +=
+          groundTruthState.velocityMetersPerSecond * dt
+              + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+
+      var wheelSpeeds =
+          kinematics.toWheelSpeeds(
+              new ChassisSpeeds(
+                  groundTruthState.velocityMetersPerSecond
+                      * groundTruthState.poseMeters.getRotation().getCos(),
+                  groundTruthState.velocityMetersPerSecond
+                      * groundTruthState.poseMeters.getRotation().getSin(),
+                  0));
+
+      wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
+
+      wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
+      wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
+      wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
+      wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
+
+      var lastPose = odometry.getPoseMeters();
+
+      var xHat = odometry.update(new Rotation2d(rand.nextGaussian() * 0.05), wheelPositions);
+
+      odometryDistanceTravelled += lastPose.getTranslation().getDistance(xHat.getTranslation());
+
+      double error =
+          groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error");
+    assertEquals(0.0, maxError, 0.3, "Incorrect max error");
+    assertEquals(
+        1.0,
+        odometryDistanceTravelled / trajectoryDistanceTravelled,
+        0.05,
+        "Incorrect distance travelled");
+  }
 }
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 574e086..43dd02a 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
@@ -41,16 +41,28 @@
 
   @Test
   void testStraightLineForwardKinematics() { // test forward kinematics going in a straight line
-    SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
+    SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(0.0));
     var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
 
     assertAll(
-        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
         () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
   }
 
   @Test
+  void testStraightLineForwardKinematicsWithDeltas() {
+    // test forward kinematics going in a straight line
+    SwerveModulePosition delta = new SwerveModulePosition(5.0, Rotation2d.fromDegrees(0.0));
+    var twist = m_kinematics.toTwist2d(delta, delta, delta, delta);
+
+    assertAll(
+        () -> assertEquals(5.0, twist.dx, kEpsilon),
+        () -> assertEquals(0.0, twist.dy, kEpsilon),
+        () -> assertEquals(0.0, twist.dtheta, kEpsilon));
+  }
+
+  @Test
   void testStraightStrafeInverseKinematics() {
     ChassisSpeeds speeds = new ChassisSpeeds(0, 5, 0);
     var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
@@ -78,13 +90,43 @@
   }
 
   @Test
+  void testStraightStrafeForwardKinematicsWithDeltas() {
+    SwerveModulePosition delta = new SwerveModulePosition(5.0, Rotation2d.fromDegrees(90.0));
+    var twist = m_kinematics.toTwist2d(delta, delta, delta, delta);
+
+    assertAll(
+        () -> assertEquals(0.0, twist.dx, kEpsilon),
+        () -> assertEquals(5.0, twist.dy, kEpsilon),
+        () -> assertEquals(0.0, twist.dtheta, kEpsilon));
+  }
+
+  @Test
+  void testConserveWheelAngle() {
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
+    m_kinematics.toSwerveModuleStates(speeds);
+    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(135.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon));
+  }
+
+  @Test
   void testTurnInPlaceInverseKinematics() {
     ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
     var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
 
     /*
-    The circumference of the wheels about the COR is pi * diameter, or 2 * pi * radius
-    the radius is the sqrt(12^2in + 12^2in), or 16.9706in, so the circumference the wheels
+    The circumference of the wheels about the COR is π * diameter, or 2π * radius
+    the radius is the √(12²in + 12²in), or 16.9706in, so the circumference the wheels
     trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second,
     our wheels must trace out 1 rotation (or 106.63 inches) per second.
       */
@@ -116,6 +158,21 @@
   }
 
   @Test
+  void testTurnInPlaceForwardKinematicsWithDeltas() {
+    SwerveModulePosition flDelta = new SwerveModulePosition(106.629, Rotation2d.fromDegrees(135));
+    SwerveModulePosition frDelta = new SwerveModulePosition(106.629, Rotation2d.fromDegrees(45));
+    SwerveModulePosition blDelta = new SwerveModulePosition(106.629, Rotation2d.fromDegrees(-135));
+    SwerveModulePosition brDelta = new SwerveModulePosition(106.629, Rotation2d.fromDegrees(-45));
+
+    var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta);
+
+    assertAll(
+        () -> assertEquals(0.0, twist.dx, kEpsilon),
+        () -> assertEquals(0.0, twist.dy, kEpsilon),
+        () -> assertEquals(2 * Math.PI, twist.dtheta, 0.1));
+  }
+
+  @Test
   void testOffCenterCORRotationInverseKinematics() {
     ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
     var moduleStates = m_kinematics.toSwerveModuleStates(speeds, m_fl);
@@ -124,7 +181,7 @@
     This one is a bit trickier. Because we are rotating about the front-left wheel,
     it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel
     an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with
-    radius sqrt(24^2 + 24^2) and circumference 213.2584. As for angles, the front-right wheel
+    radius √(24² + 24²) and circumference 213.2584. As for angles, the front-right wheel
     should be pointing straight forward, the back-left wheel should be pointing straight right,
     and the back-right wheel should be at a -45 degree angle
     */
@@ -150,12 +207,12 @@
     var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
 
     /*
-    We already know that our omega should be 2pi from the previous test. Next, we need to determine
+    We already know that our omega should be 2π from the previous test. Next, we need to determine
     the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center,
     we know that vx and vy must be the same. Furthermore, we know that the center of mass makes
     a full revolution about the center of revolution once every second. Therefore, the center of
-    mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triagle are
-    1:sqrt(2)/2:sqrt(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
+    mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triangle are
+    1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
     */
 
     assertAll(
@@ -164,6 +221,30 @@
         () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1));
   }
 
+  @Test
+  void testOffCenterCORRotationForwardKinematicsWithDeltas() {
+    SwerveModulePosition flDelta = new SwerveModulePosition(0.0, Rotation2d.fromDegrees(0.0));
+    SwerveModulePosition frDelta = new SwerveModulePosition(150.796, Rotation2d.fromDegrees(0.0));
+    SwerveModulePosition blDelta = new SwerveModulePosition(150.796, Rotation2d.fromDegrees(-90));
+    SwerveModulePosition brDelta = new SwerveModulePosition(213.258, Rotation2d.fromDegrees(-45));
+
+    var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta);
+
+    /*
+    We already know that our omega should be 2π from the previous test. Next, we need to determine
+    the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center,
+    we know that vx and vy must be the same. Furthermore, we know that the center of mass makes
+    a full revolution about the center of revolution once every second. Therefore, the center of
+    mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triangle are
+    1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
+    */
+
+    assertAll(
+        () -> assertEquals(75.398, twist.dx, 0.1),
+        () -> assertEquals(-75.398, twist.dy, 0.1),
+        () -> assertEquals(2 * Math.PI, twist.dtheta, 0.1));
+  }
+
   private void assertModuleState(
       SwerveModuleState expected, SwerveModuleState actual, SwerveModuleState tolerance) {
     assertAll(
@@ -229,6 +310,30 @@
   }
 
   @Test
+  void testOffCenterCORRotationAndTranslationForwardKinematicsWithDeltas() {
+    SwerveModulePosition flDelta = new SwerveModulePosition(23.43, Rotation2d.fromDegrees(-140.19));
+    SwerveModulePosition frDelta = new SwerveModulePosition(23.43, Rotation2d.fromDegrees(-39.81));
+    SwerveModulePosition blDelta = new SwerveModulePosition(54.08, Rotation2d.fromDegrees(-109.44));
+    SwerveModulePosition brDelta = new SwerveModulePosition(54.08, Rotation2d.fromDegrees(-70.56));
+
+    var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta);
+
+    /*
+    From equation (13.17), we know that chassis motion is th dot product of the
+    pseudoinverse of the inverseKinematics matrix (with the center of rotation at
+    (0,0) -- we don't want the motion of the center of rotation, we want it of
+    the center of the robot). These above SwerveModuleStates are known to be from
+    a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been
+    calculated using Numpy's linalg.pinv function.
+    */
+
+    assertAll(
+        () -> assertEquals(0.0, twist.dx, 0.1),
+        () -> assertEquals(-33.0, twist.dy, 0.1),
+        () -> assertEquals(1.5, twist.dtheta, 0.1));
+  }
+
+  @Test
   void testDesaturate() {
     SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d());
     SwerveModuleState fr = new SwerveModuleState(6, new Rotation2d());
@@ -246,4 +351,24 @@
         () -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
         () -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon));
   }
+
+  @Test
+  void testDesaturateSmooth() {
+    SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d());
+    SwerveModuleState fr = new SwerveModuleState(6, new Rotation2d());
+    SwerveModuleState bl = new SwerveModuleState(4, new Rotation2d());
+    SwerveModuleState br = new SwerveModuleState(7, new Rotation2d());
+
+    SwerveModuleState[] arr = {fl, fr, bl, br};
+    SwerveDriveKinematics.desaturateWheelSpeeds(
+        arr, m_kinematics.toChassisSpeeds(arr), 5.5, 5.5, 3.5);
+
+    double factor = 5.5 / 7.0;
+
+    assertAll(
+        () -> assertEquals(5.0 * factor, arr[0].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(6.0 * factor, arr[1].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon));
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
index cb6dfdf..716da03 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
@@ -10,6 +10,10 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import java.util.Random;
 import org.junit.jupiter.api.Test;
 
 class SwerveDriveOdometryTest {
@@ -18,30 +22,34 @@
   private final Translation2d m_bl = new Translation2d(-12, 12);
   private final Translation2d m_br = new Translation2d(-12, -12);
 
+  private final SwerveModulePosition zero = new SwerveModulePosition();
+
   private final SwerveDriveKinematics m_kinematics =
       new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
 
   private final SwerveDriveOdometry m_odometry =
-      new SwerveDriveOdometry(m_kinematics, new Rotation2d());
+      new SwerveDriveOdometry(
+          m_kinematics, new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
 
   @Test
   void testTwoIterations() {
-    // 5 units/sec  in the x axis (forward)
-    final SwerveModuleState[] wheelSpeeds = {
-      new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
-      new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
-      new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
-      new SwerveModuleState(5, Rotation2d.fromDegrees(0))
+    // 5 units/sec  in the x-axis (forward)
+    final SwerveModulePosition[] wheelDeltas = {
+      new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0)),
+      new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0)),
+      new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0)),
+      new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0))
     };
 
-    m_odometry.updateWithTime(
-        0.0,
+    m_odometry.update(
         new Rotation2d(),
-        new SwerveModuleState(),
-        new SwerveModuleState(),
-        new SwerveModuleState(),
-        new SwerveModuleState());
-    var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
+        new SwerveModulePosition[] {
+          new SwerveModulePosition(),
+          new SwerveModulePosition(),
+          new SwerveModulePosition(),
+          new SwerveModulePosition()
+        });
+    var pose = m_odometry.update(new Rotation2d(), wheelDeltas);
 
     assertAll(
         () -> assertEquals(5.0 / 10.0, pose.getX(), 0.01),
@@ -57,16 +65,16 @@
     //        Module 2: speed 18.84955592153876 angle -90.0
     //        Module 3: speed 42.14888838624436 angle -26.565051177077986
 
-    final SwerveModuleState[] wheelSpeeds = {
-      new SwerveModuleState(18.85, Rotation2d.fromDegrees(90.0)),
-      new SwerveModuleState(42.15, Rotation2d.fromDegrees(26.565)),
-      new SwerveModuleState(18.85, Rotation2d.fromDegrees(-90)),
-      new SwerveModuleState(42.15, Rotation2d.fromDegrees(-26.565))
+    final SwerveModulePosition[] wheelDeltas = {
+      new SwerveModulePosition(18.85, Rotation2d.fromDegrees(90.0)),
+      new SwerveModulePosition(42.15, Rotation2d.fromDegrees(26.565)),
+      new SwerveModulePosition(18.85, Rotation2d.fromDegrees(-90)),
+      new SwerveModulePosition(42.15, Rotation2d.fromDegrees(-26.565))
     };
-    final var zero = new SwerveModuleState();
+    final var zero = new SwerveModulePosition();
 
-    m_odometry.updateWithTime(0.0, new Rotation2d(), zero, zero, zero, zero);
-    final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
+    m_odometry.update(new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
+    final var pose = m_odometry.update(Rotation2d.fromDegrees(90.0), wheelDeltas);
 
     assertAll(
         () -> assertEquals(12.0, pose.getX(), 0.01),
@@ -78,15 +86,191 @@
   void testGyroAngleReset() {
     var gyro = Rotation2d.fromDegrees(90.0);
     var fieldAngle = Rotation2d.fromDegrees(0.0);
-    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
-    var state = new SwerveModuleState();
-    m_odometry.updateWithTime(0.0, gyro, state, state, state, state);
-    state = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0));
-    var pose = m_odometry.updateWithTime(1.0, gyro, state, state, state, state);
+    m_odometry.resetPosition(
+        gyro,
+        new SwerveModulePosition[] {zero, zero, zero, zero},
+        new Pose2d(new Translation2d(), fieldAngle));
+    var delta = new SwerveModulePosition();
+    m_odometry.update(gyro, new SwerveModulePosition[] {delta, delta, delta, delta});
+    delta = new SwerveModulePosition(1.0, Rotation2d.fromDegrees(0));
+    var pose = m_odometry.update(gyro, new SwerveModulePosition[] {delta, delta, delta, delta});
 
     assertAll(
         () -> assertEquals(1.0, pose.getX(), 0.1),
         () -> assertEquals(0.00, pose.getY(), 0.1),
         () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1));
   }
+
+  @Test
+  void testAccuracyFacingTrajectory() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1),
+            new Translation2d(-1, 1));
+    var odometry =
+        new SwerveDriveOdometry(
+            kinematics, new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
+
+    SwerveModulePosition fl = new SwerveModulePosition();
+    SwerveModulePosition fr = new SwerveModulePosition();
+    SwerveModulePosition bl = new SwerveModulePosition();
+    SwerveModulePosition br = new SwerveModulePosition();
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(0.5, 2));
+
+    var rand = new Random(4915);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTimeSeconds()) {
+      var groundTruthState = trajectory.sample(t);
+
+      var moduleStates =
+          kinematics.toSwerveModuleStates(
+              new ChassisSpeeds(
+                  groundTruthState.velocityMetersPerSecond,
+                  0.0,
+                  groundTruthState.velocityMetersPerSecond
+                      * groundTruthState.curvatureRadPerMeter));
+      for (var moduleState : moduleStates) {
+        moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
+        moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1;
+      }
+
+      fl.distanceMeters += moduleStates[0].speedMetersPerSecond * dt;
+      fr.distanceMeters += moduleStates[1].speedMetersPerSecond * dt;
+      bl.distanceMeters += moduleStates[2].speedMetersPerSecond * dt;
+      br.distanceMeters += moduleStates[3].speedMetersPerSecond * dt;
+
+      fl.angle = moduleStates[0].angle;
+      fr.angle = moduleStates[1].angle;
+      bl.angle = moduleStates[2].angle;
+      br.angle = moduleStates[3].angle;
+
+      var xHat =
+          odometry.update(
+              groundTruthState
+                  .poseMeters
+                  .getRotation()
+                  .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
+              new SwerveModulePosition[] {fl, fr, bl, br});
+
+      double error =
+          groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X");
+    assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y");
+    assertEquals(
+        Math.PI / 4,
+        odometry.getPoseMeters().getRotation().getRadians(),
+        10 * Math.PI / 180,
+        "Incorrect Final Theta");
+
+    assertEquals(
+        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
+    assertEquals(0.0, maxError, 0.125, "Incorrect max error");
+  }
+
+  @Test
+  void testAccuracyFacingXAxis() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1),
+            new Translation2d(-1, 1));
+    var odometry =
+        new SwerveDriveOdometry(
+            kinematics, new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
+
+    SwerveModulePosition fl = new SwerveModulePosition();
+    SwerveModulePosition fr = new SwerveModulePosition();
+    SwerveModulePosition bl = new SwerveModulePosition();
+    SwerveModulePosition br = new SwerveModulePosition();
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(0.5, 2));
+
+    var rand = new Random(4915);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTimeSeconds()) {
+      var groundTruthState = trajectory.sample(t);
+
+      fl.distanceMeters +=
+          groundTruthState.velocityMetersPerSecond * dt
+              + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+      fr.distanceMeters +=
+          groundTruthState.velocityMetersPerSecond * dt
+              + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+      bl.distanceMeters +=
+          groundTruthState.velocityMetersPerSecond * dt
+              + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+      br.distanceMeters +=
+          groundTruthState.velocityMetersPerSecond * dt
+              + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
+
+      fl.angle = groundTruthState.poseMeters.getRotation();
+      fr.angle = groundTruthState.poseMeters.getRotation();
+      bl.angle = groundTruthState.poseMeters.getRotation();
+      br.angle = groundTruthState.poseMeters.getRotation();
+
+      var xHat =
+          odometry.update(
+              new Rotation2d(rand.nextGaussian() * 0.05),
+              new SwerveModulePosition[] {fl, fr, bl, br});
+
+      double error =
+          groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X");
+    assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y");
+    assertEquals(
+        0.0,
+        odometry.getPoseMeters().getRotation().getRadians(),
+        10 * Math.PI / 180,
+        "Incorrect Final Theta");
+
+    assertEquals(
+        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.06, "Incorrect mean error");
+    assertEquals(0.0, maxError, 0.125, "Incorrect max error");
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java b/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java
index dd08e45..8e3d02b 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java
@@ -23,7 +23,6 @@
   private static final double kMaxDy = 0.00127;
   private static final double kMaxDtheta = 0.0872;
 
-  @SuppressWarnings("ParameterName")
   private void run(Pose2d a, List<Translation2d> waypoints, Pose2d b) {
     // Start the timer.
     // var start = System.nanoTime();
@@ -97,13 +96,11 @@
                 1E-9));
   }
 
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
   @Test
   void testStraightLine() {
     run(new Pose2d(), new ArrayList<>(), new Pose2d(3, 0, new Rotation2d()));
   }
 
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
   @Test
   void testSCurve() {
     var start = new Pose2d(0, 0, Rotation2d.fromDegrees(90.0));
@@ -115,7 +112,6 @@
     run(start, waypoints, end);
   }
 
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
   @Test
   void testOneInterior() {
     var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
@@ -126,7 +122,6 @@
     run(start, waypoints, end);
   }
 
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
   @Test
   void testWindyPath() {
     final var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
diff --git a/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java b/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java
index 8367070..6435dd5 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java
@@ -20,7 +20,6 @@
   private static final double kMaxDy = 0.00127;
   private static final double kMaxDtheta = 0.0872;
 
-  @SuppressWarnings("ParameterName")
   private void run(Pose2d a, Pose2d b) {
     // Start the timer.
     // var start = System.nanoTime();
@@ -68,19 +67,16 @@
                 1E-9));
   }
 
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
   @Test
   void testStraightLine() {
     run(new Pose2d(), new Pose2d(3, 0, new Rotation2d()));
   }
 
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
   @Test
   void testSimpleSCurve() {
     run(new Pose2d(), new Pose2d(1, 1, new Rotation2d()));
   }
 
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
   @Test
   void testSquiggly() {
     run(
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 3dff90b..2182674 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
@@ -58,9 +58,9 @@
     assertEquals(x1Truth, x1Discrete);
   }
 
-  //                                             dt
-  // Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-  //                                             0
+  //                                               T
+  // Test that the discrete approximation of Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+  //                                               0
   @Test
   void testDiscretizeSlowModelAQ() {
     final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
@@ -68,6 +68,9 @@
 
     final double dt = 1.0;
 
+    //       T
+    // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+    //       0
     final var discQIntegrated =
         RungeKuttaTimeVarying.rungeKuttaTimeVarying(
             (Double t, Matrix<N2, N2> x) ->
@@ -87,9 +90,9 @@
             + discQIntegrated);
   }
 
-  //                                             dt
-  // Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-  //                                             0
+  //                                               T
+  // Test that the discrete approximation of Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+  //                                               0
   @Test
   void testDiscretizeFastModelAQ() {
     final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1406.29);
@@ -97,6 +100,9 @@
 
     final var dt = 0.005;
 
+    //       T
+    // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+    //       0
     final var discQIntegrated =
         RungeKuttaTimeVarying.rungeKuttaTimeVarying(
             (Double t, Matrix<N2, N2> x) ->
@@ -130,6 +136,9 @@
       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) ->
@@ -173,6 +182,9 @@
       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) ->
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 6671963..72fb5ea 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,24 +31,6 @@
   }
 
   @Test
-  void testExponentialRKF45() {
-    Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
-
-    var y1 =
-        NumericalIntegration.rkf45(
-            (x, u) -> {
-              var y = new Matrix<>(Nat.N1(), Nat.N1());
-              y.set(0, 0, Math.exp(x.get(0, 0)));
-              return y;
-            },
-            y0,
-            VecBuilder.fill(0),
-            0.1);
-
-    assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
-  }
-
-  @Test
   void testExponentialRKDP() {
     Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
 
diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java
index 7b5e844..d0e6c12 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java
@@ -23,7 +23,6 @@
    * @param y The initial value of y.
    * @param dtSeconds The time over which to integrate.
    */
-  @SuppressWarnings("MethodTypeParameterName")
   public static <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> rungeKuttaTimeVarying(
       BiFunction<Double, Matrix<Rows, Cols>, Matrix<Rows, Cols>> f,
       double t,
diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
index e4df93f..3db878e 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
@@ -15,16 +15,16 @@
 class RungeKuttaTimeVaryingTest {
   private static Matrix<N1, N1> rungeKuttaTimeVaryingSolution(double t) {
     return new MatBuilder<>(Nat.N1(), Nat.N1())
-        .fill(12.0 * Math.exp(t) / (Math.pow(Math.exp(t) + 1.0, 2.0)));
+        .fill(12.0 * Math.exp(t) / Math.pow(Math.exp(t) + 1.0, 2.0));
   }
 
   // Tests RK4 with a time varying solution. From
   // http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
-  //   x' = x (2 / (e^t + 1) - 1)
+  //   x' = x (2/(eᵗ + 1) - 1)
   //
   // The true (analytical) solution is:
   //
-  // x(t) = 12 * e^t / ((e^t + 1)^2)
+  // x(t) = 12eᵗ/(eᵗ + 1)²
   @Test
   void rungeKuttaTimeVaryingTest() {
     final var y0 = rungeKuttaTimeVaryingSolution(5.0);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java
index 1805589..fa2314e 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java
@@ -12,7 +12,6 @@
 import org.junit.jupiter.api.Test;
 
 class CentripetalAccelerationConstraintTest {
-  @SuppressWarnings("LocalVariableName")
   @Test
   void testCentripetalAccelerationConstraint() {
     double maxCentripetalAcceleration = Units.feetToMeters(7.0); // 7 feet per second squared
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java
index 4c8a8ba..bcc3c16 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java
@@ -15,7 +15,6 @@
 import org.junit.jupiter.api.Test;
 
 class DifferentialDriveKinematicsConstraintTest {
-  @SuppressWarnings("LocalVariableName")
   @Test
   void testDifferentialDriveKinematicsConstraint() {
     double maxVelocity = Units.feetToMeters(12.0); // 12 feet per second
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
index 87c1bd9..917edb4 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
@@ -20,7 +20,6 @@
 import org.junit.jupiter.api.Test;
 
 class DifferentialDriveVoltageConstraintTest {
-  @SuppressWarnings("LocalVariableName")
   @Test
   void testDifferentialDriveVoltageConstraint() {
     // Pick an unreasonably large kA to ensure the constraint has to do some work
diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java
index 97c1858..45ee1d3 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java
@@ -50,7 +50,6 @@
   }
 
   @Test
-  @SuppressWarnings("LocalVariableName")
   void testGenerationAndConstraints() {
     Trajectory trajectory = getTrajectory(new ArrayList<>());
 
diff --git a/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
new file mode 100644
index 0000000..e397af1
--- /dev/null
+++ b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
@@ -0,0 +1,18 @@
+// 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/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}};
+  frc::Transform3d cameraToObject{frc::Translation3d{1_m, 1_m, 1_m},
+                                  frc::Rotation3d{0_deg, -20_deg, 45_deg}};
+  frc::Transform3d robotToCamera{frc::Translation3d{1_m, 0_m, 2_m},
+                                 frc::Rotation3d{0_deg, 0_deg, 25_deg}};
+  frc::Pose3d object = robot + robotToCamera + cameraToObject;
+
+  EXPECT_EQ(robot,
+            frc::ObjectToRobotPose(object, cameraToObject, robotToCamera));
+}
diff --git a/wpimath/src/test/native/cpp/EigenTest.cpp b/wpimath/src/test/native/cpp/EigenTest.cpp
index c1786c3..03be9b3 100644
--- a/wpimath/src/test/native/cpp/EigenTest.cpp
+++ b/wpimath/src/test/native/cpp/EigenTest.cpp
@@ -2,34 +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 "Eigen/Core"
 #include "Eigen/LU"
+#include "frc/EigenCore.h"
 #include "gtest/gtest.h"
 
 TEST(EigenTest, Multiplication) {
-  Eigen::Matrix<double, 2, 2> m1{{2, 1}, {0, 1}};
-  Eigen::Matrix<double, 2, 2> m2{{3, 0}, {0, 2.5}};
+  frc::Matrixd<2, 2> m1{{2, 1}, {0, 1}};
+  frc::Matrixd<2, 2> m2{{3, 0}, {0, 2.5}};
 
   const auto result = m1 * m2;
 
-  Eigen::Matrix<double, 2, 2> expectedResult{{6.0, 2.5}, {0.0, 2.5}};
+  frc::Matrixd<2, 2> expectedResult{{6.0, 2.5}, {0.0, 2.5}};
 
   EXPECT_TRUE(expectedResult.isApprox(result));
 
-  Eigen::Matrix<double, 2, 3> m3{{1.0, 3.0, 0.5}, {2.0, 4.3, 1.2}};
-  Eigen::Matrix<double, 3, 4> m4{
+  frc::Matrixd<2, 3> m3{{1.0, 3.0, 0.5}, {2.0, 4.3, 1.2}};
+  frc::Matrixd<3, 4> m4{
       {3.0, 1.5, 2.0, 4.5}, {2.3, 1.0, 1.6, 3.1}, {5.2, 2.1, 2.0, 1.0}};
 
   const auto result2 = m3 * m4;
 
-  Eigen::Matrix<double, 2, 4> expectedResult2{{12.5, 5.55, 7.8, 14.3},
-                                              {22.13, 9.82, 13.28, 23.53}};
+  frc::Matrixd<2, 4> expectedResult2{{12.5, 5.55, 7.8, 14.3},
+                                     {22.13, 9.82, 13.28, 23.53}};
 
   EXPECT_TRUE(expectedResult2.isApprox(result2));
 }
 
 TEST(EigenTest, Transpose) {
-  Eigen::Vector<double, 3> vec{1, 2, 3};
+  frc::Vectord<3> vec{1, 2, 3};
 
   const auto transpose = vec.transpose();
 
@@ -39,8 +39,7 @@
 }
 
 TEST(EigenTest, Inverse) {
-  Eigen::Matrix<double, 3, 3> mat{
-      {1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}};
+  frc::Matrixd<3, 3> 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 cd7ef5c..0e2f77c 100644
--- a/wpimath/src/test/native/cpp/FormatterTest.cpp
+++ b/wpimath/src/test/native/cpp/FormatterTest.cpp
@@ -2,6 +2,8 @@
 // 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 <vector>
+
 #include <fmt/format.h>
 
 #include "frc/fmt/Eigen.h"
@@ -10,12 +12,33 @@
 #include "units/velocity.h"
 
 TEST(FormatterTest, Eigen) {
-  Eigen::Matrix<double, 3, 2> A{{1.0, 2.0}, {3.0, 4.0}, {5.0, 6.0}};
+  Eigen::Matrix<double, 3, 2> A{{0.0, 1.0}, {2.0, 3.0}, {4.0, 5.0}};
   EXPECT_EQ(
-      "  1.000000  2.000000\n"
-      "  3.000000  4.000000\n"
-      "  5.000000  6.000000",
+      "  0.000000  1.000000\n"
+      "  2.000000  3.000000\n"
+      "  4.000000  5.000000",
       fmt::format("{}", 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));
+
+  Eigen::SparseMatrix<double> C{3, 2};
+  std::vector<Eigen::Triplet<double>> triplets;
+  triplets.emplace_back(0, 1, 1.0);
+  triplets.emplace_back(1, 0, 2.0);
+  triplets.emplace_back(1, 1, 3.0);
+  triplets.emplace_back(2, 0, 4.0);
+  triplets.emplace_back(2, 1, 5.0);
+  C.setFromTriplets(triplets.begin(), triplets.end());
+  EXPECT_EQ(
+      "  0.000000  1.000000\n"
+      "  2.000000  3.000000\n"
+      "  4.000000  5.000000",
+      fmt::format("{}", C));
 }
 
 TEST(FormatterTest, Units) {
diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp
index 6b5af2b..a836a77 100644
--- a/wpimath/src/test/native/cpp/MathUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp
@@ -2,6 +2,8 @@
 // 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 <limits>
+
 #include "frc/MathUtil.h"
 #include "gtest/gtest.h"
 #include "units/angle.h"
@@ -10,7 +12,7 @@
 
 #define EXPECT_UNITS_NEAR(a, b, c) EXPECT_NEAR((a).value(), (b).value(), c)
 
-TEST(MathUtilTest, ApplyDeadband) {
+TEST(MathUtilTest, ApplyDeadbandUnityScale) {
   // < 0
   EXPECT_DOUBLE_EQ(-1.0, frc::ApplyDeadband(-1.0, 0.02));
   EXPECT_DOUBLE_EQ((-0.03 + 0.02) / (1.0 - 0.02),
@@ -29,6 +31,33 @@
   EXPECT_DOUBLE_EQ(1.0, frc::ApplyDeadband(1.0, 0.02));
 }
 
+TEST(MathUtilTest, ApplyDeadbandArbitraryScale) {
+  // < 0
+  EXPECT_DOUBLE_EQ(-2.5, frc::ApplyDeadband(-2.5, 0.02, 2.5));
+  EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.02, 0.02, 2.5));
+  EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.01, 0.02, 2.5));
+
+  // == 0
+  EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.0, 0.02, 2.5));
+
+  // > 0
+  EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.01, 0.02, 2.5));
+  EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.02, 0.02, 2.5));
+  EXPECT_DOUBLE_EQ(2.5, frc::ApplyDeadband(2.5, 0.02, 2.5));
+}
+
+TEST(MathUtilTest, ApplyDeadbandUnits) {
+  // < 0
+  EXPECT_DOUBLE_EQ(
+      -20, frc::ApplyDeadband<units::radian_t>(-20_rad, 1_rad, 20_rad).value());
+}
+
+TEST(MathUtilTest, ApplyDeadbandLargeMaxMagnitude) {
+  EXPECT_DOUBLE_EQ(
+      80.0,
+      frc::ApplyDeadband(100.0, 20.0, std::numeric_limits<double>::infinity()));
+}
+
 TEST(MathUtilTest, InputModulus) {
   // These tests check error wrapping. That is, the result of wrapping the
   // result of an angle reference minus the measurement.
@@ -71,20 +100,20 @@
 
 TEST(MathUtilTest, AngleModulus) {
   EXPECT_UNITS_NEAR(
-      frc::AngleModulus(units::radian_t{-2000 * wpi::numbers::pi / 180}),
-      units::radian_t{160 * wpi::numbers::pi / 180}, 1e-10);
+      frc::AngleModulus(units::radian_t{-2000 * std::numbers::pi / 180}),
+      units::radian_t{160 * std::numbers::pi / 180}, 1e-10);
   EXPECT_UNITS_NEAR(
-      frc::AngleModulus(units::radian_t{358 * wpi::numbers::pi / 180}),
-      units::radian_t{-2 * wpi::numbers::pi / 180}, 1e-10);
-  EXPECT_UNITS_NEAR(frc::AngleModulus(units::radian_t{2.0 * wpi::numbers::pi}),
+      frc::AngleModulus(units::radian_t{358 * std::numbers::pi / 180}),
+      units::radian_t{-2 * std::numbers::pi / 180}, 1e-10);
+  EXPECT_UNITS_NEAR(frc::AngleModulus(units::radian_t{2.0 * std::numbers::pi}),
                     0_rad, 1e-10);
 
-  EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(5 * wpi::numbers::pi)),
-                  units::radian_t(wpi::numbers::pi));
-  EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(-5 * wpi::numbers::pi)),
-                  units::radian_t(wpi::numbers::pi));
-  EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(wpi::numbers::pi / 2)),
-                  units::radian_t(wpi::numbers::pi / 2));
-  EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(-wpi::numbers::pi / 2)),
-                  units::radian_t(-wpi::numbers::pi / 2));
+  EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{5 * std::numbers::pi}),
+                  units::radian_t{std::numbers::pi});
+  EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{-5 * std::numbers::pi}),
+                  units::radian_t{std::numbers::pi});
+  EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{std::numbers::pi / 2}),
+                  units::radian_t{std::numbers::pi / 2});
+  EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{-std::numbers::pi / 2}),
+                  units::radian_t{-std::numbers::pi / 2});
 }
diff --git a/wpimath/src/test/native/cpp/StateSpaceTest.cpp b/wpimath/src/test/native/cpp/StateSpaceTest.cpp
index 0dc3978..ee7842c 100644
--- a/wpimath/src/test/native/cpp/StateSpaceTest.cpp
+++ b/wpimath/src/test/native/cpp/StateSpaceTest.cpp
@@ -7,7 +7,7 @@
 #include <cmath>
 #include <random>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/controller/LinearPlantInversionFeedforward.h"
 #include "frc/controller/LinearQuadraticRegulator.h"
 #include "frc/estimator/KalmanFilter.h"
@@ -45,8 +45,7 @@
 
 void Update(const LinearSystem<2, 1, 1>& plant, LinearSystemLoop<2, 1, 1>& loop,
             double noise) {
-  Eigen::Vector<double, 1> y =
-      plant.CalculateY(loop.Xhat(), loop.U()) + Eigen::Vector<double, 1>{noise};
+  Vectord<1> y = plant.CalculateY(loop.Xhat(), loop.U()) + Vectord<1>{noise};
   loop.Correct(y);
   loop.Predict(kDt);
 }
@@ -55,7 +54,7 @@
   std::default_random_engine generator;
   std::normal_distribution<double> dist{0.0, kPositionStddev};
 
-  Eigen::Vector<double, 2> references{2.0, 0.0};
+  Vectord<2> references{2.0, 0.0};
   loop.SetNextR(references);
 
   for (int i = 0; i < 1000; i++) {
diff --git a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
index 57b93bb..105a434 100644
--- a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
@@ -6,51 +6,12 @@
 
 #include <array>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/system/NumericalIntegration.h"
 
-TEST(StateSpaceUtilTest, MakeMatrix) {
-  // Column vector
-  Eigen::Vector<double, 2> mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0);
-  EXPECT_NEAR(mat1(0), 1.0, 1e-3);
-  EXPECT_NEAR(mat1(1), 2.0, 1e-3);
-
-  // Row vector
-  Eigen::RowVector<double, 2> mat2 = frc::MakeMatrix<1, 2>(1.0, 2.0);
-  EXPECT_NEAR(mat2(0), 1.0, 1e-3);
-  EXPECT_NEAR(mat2(1), 2.0, 1e-3);
-
-  // Square matrix
-  Eigen::Matrix<double, 2, 2> mat3 = frc::MakeMatrix<2, 2>(1.0, 2.0, 3.0, 4.0);
-  EXPECT_NEAR(mat3(0, 0), 1.0, 1e-3);
-  EXPECT_NEAR(mat3(0, 1), 2.0, 1e-3);
-  EXPECT_NEAR(mat3(1, 0), 3.0, 1e-3);
-  EXPECT_NEAR(mat3(1, 1), 4.0, 1e-3);
-
-  // Nonsquare matrix with more rows than columns
-  Eigen::Matrix<double, 3, 2> mat4 =
-      frc::MakeMatrix<3, 2>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
-  EXPECT_NEAR(mat4(0, 0), 1.0, 1e-3);
-  EXPECT_NEAR(mat4(0, 1), 2.0, 1e-3);
-  EXPECT_NEAR(mat4(1, 0), 3.0, 1e-3);
-  EXPECT_NEAR(mat4(1, 1), 4.0, 1e-3);
-  EXPECT_NEAR(mat4(2, 0), 5.0, 1e-3);
-  EXPECT_NEAR(mat4(2, 1), 6.0, 1e-3);
-
-  // Nonsquare matrix with more columns than rows
-  Eigen::Matrix<double, 2, 3> mat5 =
-      frc::MakeMatrix<2, 3>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
-  EXPECT_NEAR(mat5(0, 0), 1.0, 1e-3);
-  EXPECT_NEAR(mat5(0, 1), 2.0, 1e-3);
-  EXPECT_NEAR(mat5(0, 2), 3.0, 1e-3);
-  EXPECT_NEAR(mat5(1, 0), 4.0, 1e-3);
-  EXPECT_NEAR(mat5(1, 1), 5.0, 1e-3);
-  EXPECT_NEAR(mat5(1, 2), 6.0, 1e-3);
-}
-
 TEST(StateSpaceUtilTest, CostParameterPack) {
-  Eigen::Matrix<double, 3, 3> mat = frc::MakeCostMatrix(1.0, 2.0, 3.0);
+  frc::Matrixd<3, 3> mat = frc::MakeCostMatrix(1.0, 2.0, 3.0);
   EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
   EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
   EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -63,7 +24,7 @@
 }
 
 TEST(StateSpaceUtilTest, CostArray) {
-  Eigen::Matrix<double, 3, 3> mat = frc::MakeCostMatrix<3>({1.0, 2.0, 3.0});
+  frc::Matrixd<3, 3> mat = frc::MakeCostMatrix<3>({1.0, 2.0, 3.0});
   EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
   EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
   EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -76,7 +37,7 @@
 }
 
 TEST(StateSpaceUtilTest, CovParameterPack) {
-  Eigen::Matrix<double, 3, 3> mat = frc::MakeCovMatrix(1.0, 2.0, 3.0);
+  frc::Matrixd<3, 3> mat = frc::MakeCovMatrix(1.0, 2.0, 3.0);
   EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
   EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
   EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -89,7 +50,7 @@
 }
 
 TEST(StateSpaceUtilTest, CovArray) {
-  Eigen::Matrix<double, 3, 3> mat = frc::MakeCovMatrix<3>({1.0, 2.0, 3.0});
+  frc::Matrixd<3, 3> mat = frc::MakeCovMatrix<3>({1.0, 2.0, 3.0});
   EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
   EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
   EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -102,59 +63,59 @@
 }
 
 TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) {
-  Eigen::Vector<double, 2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
+  frc::Vectord<2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
   static_cast<void>(vec);
 }
 
 TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) {
-  Eigen::Vector<double, 2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
+  frc::Vectord<2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
   static_cast<void>(vec);
 }
 
 TEST(StateSpaceUtilTest, IsStabilizable) {
-  Eigen::Matrix<double, 2, 1> B{0, 1};
+  frc::Matrixd<2, 1> B{0, 1};
 
   // First eigenvalue is uncontrollable and unstable.
   // Second eigenvalue is controllable and stable.
-  EXPECT_FALSE((frc::IsStabilizable<2, 1>(
-      Eigen::Matrix<double, 2, 2>{{1.2, 0}, {0, 0.5}}, B)));
+  EXPECT_FALSE(
+      (frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, B)));
 
   // First eigenvalue is uncontrollable and marginally stable.
   // Second eigenvalue is controllable and stable.
-  EXPECT_FALSE((frc::IsStabilizable<2, 1>(
-      Eigen::Matrix<double, 2, 2>{{1, 0}, {0, 0.5}}, B)));
+  EXPECT_FALSE(
+      (frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, B)));
 
   // First eigenvalue is uncontrollable and stable.
   // Second eigenvalue is controllable and stable.
-  EXPECT_TRUE((frc::IsStabilizable<2, 1>(
-      Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 0.5}}, B)));
+  EXPECT_TRUE(
+      (frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, B)));
 
   // First eigenvalue is uncontrollable and stable.
   // Second eigenvalue is controllable and unstable.
-  EXPECT_TRUE((frc::IsStabilizable<2, 1>(
-      Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, B)));
+  EXPECT_TRUE(
+      (frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, B)));
 }
 
 TEST(StateSpaceUtilTest, IsDetectable) {
-  Eigen::Matrix<double, 1, 2> C{0, 1};
+  frc::Matrixd<1, 2> C{0, 1};
 
   // First eigenvalue is unobservable and unstable.
   // Second eigenvalue is observable and stable.
-  EXPECT_FALSE((frc::IsDetectable<2, 1>(
-      Eigen::Matrix<double, 2, 2>{{1.2, 0}, {0, 0.5}}, C)));
+  EXPECT_FALSE(
+      (frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, C)));
 
   // First eigenvalue is unobservable and marginally stable.
   // Second eigenvalue is observable and stable.
-  EXPECT_FALSE((frc::IsDetectable<2, 1>(
-      Eigen::Matrix<double, 2, 2>{{1, 0}, {0, 0.5}}, C)));
+  EXPECT_FALSE(
+      (frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, C)));
 
   // First eigenvalue is unobservable and stable.
   // Second eigenvalue is observable and stable.
-  EXPECT_TRUE((frc::IsDetectable<2, 1>(
-      Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 0.5}}, C)));
+  EXPECT_TRUE(
+      (frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, C)));
 
   // First eigenvalue is unobservable and stable.
   // Second eigenvalue is observable and unstable.
-  EXPECT_TRUE((frc::IsDetectable<2, 1>(
-      Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, C)));
+  EXPECT_TRUE(
+      (frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, C)));
 }
diff --git a/wpimath/src/test/native/cpp/UnitsTest.cpp b/wpimath/src/test/native/cpp/UnitsTest.cpp
index d5ba717..1059260 100644
--- a/wpimath/src/test/native/cpp/UnitsTest.cpp
+++ b/wpimath/src/test/native/cpp/UnitsTest.cpp
@@ -1420,6 +1420,7 @@
 }
 #endif
 
+#if !defined(UNIT_LIB_DISABLE_FMT)
 TEST_F(UnitContainer, fmtlib) {
   testing::internal::CaptureStdout();
   fmt::print("{}", degree_t(349.87));
@@ -1500,6 +1501,7 @@
   EXPECT_STREQ("5.670367e-08 kg s^-3 K^-4", output.c_str());
 #endif
 }
+#endif
 
 TEST_F(UnitContainer, to_string) {
   foot_t a(3.5);
@@ -3063,14 +3065,17 @@
   constexpr meter_t result0(0);
   constexpr auto result1 = make_unit<meter_t>(1);
   constexpr auto result2 = meter_t(2);
+  constexpr auto result3 = -3_m;
 
   EXPECT_EQ(meter_t(0), result0);
   EXPECT_EQ(meter_t(1), result1);
   EXPECT_EQ(meter_t(2), result2);
+  EXPECT_EQ(meter_t(-3), result3);
 
   EXPECT_TRUE(noexcept(result0));
   EXPECT_TRUE(noexcept(result1));
   EXPECT_TRUE(noexcept(result2));
+  EXPECT_TRUE(noexcept(result3));
 }
 
 TEST_F(Constexpr, constants) {
diff --git a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
index 354ed18..b2bcee6 100644
--- a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
@@ -6,47 +6,46 @@
 
 #include <cmath>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/controller/ControlAffinePlantInversionFeedforward.h"
 #include "units/time.h"
 
 namespace frc {
 
-Eigen::Vector<double, 2> Dynamics(const Eigen::Vector<double, 2>& x,
-                                  const Eigen::Vector<double, 1>& u) {
-  return Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x +
-         Eigen::Matrix<double, 2, 1>{0.0, 1.0} * u;
+Vectord<2> Dynamics(const Vectord<2>& x, const Vectord<1>& u) {
+  return Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x +
+         Matrixd<2, 1>{0.0, 1.0} * u;
 }
 
-Eigen::Vector<double, 2> StateDynamics(const Eigen::Vector<double, 2>& x) {
-  return Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x;
+Vectord<2> StateDynamics(const Vectord<2>& x) {
+  return Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x;
 }
 
 TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
-  std::function<Eigen::Vector<double, 2>(const Eigen::Vector<double, 2>&,
-                                         const Eigen::Vector<double, 1>&)>
+  std::function<Vectord<2>(const Vectord<2>&, const Vectord<1>&)>
       modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); };
 
   frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
       modelDynamics, units::second_t{0.02}};
 
-  Eigen::Vector<double, 2> r{2, 2};
-  Eigen::Vector<double, 2> nextR{3, 3};
+  Vectord<2> r{2, 2};
+  Vectord<2> nextR{3, 3};
 
   EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
 }
 
 TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) {
-  std::function<Eigen::Vector<double, 2>(const Eigen::Vector<double, 2>&)>
-      modelDynamics = [](auto& x) { return StateDynamics(x); };
+  std::function<Vectord<2>(const Vectord<2>&)> modelDynamics = [](auto& x) {
+    return StateDynamics(x);
+  };
 
-  Eigen::Matrix<double, 2, 1> B{0, 1};
+  Matrixd<2, 1> B{0, 1};
 
-  frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
-      modelDynamics, B, units::second_t(0.02)};
+  frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{modelDynamics,
+                                                                B, 20_ms};
 
-  Eigen::Vector<double, 2> r{2, 2};
-  Eigen::Vector<double, 2> nextR{3, 3};
+  Vectord<2> r{2, 2};
+  Vectord<2> nextR{3, 3};
 
   EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
 }
diff --git a/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp b/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp
new file mode 100644
index 0000000..ab8359c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp
@@ -0,0 +1,257 @@
+// 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/controller/DifferentialDriveAccelerationLimiter.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "units/math.h"
+
+namespace frc {
+
+TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
+  constexpr auto trackwidth = 0.9_m;
+  constexpr auto dt = 5_ms;
+  constexpr auto maxA = 2_mps_sq;
+  constexpr auto maxAlpha = 2_rad_per_s_sq;
+
+  using Kv_t = decltype(1_V / 1_mps);
+  using Ka_t = decltype(1_V / 1_mps_sq);
+  auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
+                                                        Kv_t{1.0}, Ka_t{1.0});
+
+  DifferentialDriveAccelerationLimiter accelLimiter{plant, trackwidth, maxA,
+                                                    maxAlpha};
+
+  Vectord<2> x{0.0, 0.0};
+  Vectord<2> xAccelLimiter{0.0, 0.0};
+
+  // Ensure voltage exceeds acceleration before limiting
+  {
+    Vectord<2> accels =
+        plant.A() * xAccelLimiter + plant.B() * Vectord<2>{12.0, 12.0};
+    units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
+    EXPECT_GT(units::math::abs(a), maxA);
+  }
+  {
+    Vectord<2> accels =
+        plant.A() * xAccelLimiter + plant.B() * Vectord<2>{-12.0, 12.0};
+    units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
+                                              trackwidth.value()};
+    EXPECT_GT(units::math::abs(alpha), maxAlpha);
+  }
+
+  // Forward
+  Vectord<2> u{12.0, 12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    auto [left, right] =
+        accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
+                               units::meters_per_second_t{xAccelLimiter(1)},
+                               units::volt_t{u(0)}, units::volt_t{u(1)});
+    xAccelLimiter =
+        plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
+
+    Vectord<2> accels =
+        plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
+    units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
+    units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
+                                              trackwidth.value()};
+    EXPECT_LE(units::math::abs(a), maxA);
+    EXPECT_LE(units::math::abs(alpha), maxAlpha);
+  }
+
+  // Backward
+  u = Vectord<2>{-12.0, -12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    auto [left, right] =
+        accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
+                               units::meters_per_second_t{xAccelLimiter(1)},
+                               units::volt_t{u(0)}, units::volt_t{u(1)});
+    xAccelLimiter =
+        plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
+
+    Vectord<2> accels =
+        plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
+    units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
+    units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
+                                              trackwidth.value()};
+    EXPECT_LE(units::math::abs(a), maxA);
+    EXPECT_LE(units::math::abs(alpha), maxAlpha);
+  }
+
+  // Rotate CCW
+  u = Vectord<2>{-12.0, 12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    auto [left, right] =
+        accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
+                               units::meters_per_second_t{xAccelLimiter(1)},
+                               units::volt_t{u(0)}, units::volt_t{u(1)});
+    xAccelLimiter =
+        plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
+
+    Vectord<2> accels =
+        plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
+    units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
+    units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
+                                              trackwidth.value()};
+    EXPECT_LE(units::math::abs(a), maxA);
+    EXPECT_LE(units::math::abs(alpha), maxAlpha);
+  }
+}
+
+TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
+  constexpr auto trackwidth = 0.9_m;
+  constexpr auto dt = 5_ms;
+
+  using Kv_t = decltype(1_V / 1_mps);
+  using Ka_t = decltype(1_V / 1_mps_sq);
+
+  auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
+                                                        Kv_t{1.0}, Ka_t{1.0});
+
+  // Limits are so high, they don't get hit, so states of constrained and
+  // unconstrained systems should match
+  DifferentialDriveAccelerationLimiter accelLimiter{
+      plant, trackwidth, 1e3_mps_sq, 1e3_rad_per_s_sq};
+
+  Vectord<2> x{0.0, 0.0};
+  Vectord<2> xAccelLimiter{0.0, 0.0};
+
+  // Forward
+  Vectord<2> u{12.0, 12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    auto [left, right] =
+        accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
+                               units::meters_per_second_t{xAccelLimiter(1)},
+                               units::volt_t{u(0)}, units::volt_t{u(1)});
+    xAccelLimiter =
+        plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
+
+    EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0));
+    EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1));
+  }
+
+  // Backward
+  x.setZero();
+  xAccelLimiter.setZero();
+  u = Vectord<2>{-12.0, -12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    auto [left, right] =
+        accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
+                               units::meters_per_second_t{xAccelLimiter(1)},
+                               units::volt_t{u(0)}, units::volt_t{u(1)});
+    xAccelLimiter =
+        plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
+
+    EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0));
+    EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1));
+  }
+
+  // Rotate CCW
+  x.setZero();
+  xAccelLimiter.setZero();
+  u = Vectord<2>{-12.0, 12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    auto [left, right] =
+        accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
+                               units::meters_per_second_t{xAccelLimiter(1)},
+                               units::volt_t{u(0)}, units::volt_t{u(1)});
+    xAccelLimiter =
+        plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
+
+    EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0));
+    EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1));
+  }
+}
+
+TEST(DifferentialDriveAccelerationLimiterTest, SeperateMinMaxLowLimits) {
+  constexpr auto trackwidth = 0.9_m;
+  constexpr auto dt = 5_ms;
+  constexpr auto minA = -1_mps_sq;
+  constexpr auto maxA = 2_mps_sq;
+  constexpr auto maxAlpha = 2_rad_per_s_sq;
+
+  using Kv_t = decltype(1_V / 1_mps);
+  using Ka_t = decltype(1_V / 1_mps_sq);
+  auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
+                                                        Kv_t{1.0}, Ka_t{1.0});
+
+  DifferentialDriveAccelerationLimiter accelLimiter{plant, trackwidth, minA,
+                                                    maxA, maxAlpha};
+
+  Vectord<2> x{0.0, 0.0};
+  Vectord<2> xAccelLimiter{0.0, 0.0};
+
+  // Ensure voltage exceeds acceleration before limiting
+  {
+    Vectord<2> accels =
+        plant.A() * xAccelLimiter + plant.B() * Vectord<2>{12.0, 12.0};
+    units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
+    EXPECT_GT(units::math::abs(a), maxA);
+    EXPECT_GT(units::math::abs(a), -minA);
+  }
+
+  // a should always be within [minA, maxA]
+  // Forward
+  Vectord<2> u{12.0, 12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    auto [left, right] =
+        accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
+                               units::meters_per_second_t{xAccelLimiter(1)},
+                               units::volt_t{u(0)}, units::volt_t{u(1)});
+    xAccelLimiter =
+        plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
+
+    Vectord<2> accels =
+        plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
+    units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
+    EXPECT_GE(a, minA);
+    EXPECT_LE(a, maxA);
+  }
+
+  // Backward
+  u = Vectord<2>{-12.0, -12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    auto [left, right] =
+        accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
+                               units::meters_per_second_t{xAccelLimiter(1)},
+                               units::volt_t{u(0)}, units::volt_t{u(1)});
+    xAccelLimiter =
+        plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
+
+    Vectord<2> accels =
+        plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
+    units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
+    EXPECT_GE(a, minA);
+    EXPECT_LE(a, maxA);
+  }
+}
+
+TEST(DifferentialDriveAccelerationLimiterTest, MinAccelGreaterThanMaxAccel) {
+  using Kv_t = decltype(1_V / 1_mps);
+  using Ka_t = decltype(1_V / 1_mps_sq);
+  auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
+                                                        Kv_t{1.0}, Ka_t{1.0});
+  EXPECT_NO_THROW({
+    DifferentialDriveAccelerationLimiter accelLimiter(plant, 1_m, 1_mps_sq,
+                                                      1_rad_per_s_sq);
+  });
+
+  EXPECT_THROW(
+      {
+        DifferentialDriveAccelerationLimiter accelLimiter(
+            plant, 1_m, 1_mps_sq, -1_mps_sq, 1_rad_per_s_sq);
+      },
+      std::invalid_argument);
+}
+
+}  // namespace frc
diff --git a/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp
new file mode 100644
index 0000000..74d945c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp
@@ -0,0 +1,84 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include "frc/EigenCore.h"
+#include "frc/controller/DifferentialDriveFeedforward.h"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "units/acceleration.h"
+#include "units/length.h"
+#include "units/time.h"
+
+TEST(DifferentialDriveFeedforwardTest, CalculateWithTrackwidth) {
+  constexpr auto kVLinear = 1_V / 1_mps;
+  constexpr auto kALinear = 1_V / 1_mps_sq;
+  constexpr auto kVAngular = 1_V / 1_rad_per_s;
+  constexpr auto kAAngular = 1_V / 1_rad_per_s_sq;
+  constexpr auto trackwidth = 1_m;
+  constexpr auto dt = 20_ms;
+
+  frc::DifferentialDriveFeedforward differentialDriveFeedforward{
+      kVLinear, kALinear, kVAngular, kAAngular, trackwidth};
+  frc::LinearSystem<2, 2, 2> plant =
+      frc::LinearSystemId::IdentifyDrivetrainSystem(
+          kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
+  for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps;
+       currentLeftVelocity += 2_mps) {
+    for (auto currentRightVelocity = -4_mps; currentRightVelocity <= 4_mps;
+         currentRightVelocity += 2_mps) {
+      for (auto nextLeftVelocity = -4_mps; nextLeftVelocity <= 4_mps;
+           nextLeftVelocity += 2_mps) {
+        for (auto nextRightVelocity = -4_mps; nextRightVelocity <= 4_mps;
+             nextRightVelocity += 2_mps) {
+          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);
+          EXPECT_NEAR(nextX(0), nextLeftVelocity.value(), 1e-6);
+          EXPECT_NEAR(nextX(1), nextRightVelocity.value(), 1e-6);
+        }
+      }
+    }
+  }
+}
+
+TEST(DifferentialDriveFeedforwardTest, CalculateWithoutTrackwidth) {
+  constexpr auto kVLinear = 1_V / 1_mps;
+  constexpr auto kALinear = 1_V / 1_mps_sq;
+  constexpr auto kVAngular = 1_V / 1_mps;
+  constexpr auto kAAngular = 1_V / 1_mps_sq;
+  constexpr auto dt = 20_ms;
+
+  frc::DifferentialDriveFeedforward differentialDriveFeedforward{
+      kVLinear, kALinear, kVAngular, kAAngular};
+  frc::LinearSystem<2, 2, 2> plant =
+      frc::LinearSystemId::IdentifyDrivetrainSystem(kVLinear, kALinear,
+                                                    kVAngular, kAAngular);
+  for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps;
+       currentLeftVelocity += 2_mps) {
+    for (auto currentRightVelocity = -4_mps; currentRightVelocity <= 4_mps;
+         currentRightVelocity += 2_mps) {
+      for (auto nextLeftVelocity = -4_mps; nextLeftVelocity <= 4_mps;
+           nextLeftVelocity += 2_mps) {
+        for (auto nextRightVelocity = -4_mps; nextRightVelocity <= 4_mps;
+             nextRightVelocity += 2_mps) {
+          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);
+          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/HolonomicDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
index c6b669c..134a4e0 100644
--- a/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.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 <wpi/numbers>
+#include <numbers>
 
 #include "frc/MathUtil.h"
 #include "frc/controller/HolonomicDriveController.h"
@@ -16,7 +16,7 @@
   EXPECT_LE(units::math::abs(val1 - val2), eps)
 
 static constexpr units::meter_t kTolerance{1 / 12.0};
-static constexpr units::radian_t kAngularTolerance{2.0 * wpi::numbers::pi /
+static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
                                                    180.0};
 
 TEST(HolonomicDriveControllerTest, ReachesReference) {
@@ -25,10 +25,10 @@
       frc::ProfiledPIDController<units::radian>{
           1.0, 0.0, 0.0,
           frc::TrapezoidProfile<units::radian>::Constraints{
-              units::radians_per_second_t{2.0 * wpi::numbers::pi},
-              units::radians_per_second_squared_t{wpi::numbers::pi}}}};
+              units::radians_per_second_t{2.0 * std::numbers::pi},
+              units::radians_per_second_squared_t{std::numbers::pi}}}};
 
-  frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
+  frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
 
   auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
                                frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
@@ -60,7 +60,7 @@
               4_rad_per_s, 2_rad_per_s / 1_s}}};
 
   frc::ChassisSpeeds speeds = controller.Calculate(
-      frc::Pose2d(0_m, 0_m, 1.57_rad), frc::Pose2d(), 0_mps, 1.57_rad);
+      frc::Pose2d{0_m, 0_m, 1.57_rad}, frc::Pose2d{}, 0_mps, 1.57_rad);
 
   EXPECT_EQ(0, speeds.omega.value());
 }
diff --git a/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp
new file mode 100644
index 0000000..afa491a
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp
@@ -0,0 +1,109 @@
+// 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/controller/ImplicitModelFollower.h"
+#include "frc/system/plant/LinearSystemId.h"
+
+namespace frc {
+
+TEST(ImplicitModelFollowerTest, SameModel) {
+  constexpr auto dt = 5_ms;
+
+  using Kv_t = decltype(1_V / 1_mps);
+  using Ka_t = decltype(1_V / 1_mps_sq);
+  auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
+                                                        Kv_t{1.0}, Ka_t{1.0});
+
+  ImplicitModelFollower<2, 2> imf{plant, plant};
+
+  Vectord<2> x{0.0, 0.0};
+  Vectord<2> xImf{0.0, 0.0};
+
+  // Forward
+  Vectord<2> u{12.0, 12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+    EXPECT_DOUBLE_EQ(x(0), xImf(0));
+    EXPECT_DOUBLE_EQ(x(1), xImf(1));
+  }
+
+  // Backward
+  u = Vectord<2>{-12.0, -12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+    EXPECT_DOUBLE_EQ(x(0), xImf(0));
+    EXPECT_DOUBLE_EQ(x(1), xImf(1));
+  }
+
+  // Rotate CCW
+  u = Vectord<2>{-12.0, 12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+    EXPECT_DOUBLE_EQ(x(0), xImf(0));
+    EXPECT_DOUBLE_EQ(x(1), xImf(1));
+  }
+}
+
+TEST(ImplicitModelFollowerTest, SlowerRefModel) {
+  constexpr auto dt = 5_ms;
+
+  using Kv_t = decltype(1_V / 1_mps);
+  using Ka_t = decltype(1_V / 1_mps_sq);
+
+  auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
+                                                        Kv_t{1.0}, Ka_t{1.0});
+
+  // Linear acceleration is slower, but angular acceleration is the same
+  auto plantRef = LinearSystemId::IdentifyDrivetrainSystem(
+      Kv_t{1.0}, Ka_t{2.0}, Kv_t{1.0}, Ka_t{1.0});
+
+  ImplicitModelFollower<2, 2> imf{plant, plantRef};
+
+  Vectord<2> x{0.0, 0.0};
+  Vectord<2> xImf{0.0, 0.0};
+
+  // Forward
+  Vectord<2> u{12.0, 12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+    EXPECT_GE(x(0), xImf(0));
+    EXPECT_GE(x(1), xImf(1));
+  }
+
+  // Backward
+  x.setZero();
+  xImf.setZero();
+  u = Vectord<2>{-12.0, -12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+    EXPECT_LE(x(0), xImf(0));
+    EXPECT_LE(x(1), xImf(1));
+  }
+
+  // Rotate CCW
+  x.setZero();
+  xImf.setZero();
+  u = Vectord<2>{-12.0, 12.0};
+  for (auto t = 0_s; t < 3_s; t += dt) {
+    x = plant.CalculateX(x, u, dt);
+    xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
+
+    EXPECT_NEAR(x(0), xImf(0), 1e-5);
+    EXPECT_NEAR(x(1), xImf(1), 1e-5);
+  }
+}
+
+}  // namespace frc
diff --git a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
new file mode 100644
index 0000000..753ea34
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
@@ -0,0 +1,101 @@
+// 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 "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) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+static constexpr units::meter_t kTolerance{1 / 12.0};
+static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
+                                                   180.0};
+
+/**
+ * States of the drivetrain system.
+ */
+class State {
+ public:
+  /// X position in global coordinate frame.
+  static constexpr int kX = 0;
+
+  /// Y position in global coordinate frame.
+  static constexpr int kY = 1;
+
+  /// Heading in global coordinate frame.
+  static constexpr int kHeading = 2;
+
+  /// Left encoder velocity.
+  static constexpr int kLeftVelocity = 3;
+
+  /// Right encoder velocity.
+  static constexpr int kRightVelocity = 4;
+};
+
+static constexpr auto kLinearV = 3.02_V / 1_mps;
+static constexpr auto kLinearA = 0.642_V / 1_mps_sq;
+static constexpr auto kAngularV = 1.382_V / 1_mps;
+static constexpr auto kAngularA = 0.08495_V / 1_mps_sq;
+static auto plant = frc::LinearSystemId::IdentifyDrivetrainSystem(
+    kLinearV, kLinearA, kAngularV, kAngularA);
+static constexpr auto kTrackwidth = 0.9_m;
+
+frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
+  double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0;
+
+  frc::Vectord<5> xdot;
+  xdot(0) = v * std::cos(x(State::kHeading));
+  xdot(1) = v * std::sin(x(State::kHeading));
+  xdot(2) = ((x(State::kRightVelocity) - x(State::kLeftVelocity)) / kTrackwidth)
+                .value();
+  xdot.block<2, 1>(3, 0) = plant.A() * x.block<2, 1>(3, 0) + plant.B() * u;
+  return xdot;
+}
+
+TEST(LTVDifferentialDriveControllerTest, ReachesReference) {
+  constexpr auto kDt = 0.02_s;
+
+  frc::LTVDifferentialDriveController controller{
+      plant, kTrackwidth, {0.0625, 0.125, 2.5, 0.95, 0.95}, {12.0, 12.0}, kDt};
+  frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
+
+  auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
+                               frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      waypoints, {8.8_mps, 0.1_mps_sq});
+
+  frc::Vectord<5> x = frc::Vectord<5>::Zero();
+  x(State::kX) = robotPose.X().value();
+  x(State::kY) = robotPose.Y().value();
+  x(State::kHeading) = robotPose.Rotation().Radians().value();
+
+  auto totalTime = trajectory.TotalTime();
+  for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
+    auto state = trajectory.Sample(kDt * i);
+    robotPose =
+        frc::Pose2d{units::meter_t{x(State::kX)}, units::meter_t{x(State::kY)},
+                    units::radian_t{x(State::kHeading)}};
+    auto [leftVoltage, rightVoltage] = controller.Calculate(
+        robotPose, units::meters_per_second_t{x(State::kLeftVelocity)},
+        units::meters_per_second_t{x(State::kRightVelocity)}, state);
+
+    x = frc::RKDP(&Dynamics, x,
+                  frc::Vectord<2>{leftVoltage.value(), rightVoltage.value()},
+                  kDt);
+  }
+
+  auto& endPose = trajectory.States().back().pose;
+  EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
+  EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
+  EXPECT_NEAR_UNITS(frc::AngleModulus(endPose.Rotation().Radians() -
+                                      robotPose.Rotation().Radians()),
+                    0_rad, kAngularTolerance);
+}
diff --git a/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp
new file mode 100644
index 0000000..56faf1d
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.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 "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) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+static constexpr units::meter_t kTolerance{1 / 12.0};
+static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
+                                                   180.0};
+
+TEST(LTVUnicycleControllerTest, ReachesReference) {
+  constexpr auto kDt = 0.02_s;
+
+  frc::LTVUnicycleController controller{{0.0625, 0.125, 2.5}, {4.0, 4.0}, kDt};
+  frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
+
+  auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
+                               frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      waypoints, {8.8_mps, 0.1_mps_sq});
+
+  auto totalTime = trajectory.TotalTime();
+  for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
+    auto state = trajectory.Sample(kDt * i);
+    auto [vx, vy, omega] = controller.Calculate(robotPose, state);
+    static_cast<void>(vy);
+
+    robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, 0_m, omega * kDt});
+  }
+
+  auto& endPose = trajectory.States().back().pose;
+  EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
+  EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
+  EXPECT_NEAR_UNITS(frc::AngleModulus(endPose.Rotation().Radians() -
+                                      robotPose.Rotation().Radians()),
+                    0_rad, kAngularTolerance);
+}
diff --git a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
index 6e61706..e7107d0 100644
--- a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
@@ -6,21 +6,20 @@
 
 #include <cmath>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/controller/LinearPlantInversionFeedforward.h"
 #include "units/time.h"
 
 namespace frc {
 
 TEST(LinearPlantInversionFeedforwardTest, Calculate) {
-  Eigen::Matrix<double, 2, 2> A{{1, 0}, {0, 1}};
-  Eigen::Matrix<double, 2, 1> B{0, 1};
+  Matrixd<2, 2> A{{1, 0}, {0, 1}};
+  Matrixd<2, 1> B{0, 1};
 
-  frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B,
-                                                         units::second_t(0.02)};
+  frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B, 20_ms};
 
-  Eigen::Vector<double, 2> r{2, 2};
-  Eigen::Vector<double, 2> nextR{3, 3};
+  Vectord<2> r{2, 2};
+  Vectord<2> nextR{3, 3};
 
   EXPECT_NEAR(47.502599, feedforward.Calculate(r, nextR)(0, 0), 0.002);
 }
diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
index 7055530..1127fc2 100644
--- a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
@@ -6,7 +6,7 @@
 
 #include <cmath>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/controller/LinearQuadraticRegulator.h"
 #include "frc/system/LinearSystem.h"
 #include "frc/system/plant/DCMotor.h"
@@ -30,7 +30,7 @@
 
     return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
   }();
-  Eigen::Matrix<double, 1, 2> K =
+  Matrixd<1, 2> K =
       LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5.05_ms}.K();
 
   EXPECT_NEAR(522.15314269, K(0, 0), 1e-6);
@@ -54,7 +54,7 @@
         motors, 1.0 / 3.0 * m * r * r, G);
   }();
 
-  Eigen::Matrix<double, 1, 2> K =
+  Matrixd<1, 2> K =
       LinearQuadraticRegulator<2, 1>{plant, {0.01745, 0.08726}, {12.0}, 5.05_ms}
           .K();
 
@@ -77,7 +77,7 @@
 
     return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
   }();
-  Eigen::Matrix<double, 1, 2> K =
+  Matrixd<1, 2> K =
       LinearQuadraticRegulator<2, 1>{plant, {0.1, 0.2}, {12.0}, 20_ms}.K();
 
   EXPECT_NEAR(10.38, K(0, 0), 1e-1);
@@ -99,50 +99,44 @@
  * @param dt Discretization timestep.
  */
 template <int States, int Inputs>
-Eigen::Matrix<double, Inputs, States> GetImplicitModelFollowingK(
-    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, States>& Aref, units::second_t dt) {
+Matrixd<Inputs, States> GetImplicitModelFollowingK(
+    const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
+    const Matrixd<States, States>& Q, const Matrixd<Inputs, Inputs>& R,
+    const Matrixd<States, States>& Aref, units::second_t dt) {
   // Discretize real dynamics
-  Eigen::Matrix<double, States, States> discA;
-  Eigen::Matrix<double, States, Inputs> discB;
+  Matrixd<States, States> discA;
+  Matrixd<States, Inputs> discB;
   DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
 
   // Discretize desired dynamics
-  Eigen::Matrix<double, States, States> discAref;
+  Matrixd<States, States> discAref;
   DiscretizeA<States>(Aref, dt, &discAref);
 
-  Eigen::Matrix<double, States, States> Qimf =
+  Matrixd<States, States> Qimf =
       (discA - discAref).transpose() * Q * (discA - discAref);
-  Eigen::Matrix<double, Inputs, Inputs> Rimf =
-      discB.transpose() * Q * discB + R;
-  Eigen::Matrix<double, States, Inputs> Nimf =
-      (discA - discAref).transpose() * Q * discB;
+  Matrixd<Inputs, Inputs> Rimf = discB.transpose() * Q * discB + R;
+  Matrixd<States, Inputs> Nimf = (discA - discAref).transpose() * Q * discB;
 
   return LinearQuadraticRegulator<States, Inputs>{A, B, Qimf, Rimf, Nimf, dt}
       .K();
 }
 
 TEST(LinearQuadraticRegulatorTest, MatrixOverloadsWithSingleIntegrator) {
-  Eigen::Matrix<double, 2, 2> A{Eigen::Matrix<double, 2, 2>::Zero()};
-  Eigen::Matrix<double, 2, 2> B{Eigen::Matrix<double, 2, 2>::Identity()};
-  Eigen::Matrix<double, 2, 2> Q{Eigen::Matrix<double, 2, 2>::Identity()};
-  Eigen::Matrix<double, 2, 2> R{Eigen::Matrix<double, 2, 2>::Identity()};
+  Matrixd<2, 2> A{Matrixd<2, 2>::Zero()};
+  Matrixd<2, 2> B{Matrixd<2, 2>::Identity()};
+  Matrixd<2, 2> Q{Matrixd<2, 2>::Identity()};
+  Matrixd<2, 2> R{Matrixd<2, 2>::Identity()};
 
   // QR overload
-  Eigen::Matrix<double, 2, 2> K =
-      LinearQuadraticRegulator<2, 2>{A, B, Q, R, 5_ms}.K();
+  Matrixd<2, 2> K = LinearQuadraticRegulator<2, 2>{A, B, Q, R, 5_ms}.K();
   EXPECT_NEAR(0.99750312499512261, K(0, 0), 1e-10);
   EXPECT_NEAR(0.0, K(0, 1), 1e-10);
   EXPECT_NEAR(0.0, K(1, 0), 1e-10);
   EXPECT_NEAR(0.99750312499512261, K(1, 1), 1e-10);
 
   // QRN overload
-  Eigen::Matrix<double, 2, 2> N{Eigen::Matrix<double, 2, 2>::Identity()};
-  Eigen::Matrix<double, 2, 2> Kimf =
-      LinearQuadraticRegulator<2, 2>{A, B, Q, R, N, 5_ms}.K();
+  Matrixd<2, 2> N{Matrixd<2, 2>::Identity()};
+  Matrixd<2, 2> Kimf = LinearQuadraticRegulator<2, 2>{A, B, Q, R, N, 5_ms}.K();
   EXPECT_NEAR(1.0, Kimf(0, 0), 1e-10);
   EXPECT_NEAR(0.0, Kimf(0, 1), 1e-10);
   EXPECT_NEAR(0.0, Kimf(1, 0), 1e-10);
@@ -153,21 +147,19 @@
   double Kv = 3.02;
   double Ka = 0.642;
 
-  Eigen::Matrix<double, 2, 2> A{{0, 1}, {0, -Kv / Ka}};
-  Eigen::Matrix<double, 2, 1> B{{0}, {1.0 / Ka}};
-  Eigen::Matrix<double, 2, 2> Q{{1, 0}, {0, 0.2}};
-  Eigen::Matrix<double, 1, 1> R{0.25};
+  Matrixd<2, 2> A{{0, 1}, {0, -Kv / Ka}};
+  Matrixd<2, 1> B{{0}, {1.0 / Ka}};
+  Matrixd<2, 2> Q{{1, 0}, {0, 0.2}};
+  Matrixd<1, 1> R{0.25};
 
   // QR overload
-  Eigen::Matrix<double, 1, 2> K =
-      LinearQuadraticRegulator<2, 1>{A, B, Q, R, 5_ms}.K();
+  Matrixd<1, 2> K = LinearQuadraticRegulator<2, 1>{A, B, Q, R, 5_ms}.K();
   EXPECT_NEAR(1.9960017786537287, K(0, 0), 1e-10);
   EXPECT_NEAR(0.51182128351092726, K(0, 1), 1e-10);
 
   // QRN overload
-  Eigen::Matrix<double, 2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 2.0)}};
-  Eigen::Matrix<double, 1, 2> Kimf =
-      GetImplicitModelFollowingK<2, 1>(A, B, Q, R, Aref, 5_ms);
+  Matrixd<2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 2.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);
 }
diff --git a/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
index 379db9e..f1034f5 100644
--- a/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
@@ -9,7 +9,7 @@
  protected:
   frc2::PIDController* controller;
 
-  void SetUp() override { controller = new frc2::PIDController(0, 0, 0); }
+  void SetUp() override { controller = new frc2::PIDController{0, 0, 0}; }
 
   void TearDown() override { delete controller; }
 };
diff --git a/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
index da402ae..44d1f41 100644
--- a/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.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 <wpi/numbers>
+#include <numbers>
 
 #include "frc/controller/ProfiledPIDController.h"
 #include "gtest/gtest.h"
@@ -40,8 +40,8 @@
 
 TEST_F(ProfiledPIDInputOutputTest, ContinuousInput2) {
   controller->SetP(1);
-  controller->EnableContinuousInput(-units::radian_t{wpi::numbers::pi},
-                                    units::radian_t{wpi::numbers::pi});
+  controller->EnableContinuousInput(-units::radian_t{std::numbers::pi},
+                                    units::radian_t{std::numbers::pi});
 
   static constexpr units::radian_t kSetpoint{-3.4826633343199735};
   static constexpr units::radian_t kMeasurement{-3.1352207333939606};
@@ -52,13 +52,13 @@
 
   // Error must be less than half the input range at all times
   EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
-            units::radian_t{wpi::numbers::pi});
+            units::radian_t{std::numbers::pi});
 }
 
 TEST_F(ProfiledPIDInputOutputTest, ContinuousInput3) {
   controller->SetP(1);
-  controller->EnableContinuousInput(-units::radian_t{wpi::numbers::pi},
-                                    units::radian_t{wpi::numbers::pi});
+  controller->EnableContinuousInput(-units::radian_t{std::numbers::pi},
+                                    units::radian_t{std::numbers::pi});
 
   static constexpr units::radian_t kSetpoint{-3.5176604690006377};
   static constexpr units::radian_t kMeasurement{3.1191729343822456};
@@ -69,13 +69,13 @@
 
   // Error must be less than half the input range at all times
   EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
-            units::radian_t{wpi::numbers::pi});
+            units::radian_t{std::numbers::pi});
 }
 
 TEST_F(ProfiledPIDInputOutputTest, ContinuousInput4) {
   controller->SetP(1);
   controller->EnableContinuousInput(0_rad,
-                                    units::radian_t{2.0 * wpi::numbers::pi});
+                                    units::radian_t{2.0 * std::numbers::pi});
 
   static constexpr units::radian_t kSetpoint{2.78};
   static constexpr units::radian_t kMeasurement{3.12};
@@ -86,7 +86,7 @@
 
   // Error must be less than half the input range at all times
   EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
-            units::radian_t{wpi::numbers::pi});
+            units::radian_t{std::numbers::pi});
 }
 
 TEST_F(ProfiledPIDInputOutputTest, ProportionalGainOutput) {
diff --git a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
index 8829deb..2fd26bb 100644
--- a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
@@ -12,13 +12,13 @@
   EXPECT_LE(units::math::abs(val1 - val2), eps)
 
 static constexpr units::meter_t kTolerance{1 / 12.0};
-static constexpr units::radian_t kAngularTolerance{2.0 * wpi::numbers::pi /
+static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
                                                    180.0};
 
 TEST(RamseteControllerTest, ReachesReference) {
   frc::RamseteController controller{2.0 * 1_rad * 1_rad / (1_m * 1_m),
                                     0.7 / 1_rad};
-  frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
+  frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
 
   auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
                                frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
diff --git a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
index 3cf944e..6d10e05 100644
--- a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
+++ b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
@@ -6,7 +6,7 @@
 
 #include <cmath>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/controller/LinearPlantInversionFeedforward.h"
 #include "frc/controller/SimpleMotorFeedforward.h"
 #include "units/acceleration.h"
@@ -21,16 +21,16 @@
   double Ka = 0.6;
   auto dt = 0.02_s;
 
-  Eigen::Matrix<double, 1, 1> A{-Kv / Ka};
-  Eigen::Matrix<double, 1, 1> B{1.0 / Ka};
+  Matrixd<1, 1> A{-Kv / Ka};
+  Matrixd<1, 1> B{1.0 / Ka};
 
   frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
   frc::SimpleMotorFeedforward<units::meter> simpleMotor{
       units::volt_t{Ks}, units::volt_t{Kv} / 1_mps,
       units::volt_t{Ka} / 1_mps_sq};
 
-  Eigen::Vector<double, 1> r{2.0};
-  Eigen::Vector<double, 1> nextR{3.0};
+  Vectord<1> r{2.0};
+  Vectord<1> nextR{3.0};
 
   EXPECT_NEAR(37.524995834325161 + Ks,
               simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002);
diff --git a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
index ee1da7f..b2ee87d 100644
--- a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
@@ -4,15 +4,15 @@
 
 #include <gtest/gtest.h>
 
-#include <wpi/numbers>
+#include <numbers>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "frc/estimator/AngleStatistics.h"
 
 TEST(AngleStatisticsTest, Mean) {
-  Eigen::Matrix<double, 3, 3> sigmas{
+  frc::Matrixd<3, 3> sigmas{
       {1, 1.2, 0},
-      {359 * wpi::numbers::pi / 180, 3 * wpi::numbers::pi / 180, 0},
+      {359 * std::numbers::pi / 180, 3 * std::numbers::pi / 180, 0},
       {1, 2, 0}};
   // Weights need to produce the mean of the sigmas
   Eigen::Vector3d weights;
@@ -23,16 +23,16 @@
 }
 
 TEST(AngleStatisticsTest, Residual) {
-  Eigen::Vector3d a{1, 1 * wpi::numbers::pi / 180, 2};
-  Eigen::Vector3d b{1, 359 * wpi::numbers::pi / 180, 1};
+  Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2};
+  Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1};
 
   EXPECT_TRUE(frc::AngleResidual<3>(a, b, 1).isApprox(
-      Eigen::Vector3d{0, 2 * wpi::numbers::pi / 180, 1}));
+      Eigen::Vector3d{0, 2 * std::numbers::pi / 180, 1}));
 }
 
 TEST(AngleStatisticsTest, Add) {
-  Eigen::Vector3d a{1, 1 * wpi::numbers::pi / 180, 2};
-  Eigen::Vector3d b{1, 359 * wpi::numbers::pi / 180, 1};
+  Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2};
+  Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1};
 
   EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
 }
diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
index 4a854fd..1d623ca 100644
--- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
@@ -4,81 +4,103 @@
 
 #include <limits>
 #include <random>
+#include <tuple>
+#include <utility>
 
 #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/kinematics/DifferentialDriveOdometry.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
 #include "gtest/gtest.h"
 #include "units/angle.h"
 #include "units/length.h"
 #include "units/time.h"
 
-TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
-  frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d(),
-                                                frc::Pose2d(),
-                                                {0.02, 0.02, 0.01, 0.02, 0.02},
-                                                {0.01, 0.01, 0.001},
-                                                {0.1, 0.1, 0.01}};
+void testFollowTrajectory(
+    const frc::DifferentialDriveKinematics& kinematics,
+    frc::DifferentialDrivePoseEstimator& estimator,
+    const frc::Trajectory& trajectory,
+    std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
+        chassisSpeedsGenerator,
+    std::function<frc::Pose2d(frc::Trajectory::State&)>
+        visionMeasurementGenerator,
+    const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
+    const units::second_t dt, const units::second_t kVisionUpdateRate,
+    const units::second_t kVisionUpdateDelay, const bool checkError,
+    const bool debug) {
+  units::meter_t leftDistance = 0_m;
+  units::meter_t rightDistance = 0_m;
 
-  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
-                  frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
-                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
-                  frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
-                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
-      frc::TrajectoryConfig(10_mps, 5.0_mps_sq));
-
-  frc::DifferentialDriveKinematics kinematics{1.0_m};
-  frc::DifferentialDriveOdometry odometry{frc::Rotation2d()};
+  estimator.ResetPosition(frc::Rotation2d{}, leftDistance, rightDistance,
+                          startingPose);
 
   std::default_random_engine generator;
   std::normal_distribution<double> distribution(0.0, 1.0);
 
-  units::second_t dt = 0.02_s;
-  units::second_t t = 0.0_s;
+  units::second_t t = 0_s;
 
-  units::meter_t leftDistance = 0_m;
-  units::meter_t rightDistance = 0_m;
-
-  units::second_t kVisionUpdateRate = 0.1_s;
-  frc::Pose2d lastVisionPose;
-  units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
+  std::vector<std::pair<units::second_t, frc::Pose2d>> visionPoses;
+  std::vector<std::tuple<units::second_t, units::second_t, frc::Pose2d>>
+      visionLog;
 
   double maxError = -std::numeric_limits<double>::max();
   double errorSum = 0;
 
-  while (t <= trajectory.TotalTime()) {
-    auto groundTruthState = trajectory.Sample(t);
-    auto input = kinematics.ToWheelSpeeds(
-        {groundTruthState.velocity, 0_mps,
-         groundTruthState.velocity * groundTruthState.curvature});
+  if (debug) {
+    fmt::print(
+        "time, est_x, est_y, est_theta, true_x, true_y, true_theta, left, "
+        "right\n");
+  }
 
-    if (lastVisionUpdateTime + kVisionUpdateRate < t) {
-      if (lastVisionPose != frc::Pose2d()) {
-        estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
-      }
-      lastVisionPose =
-          groundTruthState.pose +
-          frc::Transform2d(
-              frc::Translation2d(distribution(generator) * 0.1 * 1_m,
-                                 distribution(generator) * 0.1 * 1_m),
-              frc::Rotation2d(distribution(generator) * 0.01 * 1_rad));
+  while (t < trajectory.TotalTime()) {
+    frc::Trajectory::State groundTruthState = trajectory.Sample(t);
 
-      lastVisionUpdateTime = t;
+    // We are due for a new vision measurement if it's been `visionUpdateRate`
+    // seconds since the last vision measurement
+    if (visionPoses.empty() ||
+        visionPoses.back().first + kVisionUpdateRate < t) {
+      auto visionPose =
+          visionMeasurementGenerator(groundTruthState) +
+          frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
+                                              distribution(generator) * 0.1_m},
+                           frc::Rotation2d{distribution(generator) * 0.05_rad}};
+      visionPoses.push_back({t, visionPose});
     }
 
-    leftDistance += input.left * distribution(generator) * 0.01 * dt;
-    rightDistance += input.right * distribution(generator) * 0.01 * dt;
+    // We should apply the oldest vision measurement if it has been
+    // `visionUpdateDelay` seconds since it was measured
+    if (!visionPoses.empty() &&
+        visionPoses.front().first + kVisionUpdateDelay < t) {
+      auto visionEntry = visionPoses.front();
+      estimator.AddVisionMeasurement(visionEntry.second, visionEntry.first);
+      visionPoses.erase(visionPoses.begin());
+      visionLog.push_back({t, visionEntry.first, visionEntry.second});
+    }
+
+    auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
+
+    auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+    leftDistance += wheelSpeeds.left * dt;
+    rightDistance += wheelSpeeds.right * dt;
 
     auto xhat = estimator.UpdateWithTime(
         t,
         groundTruthState.pose.Rotation() +
-            frc::Rotation2d(units::radian_t(distribution(generator) * 0.001)),
-        input, leftDistance, rightDistance);
+            frc::Rotation2d{distribution(generator) * 0.05_rad} -
+            trajectory.InitialPose().Rotation(),
+        leftDistance, rightDistance);
+
+    if (debug) {
+      fmt::print(
+          "{}, {}, {}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
+          xhat.Y().value(), xhat.Rotation().Radians().value(),
+          groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
+          groundTruthState.pose.Rotation().Radians().value(),
+          leftDistance.value(), rightDistance.value());
+    }
 
     double error = groundTruthState.pose.Translation()
                        .Distance(xhat.Translation())
@@ -92,7 +114,96 @@
     t += dt;
   }
 
-  EXPECT_NEAR(0.0, errorSum / (trajectory.TotalTime().value() / dt.value()),
-              0.2);
-  EXPECT_NEAR(0.0, maxError, 0.4);
+  if (debug) {
+    fmt::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
+
+    units::second_t apply_time;
+    units::second_t measure_time;
+    frc::Pose2d vision_pose;
+    for (auto record : visionLog) {
+      std::tie(apply_time, measure_time, vision_pose) = record;
+      fmt::print("{}, {}, {}, {}, {}\n", apply_time.value(),
+                 measure_time.value(), vision_pose.X().value(),
+                 vision_pose.Y().value(),
+                 vision_pose.Rotation().Radians().value());
+    }
+  }
+
+  EXPECT_NEAR(endingPose.X().value(),
+              estimator.GetEstimatedPosition().X().value(), 0.08);
+  EXPECT_NEAR(endingPose.Y().value(),
+              estimator.GetEstimatedPosition().Y().value(), 0.08);
+  EXPECT_NEAR(endingPose.Rotation().Radians().value(),
+              estimator.GetEstimatedPosition().Rotation().Radians().value(),
+              0.15);
+
+  if (checkError) {
+    EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.05);
+    EXPECT_LT(maxError, 0.2);
+  }
+}
+
+TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
+  frc::DifferentialDriveKinematics kinematics{1.0_m};
+
+  frc::DifferentialDrivePoseEstimator estimator{
+      kinematics,         frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
+      {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 135_deg},
+                  frc::Pose2d{-3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 45_deg}},
+      frc::TrajectoryConfig(2_mps, 2_mps_sq));
+
+  testFollowTrajectory(
+      kinematics, estimator, trajectory,
+      [&](frc::Trajectory::State& state) {
+        return frc::ChassisSpeeds{state.velocity, 0_mps,
+                                  state.velocity * state.curvature};
+      },
+      [&](frc::Trajectory::State& state) { return state.pose; },
+      trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s,
+      0.1_s, 0.25_s, true, false);
+}
+
+TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
+  frc::DifferentialDriveKinematics kinematics{1.0_m};
+
+  frc::DifferentialDrivePoseEstimator estimator{
+      kinematics,         frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
+      {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 135_deg},
+                  frc::Pose2d{-3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 45_deg}},
+      frc::TrajectoryConfig(2_mps, 2_mps_sq));
+
+  for (units::degree_t offset_direction_degs = 0_deg;
+       offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
+    for (units::degree_t offset_heading_degs = 0_deg;
+         offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
+      auto pose_offset = frc::Rotation2d{offset_direction_degs};
+      auto heading_offset = frc::Rotation2d{offset_heading_degs};
+
+      auto initial_pose =
+          trajectory.InitialPose() +
+          frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
+                                              pose_offset.Sin() * 1_m},
+                           heading_offset};
+
+      testFollowTrajectory(
+          kinematics, estimator, trajectory,
+          [&](frc::Trajectory::State& state) {
+            return frc::ChassisSpeeds{state.velocity, 0_mps,
+                                      state.velocity * state.curvature};
+          },
+          [&](frc::Trajectory::State& state) { return state.pose; },
+          initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s,
+          0.25_s, false, false);
+    }
+  }
 }
diff --git a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
index 6d51185..1924b73 100644
--- a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
@@ -7,8 +7,8 @@
 #include <array>
 #include <cmath>
 
-#include "Eigen/Core"
 #include "Eigen/QR"
+#include "frc/EigenCore.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/ExtendedKalmanFilter.h"
 #include "frc/system/NumericalJacobian.h"
@@ -18,8 +18,7 @@
 
 namespace {
 
-Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
-                                  const Eigen::Vector<double, 2>& u) {
+frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
   auto motors = frc::DCMotor::CIM(2);
 
   // constexpr double Glow = 15.32;       // Low gear ratio
@@ -41,7 +40,7 @@
   units::volt_t Vr{u(1)};
 
   auto v = 0.5 * (vl + vr);
-  return Eigen::Vector<double, 5>{
+  return frc::Vectord<5>{
       v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
       ((vr - vl) / (2.0 * rb)).value(),
       k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
@@ -50,16 +49,16 @@
           k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
 }
 
-Eigen::Vector<double, 3> LocalMeasurementModel(
-    const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
+frc::Vectord<3> LocalMeasurementModel(const frc::Vectord<5>& x,
+                                      const frc::Vectord<2>& u) {
   static_cast<void>(u);
-  return Eigen::Vector<double, 3>{x(2), x(3), x(4)};
+  return frc::Vectord<3>{x(2), x(3), x(4)};
 }
 
-Eigen::Vector<double, 5> GlobalMeasurementModel(
-    const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
+frc::Vectord<5> GlobalMeasurementModel(const frc::Vectord<5>& x,
+                                       const frc::Vectord<2>& u) {
   static_cast<void>(u);
-  return Eigen::Vector<double, 5>{x(0), x(1), x(2), x(3), x(4)};
+  return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
 }
 }  // namespace
 
@@ -71,7 +70,7 @@
                                               {0.5, 0.5, 10.0, 1.0, 1.0},
                                               {0.0001, 0.01, 0.01},
                                               dt};
-  Eigen::Vector<double, 2> u{12.0, 12.0};
+  frc::Vectord<2> u{12.0, 12.0};
   observer.Predict(u, dt);
 
   auto localY = LocalMeasurementModel(observer.Xhat(), u);
@@ -98,14 +97,13 @@
   auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
       waypoints, {8.8_mps, 0.1_mps_sq});
 
-  Eigen::Vector<double, 5> r = Eigen::Vector<double, 5>::Zero();
-  Eigen::Vector<double, 2> u = Eigen::Vector<double, 2>::Zero();
+  frc::Vectord<5> r = frc::Vectord<5>::Zero();
+  frc::Vectord<2> u = frc::Vectord<2>::Zero();
 
-  auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics,
-                                            Eigen::Vector<double, 5>::Zero(),
-                                            Eigen::Vector<double, 2>::Zero());
+  auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, frc::Vectord<5>::Zero(),
+                                            frc::Vectord<2>::Zero());
 
-  observer.SetXhat(Eigen::Vector<double, 5>{
+  observer.SetXhat(frc::Vectord<5>{
       trajectory.InitialPose().Translation().X().value(),
       trajectory.InitialPose().Translation().Y().value(),
       trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
@@ -118,17 +116,15 @@
     units::meters_per_second_t vr =
         ref.velocity * (1 + (ref.curvature * rb).value());
 
-    Eigen::Vector<double, 5> nextR{
+    frc::Vectord<5> nextR{
         ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
         ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
 
-    auto localY =
-        LocalMeasurementModel(nextR, Eigen::Vector<double, 2>::Zero());
+    auto localY = LocalMeasurementModel(nextR, frc::Vectord<2>::Zero());
     observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
 
-    Eigen::Vector<double, 5> rdot = (nextR - r) / dt.value();
-    u = B.householderQr().solve(rdot -
-                                Dynamics(r, Eigen::Vector<double, 2>::Zero()));
+    frc::Vectord<5> rdot = (nextR - r) / dt.value();
+    u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero()));
 
     observer.Predict(u, dt);
 
diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
index 881d4e8..13dc5aa 100644
--- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
@@ -4,74 +4,95 @@
 
 #include <limits>
 #include <random>
+#include <tuple>
 
 #include "frc/estimator/MecanumDrivePoseEstimator.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/MecanumDriveKinematics.h"
-#include "frc/kinematics/MecanumDriveOdometry.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
 #include "gtest/gtest.h"
 
-TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
-  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}};
+void testFollowTrajectory(
+    const frc::MecanumDriveKinematics& kinematics,
+    frc::MecanumDrivePoseEstimator& estimator,
+    const frc::Trajectory& trajectory,
+    std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
+        chassisSpeedsGenerator,
+    std::function<frc::Pose2d(frc::Trajectory::State&)>
+        visionMeasurementGenerator,
+    const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
+    const units::second_t dt, const units::second_t kVisionUpdateRate,
+    const units::second_t kVisionUpdateDelay, const bool checkError,
+    const bool debug) {
+  frc::MecanumDriveWheelPositions wheelPositions{};
 
-  frc::MecanumDrivePoseEstimator estimator{
-      frc::Rotation2d(), frc::Pose2d(), kinematics,
-      {0.1, 0.1, 0.1},   {0.05},        {0.1, 0.1, 0.1}};
-
-  frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d()};
-
-  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
-                  frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
-                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
-                  frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
-                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
-      frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+  estimator.ResetPosition(frc::Rotation2d{}, wheelPositions, startingPose);
 
   std::default_random_engine generator;
   std::normal_distribution<double> distribution(0.0, 1.0);
 
-  units::second_t dt = 0.02_s;
   units::second_t t = 0_s;
 
-  units::second_t kVisionUpdateRate = 0.1_s;
-  frc::Pose2d lastVisionPose;
-  units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
-
-  std::vector<frc::Pose2d> visionPoses;
+  std::vector<std::pair<units::second_t, frc::Pose2d>> visionPoses;
+  std::vector<std::tuple<units::second_t, units::second_t, frc::Pose2d>>
+      visionLog;
 
   double maxError = -std::numeric_limits<double>::max();
   double errorSum = 0;
 
+  if (debug) {
+    fmt::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
+  }
+
   while (t < trajectory.TotalTime()) {
     frc::Trajectory::State groundTruthState = trajectory.Sample(t);
 
-    if (lastVisionUpdateTime + kVisionUpdateRate < t) {
-      if (lastVisionPose != frc::Pose2d()) {
-        estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
-      }
-      lastVisionPose =
-          groundTruthState.pose +
-          frc::Transform2d(
-              frc::Translation2d(distribution(generator) * 0.1_m,
-                                 distribution(generator) * 0.1_m),
-              frc::Rotation2d(distribution(generator) * 0.1 * 1_rad));
-      visionPoses.push_back(lastVisionPose);
-      lastVisionUpdateTime = t;
+    // We are due for a new vision measurement if it's been `visionUpdateRate`
+    // seconds since the last vision measurement
+    if (visionPoses.empty() ||
+        visionPoses.back().first + kVisionUpdateRate < t) {
+      auto visionPose =
+          visionMeasurementGenerator(groundTruthState) +
+          frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
+                                              distribution(generator) * 0.1_m},
+                           frc::Rotation2d{distribution(generator) * 0.05_rad}};
+      visionPoses.push_back({t, visionPose});
     }
 
-    auto wheelSpeeds = kinematics.ToWheelSpeeds(
-        {groundTruthState.velocity, 0_mps,
-         groundTruthState.velocity * groundTruthState.curvature});
+    // We should apply the oldest vision measurement if it has been
+    // `visionUpdateDelay` seconds since it was measured
+    if (!visionPoses.empty() &&
+        visionPoses.front().first + kVisionUpdateDelay < t) {
+      auto visionEntry = visionPoses.front();
+      estimator.AddVisionMeasurement(visionEntry.second, visionEntry.first);
+      visionPoses.erase(visionPoses.begin());
+      visionLog.push_back({t, visionEntry.first, visionEntry.second});
+    }
+
+    auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
+
+    auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+    wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
+    wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
+    wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
+    wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
 
     auto xhat = estimator.UpdateWithTime(
         t,
         groundTruthState.pose.Rotation() +
-            frc::Rotation2d(distribution(generator) * 0.05_rad),
-        wheelSpeeds);
+            frc::Rotation2d{distribution(generator) * 0.05_rad} -
+            trajectory.InitialPose().Rotation(),
+        wheelPositions);
+
+    if (debug) {
+      fmt::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
+                 xhat.Y().value(), xhat.Rotation().Radians().value(),
+                 groundTruthState.pose.X().value(),
+                 groundTruthState.pose.Y().value(),
+                 groundTruthState.pose.Rotation().Radians().value());
+    }
+
     double error = groundTruthState.pose.Translation()
                        .Distance(xhat.Translation())
                        .value();
@@ -84,6 +105,104 @@
     t += dt;
   }
 
-  EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2);
-  EXPECT_LT(maxError, 0.4);
+  if (debug) {
+    fmt::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
+
+    units::second_t apply_time;
+    units::second_t measure_time;
+    frc::Pose2d vision_pose;
+    for (auto record : visionLog) {
+      std::tie(apply_time, measure_time, vision_pose) = record;
+      fmt::print("{}, {}, {}, {}, {}\n", apply_time.value(),
+                 measure_time.value(), vision_pose.X().value(),
+                 vision_pose.Y().value(),
+                 vision_pose.Rotation().Radians().value());
+    }
+  }
+
+  EXPECT_NEAR(endingPose.X().value(),
+              estimator.GetEstimatedPosition().X().value(), 0.08);
+  EXPECT_NEAR(endingPose.Y().value(),
+              estimator.GetEstimatedPosition().Y().value(), 0.08);
+  EXPECT_NEAR(endingPose.Rotation().Radians().value(),
+              estimator.GetEstimatedPosition().Rotation().Radians().value(),
+              0.15);
+
+  if (checkError) {
+    EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.051);
+    EXPECT_LT(maxError, 0.2);
+  }
+}
+
+TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
+  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{},
+                                           {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 135_deg},
+                  frc::Pose2d{-3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 45_deg}},
+      frc::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
+
+  testFollowTrajectory(
+      kinematics, estimator, trajectory,
+      [&](frc::Trajectory::State& state) {
+        return frc::ChassisSpeeds{state.velocity, 0_mps,
+                                  state.velocity * state.curvature};
+      },
+      [&](frc::Trajectory::State& state) { return state.pose; },
+      trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s,
+      0.1_s, 0.25_s, true, false);
+}
+
+TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
+  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{},
+                                           {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 135_deg},
+                  frc::Pose2d{-3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 45_deg}},
+      frc::TrajectoryConfig(2.0_mps, 2.0_mps_sq));
+
+  for (units::degree_t offset_direction_degs = 0_deg;
+       offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
+    for (units::degree_t offset_heading_degs = 0_deg;
+         offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
+      auto pose_offset = frc::Rotation2d{offset_direction_degs};
+      auto heading_offset = frc::Rotation2d{offset_heading_degs};
+
+      auto initial_pose =
+          trajectory.InitialPose() +
+          frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
+                                              pose_offset.Sin() * 1_m},
+                           heading_offset};
+
+      testFollowTrajectory(
+          kinematics, estimator, trajectory,
+          [&](frc::Trajectory::State& state) {
+            return frc::ChassisSpeeds{state.velocity, 0_mps,
+                                      state.velocity * state.curvature};
+          },
+          [&](frc::Trajectory::State& state) { return state.pose; },
+          initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s,
+          0.25_s, false, false);
+    }
+  }
 }
diff --git a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
index c012435..19a734b 100644
--- a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
@@ -10,26 +10,23 @@
 namespace {
 TEST(MerweScaledSigmaPointsTest, ZeroMean) {
   frc::MerweScaledSigmaPoints<2> sigmaPoints;
-  auto points = sigmaPoints.SigmaPoints(
-      Eigen::Vector<double, 2>{0.0, 0.0},
-      Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}});
+  auto points = sigmaPoints.SquareRootSigmaPoints(
+      frc::Vectord<2>{0.0, 0.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
 
   EXPECT_TRUE(
-      (points -
-       Eigen::Matrix<double, 2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
+      (points - frc::Matrixd<2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
                                    {0.0, 0.0, 0.00173205, 0.0, -0.00173205}})
           .norm() < 1e-3);
 }
 
 TEST(MerweScaledSigmaPointsTest, NonzeroMean) {
   frc::MerweScaledSigmaPoints<2> sigmaPoints;
-  auto points = sigmaPoints.SigmaPoints(
-      Eigen::Vector<double, 2>{1.0, 2.0},
-      Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 10.0}});
+  auto points = sigmaPoints.SquareRootSigmaPoints(
+      frc::Vectord<2>{1.0, 2.0},
+      frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
 
   EXPECT_TRUE(
-      (points -
-       Eigen::Matrix<double, 2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
+      (points - frc::Matrixd<2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
                                    {2.0, 2.0, 2.00548, 2.0, 1.99452}})
           .norm() < 1e-3);
 }
diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
index ee01f6f..554ca59 100644
--- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
@@ -3,75 +3,99 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <limits>
+#include <numbers>
 #include <random>
+#include <tuple>
+
+#include <fmt/format.h>
 
 #include "frc/estimator/SwerveDrivePoseEstimator.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/SwerveDriveKinematics.h"
-#include "frc/kinematics/SwerveDriveOdometry.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
 #include "gtest/gtest.h"
 
-TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
-  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}};
+void testFollowTrajectory(
+    const frc::SwerveDriveKinematics<4>& kinematics,
+    frc::SwerveDrivePoseEstimator<4>& estimator,
+    const frc::Trajectory& trajectory,
+    std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
+        chassisSpeedsGenerator,
+    std::function<frc::Pose2d(frc::Trajectory::State&)>
+        visionMeasurementGenerator,
+    const frc::Pose2d& startingPose, const frc::Pose2d& endingPose,
+    const units::second_t dt, const units::second_t kVisionUpdateRate,
+    const units::second_t kVisionUpdateDelay, const bool checkError,
+    const bool debug) {
+  wpi::array<frc::SwerveModulePosition, 4> positions{wpi::empty_array};
 
-  frc::SwerveDrivePoseEstimator<4> estimator{
-      frc::Rotation2d(), frc::Pose2d(), kinematics,
-      {0.1, 0.1, 0.1},   {0.05},        {0.1, 0.1, 0.1}};
-
-  frc::SwerveDriveOdometry<4> odometry{kinematics, frc::Rotation2d()};
-
-  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
-      std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
-                  frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
-                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
-                  frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
-                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
-      frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+  estimator.ResetPosition(frc::Rotation2d{}, positions, startingPose);
 
   std::default_random_engine generator;
   std::normal_distribution<double> distribution(0.0, 1.0);
 
-  units::second_t dt = 0.02_s;
   units::second_t t = 0_s;
 
-  units::second_t kVisionUpdateRate = 0.1_s;
-  frc::Pose2d lastVisionPose;
-  units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
-
-  std::vector<frc::Pose2d> visionPoses;
+  std::vector<std::pair<units::second_t, frc::Pose2d>> visionPoses;
+  std::vector<std::tuple<units::second_t, units::second_t, frc::Pose2d>>
+      visionLog;
 
   double maxError = -std::numeric_limits<double>::max();
   double errorSum = 0;
 
+  if (debug) {
+    fmt::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
+  }
+
   while (t < trajectory.TotalTime()) {
     frc::Trajectory::State groundTruthState = trajectory.Sample(t);
 
-    if (lastVisionUpdateTime + kVisionUpdateRate < t) {
-      if (lastVisionPose != frc::Pose2d()) {
-        estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
-      }
-      lastVisionPose =
-          groundTruthState.pose +
-          frc::Transform2d(
-              frc::Translation2d(distribution(generator) * 0.1_m,
-                                 distribution(generator) * 0.1_m),
-              frc::Rotation2d(distribution(generator) * 0.1 * 1_rad));
-      visionPoses.push_back(lastVisionPose);
-      lastVisionUpdateTime = t;
+    // We are due for a new vision measurement if it's been `visionUpdateRate`
+    // seconds since the last vision measurement
+    if (visionPoses.empty() ||
+        visionPoses.back().first + kVisionUpdateRate < t) {
+      auto visionPose =
+          visionMeasurementGenerator(groundTruthState) +
+          frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m,
+                                              distribution(generator) * 0.1_m},
+                           frc::Rotation2d{distribution(generator) * 0.05_rad}};
+      visionPoses.push_back({t, visionPose});
     }
 
-    auto moduleStates = kinematics.ToSwerveModuleStates(
-        {groundTruthState.velocity, 0_mps,
-         groundTruthState.velocity * groundTruthState.curvature});
+    // We should apply the oldest vision measurement if it has been
+    // `visionUpdateDelay` seconds since it was measured
+    if (!visionPoses.empty() &&
+        visionPoses.front().first + kVisionUpdateDelay < t) {
+      auto visionEntry = visionPoses.front();
+      estimator.AddVisionMeasurement(visionEntry.second, visionEntry.first);
+      visionPoses.erase(visionPoses.begin());
+      visionLog.push_back({t, visionEntry.first, visionEntry.second});
+    }
+
+    auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
+
+    auto moduleStates = kinematics.ToSwerveModuleStates(chassisSpeeds);
+
+    for (size_t i = 0; i < 4; i++) {
+      positions[i].distance += moduleStates[i].speed * dt;
+      positions[i].angle = moduleStates[i].angle;
+    }
 
     auto xhat = estimator.UpdateWithTime(
         t,
         groundTruthState.pose.Rotation() +
-            frc::Rotation2d(distribution(generator) * 0.05_rad),
-        moduleStates[0], moduleStates[1], moduleStates[2], moduleStates[3]);
+            frc::Rotation2d{distribution(generator) * 0.05_rad} -
+            trajectory.InitialPose().Rotation(),
+        positions);
+
+    if (debug) {
+      fmt::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
+                 xhat.Y().value(), xhat.Rotation().Radians().value(),
+                 groundTruthState.pose.X().value(),
+                 groundTruthState.pose.Y().value(),
+                 groundTruthState.pose.Rotation().Radians().value());
+    }
+
     double error = groundTruthState.pose.Translation()
                        .Distance(xhat.Translation())
                        .value();
@@ -84,6 +108,110 @@
     t += dt;
   }
 
-  EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2);
-  EXPECT_LT(maxError, 0.4);
+  if (debug) {
+    fmt::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
+
+    units::second_t apply_time;
+    units::second_t measure_time;
+    frc::Pose2d vision_pose;
+    for (auto record : visionLog) {
+      std::tie(apply_time, measure_time, vision_pose) = record;
+      fmt::print("{}, {}, {}, {}, {}\n", apply_time.value(),
+                 measure_time.value(), vision_pose.X().value(),
+                 vision_pose.Y().value(),
+                 vision_pose.Rotation().Radians().value());
+    }
+  }
+
+  EXPECT_NEAR(endingPose.X().value(),
+              estimator.GetEstimatedPosition().X().value(), 0.08);
+  EXPECT_NEAR(endingPose.Y().value(),
+              estimator.GetEstimatedPosition().Y().value(), 0.08);
+  EXPECT_NEAR(endingPose.Rotation().Radians().value(),
+              estimator.GetEstimatedPosition().Rotation().Radians().value(),
+              0.15);
+
+  if (checkError) {
+    EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.058);
+    EXPECT_LT(maxError, 0.2);
+  }
+}
+
+TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
+  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}};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 135_deg},
+                  frc::Pose2d{-3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 45_deg}},
+      frc::TrajectoryConfig(2_mps, 2.0_mps_sq));
+
+  testFollowTrajectory(
+      kinematics, estimator, trajectory,
+      [&](frc::Trajectory::State& state) {
+        return frc::ChassisSpeeds{state.velocity, 0_mps,
+                                  state.velocity * state.curvature};
+      },
+      [&](frc::Trajectory::State& state) { return state.pose; },
+      {0_m, 0_m, frc::Rotation2d{45_deg}}, {0_m, 0_m, frc::Rotation2d{45_deg}},
+      0.02_s, 0.1_s, 0.25_s, true, false);
+}
+
+TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
+  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.9, 0.9, 0.9}};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 135_deg},
+                  frc::Pose2d{-3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 45_deg}},
+      frc::TrajectoryConfig(2_mps, 2.0_mps_sq));
+
+  for (units::degree_t offset_direction_degs = 0_deg;
+       offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
+    for (units::degree_t offset_heading_degs = 0_deg;
+         offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) {
+      auto pose_offset = frc::Rotation2d{offset_direction_degs};
+      auto heading_offset = frc::Rotation2d{offset_heading_degs};
+
+      auto initial_pose =
+          trajectory.InitialPose() +
+          frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m,
+                                              pose_offset.Sin() * 1_m},
+                           heading_offset};
+
+      testFollowTrajectory(
+          kinematics, estimator, trajectory,
+          [&](frc::Trajectory::State& state) {
+            return frc::ChassisSpeeds{state.velocity, 0_mps,
+                                      state.velocity * state.curvature};
+          },
+          [&](frc::Trajectory::State& state) { return state.pose; },
+          initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 0.02_s, 0.1_s,
+          0.25_s, false, false);
+    }
+  }
 }
diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
index 9665442..68f9c40 100644
--- a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
@@ -7,8 +7,8 @@
 #include <array>
 #include <cmath>
 
-#include "Eigen/Core"
 #include "Eigen/QR"
+#include "frc/EigenCore.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/AngleStatistics.h"
 #include "frc/estimator/UnscentedKalmanFilter.h"
@@ -20,11 +20,10 @@
 
 namespace {
 
-Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
-                                  const Eigen::Vector<double, 2>& u) {
+frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
   auto motors = frc::DCMotor::CIM(2);
 
-  // constexpr double Glow = 15.32;       // Low gear ratio
+  // constexpr double Glow = 15.32;    // Low gear ratio
   constexpr double Ghigh = 7.08;       // High gear ratio
   constexpr auto rb = 0.8382_m / 2.0;  // Robot radius
   constexpr auto r = 0.0746125_m;      // Wheel radius
@@ -43,7 +42,7 @@
   units::volt_t Vr{u(1)};
 
   auto v = 0.5 * (vl + vr);
-  return Eigen::Vector<double, 5>{
+  return frc::Vectord<5>{
       v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
       ((vr - vl) / (2.0 * rb)).value(),
       k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
@@ -52,16 +51,16 @@
           k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
 }
 
-Eigen::Vector<double, 3> LocalMeasurementModel(
-    const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
+frc::Vectord<3> LocalMeasurementModel(const frc::Vectord<5>& x,
+                                      const frc::Vectord<2>& u) {
   static_cast<void>(u);
-  return Eigen::Vector<double, 3>{x(2), x(3), x(4)};
+  return frc::Vectord<3>{x(2), x(3), x(4)};
 }
 
-Eigen::Vector<double, 5> GlobalMeasurementModel(
-    const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
+frc::Vectord<5> GlobalMeasurementModel(const frc::Vectord<5>& x,
+                                       const frc::Vectord<2>& u) {
   static_cast<void>(u);
-  return Eigen::Vector<double, 5>{x(0), x(1), x(2), x(3), x(4)};
+  return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
 }
 }  // namespace
 
@@ -72,8 +71,13 @@
                                                LocalMeasurementModel,
                                                {0.5, 0.5, 10.0, 1.0, 1.0},
                                                {0.0001, 0.01, 0.01},
+                                               frc::AngleMean<5, 5>(2),
+                                               frc::AngleMean<3, 5>(0),
+                                               frc::AngleResidual<5>(2),
+                                               frc::AngleResidual<3>(0),
+                                               frc::AngleAdd<5>(2),
                                                dt};
-  Eigen::Vector<double, 2> u{12.0, 12.0};
+  frc::Vectord<2> u{12.0, 12.0};
   observer.Predict(u, dt);
 
   auto localY = LocalMeasurementModel(observer.Xhat(), u);
@@ -94,6 +98,11 @@
                                                LocalMeasurementModel,
                                                {0.5, 0.5, 10.0, 1.0, 1.0},
                                                {0.0001, 0.5, 0.5},
+                                               frc::AngleMean<5, 5>(2),
+                                               frc::AngleMean<3, 5>(0),
+                                               frc::AngleResidual<5>(2),
+                                               frc::AngleResidual<3>(0),
+                                               frc::AngleAdd<5>(2),
                                                dt};
 
   auto waypoints =
@@ -102,14 +111,13 @@
   auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
       waypoints, {8.8_mps, 0.1_mps_sq});
 
-  Eigen::Vector<double, 5> r = Eigen::Vector<double, 5>::Zero();
-  Eigen::Vector<double, 2> u = Eigen::Vector<double, 2>::Zero();
+  frc::Vectord<5> r = frc::Vectord<5>::Zero();
+  frc::Vectord<2> u = frc::Vectord<2>::Zero();
 
-  auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics,
-                                            Eigen::Vector<double, 5>::Zero(),
-                                            Eigen::Vector<double, 2>::Zero());
+  auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, frc::Vectord<5>::Zero(),
+                                            frc::Vectord<2>::Zero());
 
-  observer.SetXhat(Eigen::Vector<double, 5>{
+  observer.SetXhat(frc::Vectord<5>{
       trajectory.InitialPose().Translation().X().value(),
       trajectory.InitialPose().Translation().Y().value(),
       trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
@@ -124,17 +132,15 @@
     units::meters_per_second_t vr =
         ref.velocity * (1 + (ref.curvature * rb).value());
 
-    Eigen::Vector<double, 5> nextR{
+    frc::Vectord<5> nextR{
         ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
         ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
 
-    auto localY =
-        LocalMeasurementModel(trueXhat, Eigen::Vector<double, 2>::Zero());
+    auto localY = LocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
     observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
 
-    Eigen::Vector<double, 5> rdot = (nextR - r) / dt.value();
-    u = B.householderQr().solve(rdot -
-                                Dynamics(r, Eigen::Vector<double, 2>::Zero()));
+    frc::Vectord<5> rdot = (nextR - r) / dt.value();
+    u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero()));
 
     observer.Predict(u, dt);
 
@@ -154,12 +160,28 @@
   );
 
   auto finalPosition = trajectory.Sample(trajectory.TotalTime());
-  ASSERT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
-              1.0);
-  ASSERT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
-              1.0);
-  ASSERT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
-              1.0);
-  ASSERT_NEAR(0.0, observer.Xhat(3), 1.0);
-  ASSERT_NEAR(0.0, observer.Xhat(4), 1.0);
+  EXPECT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
+              0.055);
+  EXPECT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
+              0.15);
+  EXPECT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
+              0.000005);
+  EXPECT_NEAR(0.0, observer.Xhat(3), 0.1);
+  EXPECT_NEAR(0.0, observer.Xhat(4), 0.1);
+}
+
+TEST(UnscentedKalmanFilterTest, RoundTripP) {
+  constexpr auto dt = 5_ms;
+
+  frc::UnscentedKalmanFilter<2, 2, 2> observer{
+      [](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
+      [](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
+      {0.0, 0.0},
+      {0.0, 0.0},
+      dt};
+
+  frc::Matrixd<2, 2> P({{2, 1}, {1, 2}});
+  observer.SetP(P);
+
+  ASSERT_TRUE(observer.P().isApprox(P));
 }
diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
index 5ccd829..8299a71 100644
--- a/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
@@ -5,10 +5,9 @@
 #include "frc/filter/LinearFilter.h"  // NOLINT(build/include_order)
 
 #include <cmath>
+#include <numbers>
 #include <random>
 
-#include <wpi/numbers>
-
 #include "gtest/gtest.h"
 #include "units/time.h"
 
@@ -21,7 +20,7 @@
 enum LinearFilterNoiseTestType { kTestSinglePoleIIR, kTestMovAvg };
 
 static double GetData(double t) {
-  return 100.0 * std::sin(2.0 * wpi::numbers::pi * t);
+  return 100.0 * std::sin(2.0 * std::numbers::pi * t);
 }
 
 class LinearFilterNoiseTest
diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
index 56121fa..152737d 100644
--- a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
@@ -7,10 +7,10 @@
 #include <cmath>
 #include <functional>
 #include <memory>
+#include <numbers>
 #include <random>
 
 #include <wpi/array.h>
-#include <wpi/numbers>
 
 #include "gtest/gtest.h"
 #include "units/time.h"
@@ -33,8 +33,8 @@
 };
 
 static double GetData(double t) {
-  return 100.0 * std::sin(2.0 * wpi::numbers::pi * t) +
-         20.0 * std::cos(50.0 * wpi::numbers::pi * t);
+  return 100.0 * std::sin(2.0 * std::numbers::pi * t) +
+         20.0 * std::cos(50.0 * std::numbers::pi * t);
 }
 
 static double GetPulseData(double t) {
@@ -207,14 +207,14 @@
         return std::log(x);
       },
       [](double x) {
-        // df/dx = 1 / x
+        // df/dx = 1/x
         return 1.0 / x;
       },
       h, 1.0, 20.0);
 
   AssertCentralResults<2, 5>(
       [](double x) {
-        // f(x) = x^2
+        // f(x) = x²
         return x * x;
       },
       [](double x) {
@@ -240,7 +240,7 @@
         return std::log(x);
       },
       [](double x) {
-        // d²f/dx² = -1 / x²
+        // d²f/dx² = -1/x²
         return -1.0 / (x * x);
       },
       h, 1.0, 20.0);
@@ -280,14 +280,14 @@
         return std::log(x);
       },
       [](double x) {
-        // df/dx = 1 / x
+        // df/dx = 1/x
         return 1.0 / x;
       },
       h, 1.0, 20.0);
 
   AssertBackwardResults<2, 4>(
       [](double x) {
-        // f(x) = x^2
+        // f(x) = x²
         return x * x;
       },
       [](double x) {
@@ -313,7 +313,7 @@
         return std::log(x);
       },
       [](double x) {
-        // d²f/dx² = -1 / x²
+        // d²f/dx² = -1/x²
         return -1.0 / (x * x);
       },
       h, 1.0, 20.0);
diff --git a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
index 4896073..5dbe8c8 100644
--- a/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
+++ b/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
@@ -38,3 +38,15 @@
 
   EXPECT_EQ(limiter.Calculate(0.5_m), 0.5_m);
 }
+
+TEST_F(SlewRateLimiterTest, SlewRatePositveNegativeLimit) {
+  frc::SlewRateLimiter<units::meters> limiter(1_mps, -0.5_mps);
+
+  now += 1_s;
+
+  EXPECT_EQ(limiter.Calculate(2_m), 1_m);
+
+  now += 1_s;
+
+  EXPECT_EQ(limiter.Calculate(0_m), 0.5_m);
+}
diff --git a/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
new file mode 100644
index 0000000..fc44fa5
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
@@ -0,0 +1,163 @@
+// 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/CoordinateSystem.h"
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Transform3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+void CheckPose3dConvert(const Pose3d& poseFrom, const Pose3d& poseTo,
+                        const CoordinateSystem& coordFrom,
+                        const CoordinateSystem& coordTo) {
+  // "from" to "to"
+  EXPECT_EQ(
+      poseTo.Translation(),
+      CoordinateSystem::Convert(poseFrom.Translation(), coordFrom, coordTo));
+  EXPECT_EQ(poseTo.Rotation(),
+            CoordinateSystem::Convert(poseFrom.Rotation(), coordFrom, coordTo));
+  EXPECT_EQ(poseTo, CoordinateSystem::Convert(poseFrom, coordFrom, coordTo));
+
+  // "to" to "from"
+  EXPECT_EQ(
+      poseFrom.Translation(),
+      CoordinateSystem::Convert(poseTo.Translation(), coordTo, coordFrom));
+  EXPECT_EQ(poseFrom.Rotation(),
+            CoordinateSystem::Convert(poseTo.Rotation(), coordTo, coordFrom));
+  EXPECT_EQ(poseFrom, CoordinateSystem::Convert(poseTo, coordTo, coordFrom));
+}
+
+void CheckTransform3dConvert(const Transform3d& transformFrom,
+                             const Transform3d& transformTo,
+                             const CoordinateSystem& coordFrom,
+                             const CoordinateSystem& coordTo) {
+  // "from" to "to"
+  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));
+
+  // "to" to "from"
+  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));
+}
+
+TEST(CoordinateSystemTest, Pose3dEDNtoNWU) {
+  // No rotation from EDN to NWU
+  CheckPose3dConvert(
+      Pose3d{1_m, 2_m, 3_m, Rotation3d{}},
+      Pose3d{3_m, -1_m, -2_m, Rotation3d{-90_deg, 0_deg, -90_deg}},
+      CoordinateSystem::EDN(), CoordinateSystem::NWU());
+
+  // 45° roll from EDN to NWU
+  CheckPose3dConvert(
+      Pose3d{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}},
+      Pose3d{3_m, -1_m, -2_m, Rotation3d{-45_deg, 0_deg, -90_deg}},
+      CoordinateSystem::EDN(), CoordinateSystem::NWU());
+
+  // 45° pitch from EDN to NWU
+  CheckPose3dConvert(
+      Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 45_deg, 0_deg}},
+      Pose3d{3_m, -1_m, -2_m, Rotation3d{-90_deg, 0_deg, -135_deg}},
+      CoordinateSystem::EDN(), CoordinateSystem::NWU());
+
+  // 45° yaw from EDN to NWU
+  CheckPose3dConvert(
+      Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}},
+      Pose3d{3_m, -1_m, -2_m, Rotation3d{-90_deg, 45_deg, -90_deg}},
+      CoordinateSystem::EDN(), CoordinateSystem::NWU());
+}
+
+TEST(CoordinateSystemTest, Pose3dEDNtoNED) {
+  // No rotation from EDN to NED
+  CheckPose3dConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{}},
+                     Pose3d{3_m, 1_m, 2_m, Rotation3d{90_deg, 0_deg, 90_deg}},
+                     CoordinateSystem::EDN(), CoordinateSystem::NED());
+
+  // 45° roll from EDN to NED
+  CheckPose3dConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}},
+                     Pose3d{3_m, 1_m, 2_m, Rotation3d{135_deg, 0_deg, 90_deg}},
+                     CoordinateSystem::EDN(), CoordinateSystem::NED());
+
+  // 45° pitch from EDN to NED
+  CheckPose3dConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 45_deg, 0_deg}},
+                     Pose3d{3_m, 1_m, 2_m, Rotation3d{90_deg, 0_deg, 135_deg}},
+                     CoordinateSystem::EDN(), CoordinateSystem::NED());
+
+  // 45° yaw from EDN to NED
+  CheckPose3dConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}},
+                     Pose3d{3_m, 1_m, 2_m, Rotation3d{90_deg, -45_deg, 90_deg}},
+                     CoordinateSystem::EDN(), CoordinateSystem::NED());
+}
+
+TEST(CoordinateSystemTest, Transform3dEDNtoNWU) {
+  // 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}},
+      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}},
+                          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}},
+                          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}},
+                          CoordinateSystem::EDN(), CoordinateSystem::NWU());
+}
+
+TEST(CoordinateSystemTest, Transform3dEDNtoNED) {
+  // 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}},
+      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}},
+                          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}},
+                          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}},
+                          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 cd5b127..5ce6819 100644
--- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
@@ -9,51 +9,66 @@
 
 using namespace frc;
 
-static constexpr double kEpsilon = 1E-9;
-
 TEST(Pose2dTest, TransformBy) {
-  const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
-  const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
+  const Pose2d initial{1_m, 2_m, 45_deg};
+  const Transform2d transform{Translation2d{5_m, 0_m}, 5_deg};
 
   const auto transformed = initial + transform;
 
-  EXPECT_NEAR(transformed.X().value(), 1 + 5 / std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(transformed.Y().value(), 2 + 5 / std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(transformed.Rotation().Degrees().value(), 50.0, kEpsilon);
+  EXPECT_DOUBLE_EQ(1.0 + 5.0 / std::sqrt(2.0), transformed.X().value());
+  EXPECT_DOUBLE_EQ(2.0 + 5.0 / std::sqrt(2.0), transformed.Y().value());
+  EXPECT_DOUBLE_EQ(50.0, transformed.Rotation().Degrees().value());
 }
 
 TEST(Pose2dTest, RelativeTo) {
-  const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
-  const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
+  const Pose2d initial{0_m, 0_m, 45_deg};
+  const Pose2d final{5_m, 5_m, 45.0_deg};
 
   const auto finalRelativeToInitial = final.RelativeTo(initial);
 
-  EXPECT_NEAR(finalRelativeToInitial.X().value(), 5.0 * std::sqrt(2.0),
-              kEpsilon);
-  EXPECT_NEAR(finalRelativeToInitial.Y().value(), 0.0, kEpsilon);
-  EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().value(), 0.0,
-              kEpsilon);
+  EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value());
+  EXPECT_NEAR(0.0, finalRelativeToInitial.Y().value(), 1e-9);
+  EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Rotation().Degrees().value());
 }
 
 TEST(Pose2dTest, Equality) {
-  const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
-  const Pose2d b{0_m, 5_m, Rotation2d(43_deg)};
+  const Pose2d a{0_m, 5_m, 43_deg};
+  const Pose2d b{0_m, 5_m, 43_deg};
   EXPECT_TRUE(a == b);
 }
 
 TEST(Pose2dTest, Inequality) {
-  const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
-  const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)};
+  const Pose2d a{0_m, 5_m, 43_deg};
+  const Pose2d b{0_m, 5_ft, 43_deg};
   EXPECT_TRUE(a != b);
 }
 
 TEST(Pose2dTest, Minus) {
-  const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
-  const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
+  const Pose2d initial{0_m, 0_m, 45_deg};
+  const Pose2d final{5_m, 5_m, 45_deg};
 
   const auto transform = final - initial;
 
-  EXPECT_NEAR(transform.X().value(), 5.0 * std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(transform.Y().value(), 0.0, kEpsilon);
-  EXPECT_NEAR(transform.Rotation().Degrees().value(), 0.0, kEpsilon);
+  EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), transform.X().value());
+  EXPECT_NEAR(0.0, transform.Y().value(), 1e-9);
+  EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Degrees().value());
+}
+
+TEST(Pose2dTest, Constexpr) {
+  constexpr Pose2d defaultConstructed;
+  constexpr Pose2d translationRotation{Translation2d{0_m, 1_m},
+                                       Rotation2d{0_deg}};
+  constexpr Pose2d coordRotation{0_m, 0_m, Rotation2d{45_deg}};
+
+  constexpr auto added =
+      translationRotation + Transform2d{Translation2d{}, Rotation2d{45_deg}};
+  constexpr auto multiplied = coordRotation * 2;
+
+  static_assert(defaultConstructed.X() == 0_m);
+  static_assert(translationRotation.Y() == 1_m);
+  static_assert(coordRotation.Rotation().Degrees() == 45_deg);
+  static_assert(added.X() == 0_m);
+  static_assert(added.Y() == 1_m);
+  static_assert(added.Rotation().Degrees() == 45_deg);
+  static_assert(multiplied.Rotation().Degrees() == 90_deg);
 }
diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
new file mode 100644
index 0000000..8c4452c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
@@ -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.
+
+#include <cmath>
+
+#include "frc/geometry/Pose3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(Pose3dTest, TestTransformByRotations) {
+  const double kEpsilon = 1E-9;
+
+  const Pose3d initialPose{0_m, 0_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}};
+  const Transform3d transform1{Translation3d{0_m, 0_m, 0_m},
+                               Rotation3d{90_deg, 45_deg, 0_deg}};
+  const Transform3d transform2{Translation3d{0_m, 0_m, 0_m},
+                               Rotation3d{-90_deg, 0_deg, 0_deg}};
+  const Transform3d transform3{Translation3d{0_m, 0_m, 0_m},
+                               Rotation3d{0_deg, -45_deg, 0_deg}};
+
+  Pose3d finalPose = initialPose.TransformBy(transform1)
+                         .TransformBy(transform2)
+                         .TransformBy(transform3);
+
+  EXPECT_NEAR(finalPose.Rotation().X().value(),
+              initialPose.Rotation().X().value(), kEpsilon);
+  EXPECT_NEAR(finalPose.Rotation().Y().value(),
+              initialPose.Rotation().Y().value(), kEpsilon);
+  EXPECT_NEAR(finalPose.Rotation().Z().value(),
+              initialPose.Rotation().Z().value(), kEpsilon);
+}
+
+TEST(Pose3dTest, TransformBy) {
+  Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const Pose3d initial{1_m, 2_m, 0_m, Rotation3d{zAxis, 45.0_deg}};
+  const Transform3d transform{Translation3d{5_m, 0_m, 0_m},
+                              Rotation3d{zAxis, 5_deg}};
+
+  const auto transformed = initial + transform;
+
+  EXPECT_DOUBLE_EQ(1.0 + 5.0 / std::sqrt(2.0), transformed.X().value());
+  EXPECT_DOUBLE_EQ(2.0 + 5.0 / std::sqrt(2.0), transformed.Y().value());
+  EXPECT_DOUBLE_EQ(transformed.Rotation().Z().value(),
+                   units::radian_t{50_deg}.value());
+}
+
+TEST(Pose3dTest, RelativeTo) {
+  Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const Pose3d initial{0_m, 0_m, 0_m, Rotation3d{zAxis, 45_deg}};
+  const Pose3d final{5_m, 5_m, 0_m, Rotation3d{zAxis, 45_deg}};
+
+  const auto finalRelativeToInitial = final.RelativeTo(initial);
+
+  EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value());
+  EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Y().value());
+  EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Rotation().Z().value());
+}
+
+TEST(Pose3dTest, Equality) {
+  Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const Pose3d a{0_m, 5_m, 0_m, Rotation3d{zAxis, 43_deg}};
+  const Pose3d b{0_m, 5_m, 0_m, Rotation3d{zAxis, 43_deg}};
+  EXPECT_TRUE(a == b);
+}
+
+TEST(Pose3dTest, Inequality) {
+  Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const Pose3d a{0_m, 5_m, 0_m, Rotation3d{zAxis, 43_deg}};
+  const Pose3d b{0_m, 5_ft, 0_m, Rotation3d{zAxis, 43_deg}};
+  EXPECT_TRUE(a != b);
+}
+
+TEST(Pose3dTest, Minus) {
+  Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const Pose3d initial{0_m, 0_m, 0_m, Rotation3d{zAxis, 45_deg}};
+  const Pose3d final{5_m, 5_m, 0_m, Rotation3d{zAxis, 45_deg}};
+
+  const auto transform = final - initial;
+
+  EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), transform.X().value());
+  EXPECT_DOUBLE_EQ(0.0, transform.Y().value());
+  EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Z().value());
+}
+
+TEST(Pose3dTest, ToPose2d) {
+  Pose3d pose{1_m, 2_m, 3_m, Rotation3d{20_deg, 30_deg, 40_deg}};
+  Pose2d expected{1_m, 2_m, 40_deg};
+
+  EXPECT_EQ(expected, pose.ToPose2d());
+}
diff --git a/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp b/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp
new file mode 100644
index 0000000..5b95abb
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp
@@ -0,0 +1,82 @@
+// 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 <numbers>
+
+#include "frc/geometry/Quaternion.h"
+#include "gtest/gtest.h"
+#include "units/angle.h"
+#include "units/math.h"
+
+using namespace frc;
+
+TEST(QuaternionTest, Init) {
+  // Identity
+  Quaternion q1;
+  EXPECT_DOUBLE_EQ(1.0, q1.W());
+  EXPECT_DOUBLE_EQ(0.0, q1.X());
+  EXPECT_DOUBLE_EQ(0.0, q1.Y());
+  EXPECT_DOUBLE_EQ(0.0, q1.Z());
+
+  // Normalized
+  Quaternion q2{0.5, 0.5, 0.5, 0.5};
+  EXPECT_DOUBLE_EQ(0.5, q2.W());
+  EXPECT_DOUBLE_EQ(0.5, q2.X());
+  EXPECT_DOUBLE_EQ(0.5, q2.Y());
+  EXPECT_DOUBLE_EQ(0.5, q2.Z());
+
+  // Unnormalized
+  Quaternion q3{0.75, 0.3, 0.4, 0.5};
+  EXPECT_DOUBLE_EQ(0.75, q3.W());
+  EXPECT_DOUBLE_EQ(0.3, q3.X());
+  EXPECT_DOUBLE_EQ(0.4, q3.Y());
+  EXPECT_DOUBLE_EQ(0.5, q3.Z());
+
+  q3 = q3.Normalize();
+  double norm = std::sqrt(0.75 * 0.75 + 0.3 * 0.3 + 0.4 * 0.4 + 0.5 * 0.5);
+  EXPECT_DOUBLE_EQ(0.75 / norm, q3.W());
+  EXPECT_DOUBLE_EQ(0.3 / norm, q3.X());
+  EXPECT_DOUBLE_EQ(0.4 / norm, q3.Y());
+  EXPECT_DOUBLE_EQ(0.5 / norm, q3.Z());
+  EXPECT_DOUBLE_EQ(1.0, q3.W() * q3.W() + q3.X() * q3.X() + q3.Y() * q3.Y() +
+                            q3.Z() * q3.Z());
+}
+
+TEST(QuaternionTest, Multiply) {
+  // 90° CCW rotations around each axis
+  double c = units::math::cos(90_deg / 2.0);
+  double s = units::math::sin(90_deg / 2.0);
+  Quaternion xRot{c, s, 0.0, 0.0};
+  Quaternion yRot{c, 0.0, s, 0.0};
+  Quaternion zRot{c, 0.0, 0.0, s};
+
+  // 90° CCW X rotation, 90° CCW Y rotation, and 90° CCW Z rotation should
+  // produce a 90° CCW Y rotation
+  auto expected = yRot;
+  auto actual = zRot * yRot * xRot;
+  EXPECT_NEAR(expected.W(), actual.W(), 1e-9);
+  EXPECT_NEAR(expected.X(), actual.X(), 1e-9);
+  EXPECT_NEAR(expected.Y(), actual.Y(), 1e-9);
+  EXPECT_NEAR(expected.Z(), actual.Z(), 1e-9);
+
+  // Identity
+  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());
+}
+
+TEST(QuaternionTest, Inverse) {
+  Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276,
+               0.48507125007266594};
+  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());
+}
diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
index ed3b6b5..4e2e683 100644
--- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
@@ -3,66 +3,82 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <cmath>
-
-#include <wpi/numbers>
+#include <numbers>
 
 #include "frc/geometry/Rotation2d.h"
 #include "gtest/gtest.h"
 
 using namespace frc;
 
-static constexpr double kEpsilon = 1E-9;
-
 TEST(Rotation2dTest, RadiansToDegrees) {
-  const Rotation2d rot1{units::radian_t(wpi::numbers::pi / 3)};
-  const Rotation2d rot2{units::radian_t(wpi::numbers::pi / 4)};
+  const Rotation2d rot1{units::radian_t{std::numbers::pi / 3.0}};
+  const Rotation2d rot2{units::radian_t{std::numbers::pi / 4.0}};
 
-  EXPECT_NEAR(rot1.Degrees().value(), 60.0, kEpsilon);
-  EXPECT_NEAR(rot2.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_DOUBLE_EQ(60.0, rot1.Degrees().value());
+  EXPECT_DOUBLE_EQ(45.0, rot2.Degrees().value());
 }
 
 TEST(Rotation2dTest, DegreesToRadians) {
-  const auto rot1 = Rotation2d(45.0_deg);
-  const auto rot2 = Rotation2d(30.0_deg);
+  const auto rot1 = Rotation2d{45_deg};
+  const auto rot2 = Rotation2d{30_deg};
 
-  EXPECT_NEAR(rot1.Radians().value(), wpi::numbers::pi / 4.0, kEpsilon);
-  EXPECT_NEAR(rot2.Radians().value(), wpi::numbers::pi / 6.0, kEpsilon);
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 4.0, rot1.Radians().value());
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 6.0, rot2.Radians().value());
 }
 
 TEST(Rotation2dTest, RotateByFromZero) {
   const Rotation2d zero;
-  auto sum = zero + Rotation2d(90.0_deg);
+  auto rotated = zero + Rotation2d{90_deg};
 
-  EXPECT_NEAR(sum.Radians().value(), wpi::numbers::pi / 2.0, kEpsilon);
-  EXPECT_NEAR(sum.Degrees().value(), 90.0, kEpsilon);
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 2.0, rotated.Radians().value());
+  EXPECT_DOUBLE_EQ(90.0, rotated.Degrees().value());
 }
 
 TEST(Rotation2dTest, RotateByNonZero) {
-  auto rot = Rotation2d(90.0_deg);
-  rot = rot + Rotation2d(30.0_deg);
+  auto rot = Rotation2d{90_deg};
+  rot = rot + Rotation2d{30_deg};
 
-  EXPECT_NEAR(rot.Degrees().value(), 120.0, kEpsilon);
+  EXPECT_DOUBLE_EQ(120.0, rot.Degrees().value());
 }
 
 TEST(Rotation2dTest, Minus) {
-  const auto rot1 = Rotation2d(70.0_deg);
-  const auto rot2 = Rotation2d(30.0_deg);
+  const auto rot1 = Rotation2d{70_deg};
+  const auto rot2 = Rotation2d{30_deg};
 
-  EXPECT_NEAR((rot1 - rot2).Degrees().value(), 40.0, kEpsilon);
+  EXPECT_DOUBLE_EQ(40.0, (rot1 - rot2).Degrees().value());
 }
 
 TEST(Rotation2dTest, Equality) {
-  const auto rot1 = Rotation2d(43_deg);
-  const auto rot2 = Rotation2d(43_deg);
+  auto rot1 = Rotation2d{43_deg};
+  auto rot2 = Rotation2d{43_deg};
   EXPECT_EQ(rot1, rot2);
 
-  const auto rot3 = Rotation2d(-180_deg);
-  const auto rot4 = Rotation2d(180_deg);
-  EXPECT_EQ(rot3, rot4);
+  rot1 = Rotation2d{-180_deg};
+  rot2 = Rotation2d{180_deg};
+  EXPECT_EQ(rot1, rot2);
 }
 
 TEST(Rotation2dTest, Inequality) {
-  const auto rot1 = Rotation2d(43_deg);
-  const auto rot2 = Rotation2d(43.5_deg);
+  const auto rot1 = Rotation2d{43_deg};
+  const auto rot2 = Rotation2d{43.5_deg};
   EXPECT_NE(rot1, rot2);
 }
+
+TEST(Rotation2dTest, Constexpr) {
+  constexpr Rotation2d defaultCtor;
+  constexpr Rotation2d radianCtor{5_rad};
+  constexpr Rotation2d degreeCtor{270_deg};
+  constexpr Rotation2d rotation45{45_deg};
+  constexpr Rotation2d cartesianCtor{3.5, -3.5};
+
+  constexpr auto negated = -radianCtor;
+  constexpr auto multiplied = radianCtor * 2;
+  constexpr auto subtracted = cartesianCtor - degreeCtor;
+
+  static_assert(defaultCtor.Radians() == 0_rad);
+  static_assert(degreeCtor.Degrees() == 270_deg);
+  static_assert(negated.Radians() == -5_rad);
+  static_assert(multiplied.Radians() == 10_rad);
+  static_assert(subtracted == rotation45);
+  static_assert(radianCtor != degreeCtor);
+}
diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
new file mode 100644
index 0000000..4709ed0
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
@@ -0,0 +1,321 @@
+// 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 <wpi/MathExtras.h>
+
+#include "frc/EigenCore.h"
+#include "frc/geometry/Rotation3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+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};
+  EXPECT_EQ(rot1, rot2);
+
+  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};
+  EXPECT_EQ(rot3, rot4);
+
+  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}};
+  EXPECT_EQ(rot5, rot6);
+}
+
+TEST(Rotation3dTest, InitRotationMatrix) {
+  // No rotation
+  const Matrixd<3, 3> R1 = Matrixd<3, 3>::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};
+  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}};
+  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;
+  EXPECT_THROW(Rotation3d{R4}, std::domain_error);
+}
+
+TEST(Rotation3dTest, InitTwoVector) {
+  const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
+  const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+  const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  // 90 degree CW rotation around y-axis
+  const Rotation3d rot1{xAxis, zAxis};
+  const Rotation3d expected1{yAxis, units::radian_t{-std::numbers::pi / 2.0}};
+  EXPECT_EQ(expected1, rot1);
+
+  // 45 degree CCW rotation around z-axis
+  const Rotation3d rot2{xAxis, Eigen::Vector3d{1.0, 1.0, 0.0}};
+  const Rotation3d expected2{zAxis, units::radian_t{std::numbers::pi / 4.0}};
+  EXPECT_EQ(expected2, rot2);
+
+  // 0 degree rotation of x-axes
+  const Rotation3d rot3{xAxis, xAxis};
+  EXPECT_EQ(Rotation3d{}, rot3);
+
+  // 0 degree rotation of y-axes
+  const Rotation3d rot4{yAxis, yAxis};
+  EXPECT_EQ(Rotation3d{}, rot4);
+
+  // 0 degree rotation of z-axes
+  const Rotation3d rot5{zAxis, zAxis};
+  EXPECT_EQ(Rotation3d{}, rot5);
+
+  // 180 degree rotation tests. For 180 degree rotations, any quaternion with an
+  // orthogonal rotation axis is acceptable. The rotation axis and initial
+  // vector are orthogonal if their dot product is zero.
+
+  // 180 degree rotation of x-axes
+  const Rotation3d rot6{xAxis, -xAxis};
+  const auto q6 = rot6.GetQuaternion();
+  EXPECT_EQ(0.0, q6.W());
+  EXPECT_EQ(0.0, q6.X() * xAxis(0) + q6.Y() * xAxis(1) + q6.Z() * xAxis(2));
+
+  // 180 degree rotation of y-axes
+  const Rotation3d rot7{yAxis, -yAxis};
+  const auto q7 = rot7.GetQuaternion();
+  EXPECT_EQ(0.0, q7.W());
+  EXPECT_EQ(0.0, q7.X() * yAxis(0) + q7.Y() * yAxis(1) + q7.Z() * yAxis(2));
+
+  // 180 degree rotation of z-axes
+  const Rotation3d rot8{zAxis, -zAxis};
+  const auto q8 = rot8.GetQuaternion();
+  EXPECT_EQ(0.0, q8.W());
+  EXPECT_EQ(0.0, q8.X() * zAxis(0) + q8.Y() * zAxis(1) + q8.Z() * zAxis(2));
+}
+
+TEST(Rotation3dTest, RadiansToDegrees) {
+  const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const Rotation3d rot1{zAxis, units::radian_t{std::numbers::pi / 3}};
+  EXPECT_DOUBLE_EQ(0.0, rot1.X().value());
+  EXPECT_DOUBLE_EQ(0.0, rot1.Y().value());
+  EXPECT_DOUBLE_EQ(units::radian_t{60_deg}.value(), rot1.Z().value());
+
+  const Rotation3d rot2{zAxis, units::radian_t{std::numbers::pi / 4}};
+  EXPECT_DOUBLE_EQ(0.0, rot2.X().value());
+  EXPECT_DOUBLE_EQ(0.0, rot2.Y().value());
+  EXPECT_DOUBLE_EQ(units::radian_t{45_deg}.value(), rot2.Z().value());
+}
+
+TEST(Rotation3dTest, DegreesToRadians) {
+  const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const auto rot1 = Rotation3d{zAxis, 45_deg};
+  EXPECT_DOUBLE_EQ(0.0, rot1.X().value());
+  EXPECT_DOUBLE_EQ(0.0, rot1.Y().value());
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 4.0, rot1.Z().value());
+
+  const auto rot2 = Rotation3d{zAxis, 30_deg};
+  EXPECT_DOUBLE_EQ(0.0, rot2.X().value());
+  EXPECT_DOUBLE_EQ(0.0, rot2.Y().value());
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 6.0, rot2.Z().value());
+}
+
+TEST(Rotation3dTest, RotationLoop) {
+  Rotation3d rot;
+
+  rot = rot + Rotation3d{90_deg, 0_deg, 0_deg};
+  Rotation3d expected{90_deg, 0_deg, 0_deg};
+  EXPECT_EQ(expected, rot);
+
+  rot = rot + Rotation3d{0_deg, 90_deg, 0_deg};
+  expected = Rotation3d{
+      {1.0 / std::sqrt(3), 1.0 / std::sqrt(3), -1.0 / std::sqrt(3)}, 120_deg};
+  EXPECT_EQ(expected, rot);
+
+  rot = rot + Rotation3d{0_deg, 0_deg, 90_deg};
+  expected = Rotation3d{0_deg, 90_deg, 0_deg};
+  EXPECT_EQ(expected, rot);
+
+  rot = rot + Rotation3d{0_deg, -90_deg, 0_deg};
+  EXPECT_EQ(Rotation3d{}, rot);
+}
+
+TEST(Rotation3dTest, RotateByFromZeroX) {
+  const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
+
+  const Rotation3d zero;
+  auto rotated = zero + Rotation3d{xAxis, 90_deg};
+
+  Rotation3d expected{xAxis, 90_deg};
+  EXPECT_EQ(expected, rotated);
+}
+
+TEST(Rotation3dTest, RotateByFromZeroY) {
+  const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+
+  const Rotation3d zero;
+  auto rotated = zero + Rotation3d{yAxis, 90_deg};
+
+  Rotation3d expected{yAxis, 90_deg};
+  EXPECT_EQ(expected, rotated);
+}
+
+TEST(Rotation3dTest, RotateByFromZeroZ) {
+  const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const Rotation3d zero;
+  auto rotated = zero + Rotation3d{zAxis, 90_deg};
+
+  Rotation3d expected{zAxis, 90_deg};
+  EXPECT_EQ(expected, rotated);
+}
+
+TEST(Rotation3dTest, RotateByNonZeroX) {
+  const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
+
+  auto rot = Rotation3d{xAxis, 90_deg};
+  rot = rot + Rotation3d{xAxis, 30_deg};
+
+  Rotation3d expected{xAxis, 120_deg};
+  EXPECT_EQ(expected, rot);
+}
+
+TEST(Rotation3dTest, RotateByNonZeroY) {
+  const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+
+  auto rot = Rotation3d{yAxis, 90_deg};
+  rot = rot + Rotation3d{yAxis, 30_deg};
+
+  Rotation3d expected{yAxis, 120_deg};
+  EXPECT_EQ(expected, rot);
+}
+
+TEST(Rotation3dTest, RotateByNonZeroZ) {
+  const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  auto rot = Rotation3d{zAxis, 90_deg};
+  rot = rot + Rotation3d{zAxis, 30_deg};
+
+  Rotation3d expected{zAxis, 120_deg};
+  EXPECT_EQ(expected, rot);
+}
+
+TEST(Rotation3dTest, Minus) {
+  const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const auto rot1 = Rotation3d{zAxis, 70_deg};
+  const auto rot2 = Rotation3d{zAxis, 30_deg};
+
+  EXPECT_DOUBLE_EQ(40.0, units::degree_t{(rot1 - rot2).Z()}.value());
+}
+
+TEST(Rotation3dTest, AxisAngle) {
+  const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
+  const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+  const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  Rotation3d rot1{xAxis, 90_deg};
+  EXPECT_EQ(xAxis, rot1.Axis());
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 2.0, rot1.Angle().value());
+
+  Rotation3d rot2{yAxis, 45_deg};
+  EXPECT_EQ(yAxis, rot2.Axis());
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 4.0, rot2.Angle().value());
+
+  Rotation3d rot3{zAxis, 60_deg};
+  EXPECT_EQ(zAxis, rot3.Axis());
+  EXPECT_DOUBLE_EQ(std::numbers::pi / 3.0, rot3.Angle().value());
+}
+
+TEST(Rotation3dTest, ToRotation2d) {
+  Rotation3d rotation{20_deg, 30_deg, 40_deg};
+  Rotation2d expected{40_deg};
+
+  EXPECT_EQ(expected, rotation.ToRotation2d());
+}
+
+TEST(Rotation3dTest, Equality) {
+  const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const auto rot1 = Rotation3d{zAxis, 43_deg};
+  const auto rot2 = Rotation3d{zAxis, 43_deg};
+  EXPECT_EQ(rot1, rot2);
+
+  const auto rot3 = Rotation3d{zAxis, -180_deg};
+  const auto rot4 = Rotation3d{zAxis, 180_deg};
+  EXPECT_EQ(rot3, rot4);
+}
+
+TEST(Rotation3dTest, Inequality) {
+  const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const auto rot1 = Rotation3d{zAxis, 43_deg};
+  const auto rot2 = Rotation3d{zAxis, 43.5_deg};
+  EXPECT_NE(rot1, rot2);
+}
+
+TEST(Rotation3dTest, Interpolate) {
+  const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
+  const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+  const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  // 50 + (70 - 50) * 0.5 = 60
+  auto rot1 = Rotation3d{xAxis, 50_deg};
+  auto rot2 = Rotation3d{xAxis, 70_deg};
+  auto interpolated = wpi::Lerp(rot1, rot2, 0.5);
+  EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.X()}.value());
+  EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
+  EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value());
+
+  // -160 minus half distance between 170 and -160 (15) = -175
+  rot1 = Rotation3d{xAxis, 170_deg};
+  rot2 = Rotation3d{xAxis, -160_deg};
+  interpolated = wpi::Lerp(rot1, rot2, 0.5);
+  EXPECT_DOUBLE_EQ(-175.0, units::degree_t{interpolated.X()}.value());
+  EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
+  EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value());
+
+  // 50 + (70 - 50) * 0.5 = 60
+  rot1 = Rotation3d{yAxis, 50_deg};
+  rot2 = Rotation3d{yAxis, 70_deg};
+  interpolated = wpi::Lerp(rot1, rot2, 0.5);
+  EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
+  EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.Y()}.value());
+  EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Z()}.value());
+
+  // -160 plus half distance between 170 and -160 (165) = 5
+  rot1 = Rotation3d{yAxis, 170_deg};
+  rot2 = Rotation3d{yAxis, -160_deg};
+  interpolated = wpi::Lerp(rot1, rot2, 0.5);
+  EXPECT_DOUBLE_EQ(180.0, units::degree_t{interpolated.X()}.value());
+  EXPECT_DOUBLE_EQ(-5.0, units::degree_t{interpolated.Y()}.value());
+  EXPECT_DOUBLE_EQ(180.0, units::degree_t{interpolated.Z()}.value());
+
+  // 50 + (70 - 50) * 0.5 = 60
+  rot1 = Rotation3d{zAxis, 50_deg};
+  rot2 = Rotation3d{zAxis, 70_deg};
+  interpolated = wpi::Lerp(rot1, rot2, 0.5);
+  EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
+  EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
+  EXPECT_DOUBLE_EQ(60.0, units::degree_t{interpolated.Z()}.value());
+
+  // -160 minus half distance between 170 and -160 (15) = -175
+  rot1 = Rotation3d{zAxis, 170_deg};
+  rot2 = Rotation3d{zAxis, -160_deg};
+  interpolated = wpi::Lerp(rot1, rot2, 0.5);
+  EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
+  EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
+  EXPECT_DOUBLE_EQ(-175.0, units::degree_t{interpolated.Z()}.value());
+}
diff --git a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
index 968ab29..1b0934d 100644
--- a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
@@ -12,8 +12,6 @@
 
 using namespace frc;
 
-static constexpr double kEpsilon = 1E-9;
-
 TEST(Transform2dTest, Inverse) {
   const Pose2d initial{1_m, 2_m, 45_deg};
   const Transform2d transform{{5_m, 0_m}, 5_deg};
@@ -21,10 +19,7 @@
   auto transformed = initial + transform;
   auto untransformed = transformed + transform.Inverse();
 
-  EXPECT_NEAR(initial.X().value(), untransformed.X().value(), kEpsilon);
-  EXPECT_NEAR(initial.Y().value(), untransformed.Y().value(), kEpsilon);
-  EXPECT_NEAR(initial.Rotation().Degrees().value(),
-              untransformed.Rotation().Degrees().value(), kEpsilon);
+  EXPECT_EQ(initial, untransformed);
 }
 
 TEST(Transform2dTest, Composition) {
@@ -35,10 +30,23 @@
   auto transformedSeparate = initial + transform1 + transform2;
   auto transformedCombined = initial + (transform1 + transform2);
 
-  EXPECT_NEAR(transformedSeparate.X().value(), transformedCombined.X().value(),
-              kEpsilon);
-  EXPECT_NEAR(transformedSeparate.Y().value(), transformedCombined.Y().value(),
-              kEpsilon);
-  EXPECT_NEAR(transformedSeparate.Rotation().Degrees().value(),
-              transformedCombined.Rotation().Degrees().value(), kEpsilon);
+  EXPECT_EQ(transformedSeparate, transformedCombined);
+}
+
+TEST(Transform2dTest, Constexpr) {
+  constexpr Transform2d defaultCtor;
+  constexpr Transform2d translationRotationCtor{Translation2d{},
+                                                Rotation2d{10_deg}};
+  constexpr auto multiplied = translationRotationCtor * 5;
+  constexpr auto divided = translationRotationCtor / 2;
+
+  static_assert(defaultCtor.Translation().X() == 0_m);
+  static_assert(translationRotationCtor.X() == 0_m);
+  static_assert(translationRotationCtor.Y() == 0_m);
+  static_assert(multiplied.Rotation().Degrees() == 50_deg);
+  static_assert(translationRotationCtor.Inverse().Rotation().Degrees() ==
+                (-10_deg));
+  static_assert(translationRotationCtor.Inverse().X() == 0_m);
+  static_assert(translationRotationCtor.Inverse().Y() == 0_m);
+  static_assert(divided.Rotation().Degrees() == 5_deg);
 }
diff --git a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
new file mode 100644
index 0000000..904ec9c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
@@ -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.
+
+#include <cmath>
+
+#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;
+
+TEST(Transform3dTest, Inverse) {
+  Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const Pose3d initial{1_m, 2_m, 3_m, Rotation3d{zAxis, 45_deg}};
+  const Transform3d transform{{5_m, 4_m, 3_m}, Rotation3d{zAxis, 5_deg}};
+
+  auto transformed = initial + transform;
+  auto untransformed = transformed + transform.Inverse();
+
+  EXPECT_EQ(initial, untransformed);
+}
+
+TEST(Transform3dTest, Composition) {
+  Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const Pose3d initial{1_m, 2_m, 3_m, Rotation3d{zAxis, 45_deg}};
+  const Transform3d transform1{{5_m, 0_m, 0_m}, Rotation3d{zAxis, 5_deg}};
+  const Transform3d transform2{{0_m, 2_m, 0_m}, Rotation3d{zAxis, 5_deg}};
+
+  auto transformedSeparate = initial + transform1 + transform2;
+  auto transformedCombined = initial + (transform1 + transform2);
+
+  EXPECT_EQ(transformedSeparate, transformedCombined);
+}
diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
index efdcace..5493c43 100644
--- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
@@ -9,69 +9,67 @@
 
 using namespace frc;
 
-static constexpr double kEpsilon = 1E-9;
-
 TEST(Translation2dTest, Sum) {
-  const Translation2d one{1.0_m, 3.0_m};
-  const Translation2d two{2.0_m, 5.0_m};
+  const Translation2d one{1_m, 3_m};
+  const Translation2d two{2_m, 5_m};
 
   const auto sum = one + two;
 
-  EXPECT_NEAR(sum.X().value(), 3.0, kEpsilon);
-  EXPECT_NEAR(sum.Y().value(), 8.0, kEpsilon);
+  EXPECT_DOUBLE_EQ(3.0, sum.X().value());
+  EXPECT_DOUBLE_EQ(8.0, sum.Y().value());
 }
 
 TEST(Translation2dTest, Difference) {
-  const Translation2d one{1.0_m, 3.0_m};
-  const Translation2d two{2.0_m, 5.0_m};
+  const Translation2d one{1_m, 3_m};
+  const Translation2d two{2_m, 5_m};
 
   const auto difference = one - two;
 
-  EXPECT_NEAR(difference.X().value(), -1.0, kEpsilon);
-  EXPECT_NEAR(difference.Y().value(), -2.0, kEpsilon);
+  EXPECT_DOUBLE_EQ(-1.0, difference.X().value());
+  EXPECT_DOUBLE_EQ(-2.0, difference.Y().value());
 }
 
 TEST(Translation2dTest, RotateBy) {
-  const Translation2d another{3.0_m, 0.0_m};
-  const auto rotated = another.RotateBy(Rotation2d(90.0_deg));
+  const Translation2d another{3_m, 0_m};
+  const auto rotated = another.RotateBy(90_deg);
 
-  EXPECT_NEAR(rotated.X().value(), 0.0, kEpsilon);
-  EXPECT_NEAR(rotated.Y().value(), 3.0, kEpsilon);
+  EXPECT_NEAR(0.0, rotated.X().value(), 1e-9);
+  EXPECT_DOUBLE_EQ(3.0, rotated.Y().value());
 }
 
 TEST(Translation2dTest, Multiplication) {
-  const Translation2d original{3.0_m, 5.0_m};
+  const Translation2d original{3_m, 5_m};
   const auto mult = original * 3;
 
-  EXPECT_NEAR(mult.X().value(), 9.0, kEpsilon);
-  EXPECT_NEAR(mult.Y().value(), 15.0, kEpsilon);
+  EXPECT_DOUBLE_EQ(9.0, mult.X().value());
+  EXPECT_DOUBLE_EQ(15.0, mult.Y().value());
 }
 
 TEST(Translation2dTest, Division) {
-  const Translation2d original{3.0_m, 5.0_m};
+  const Translation2d original{3_m, 5_m};
   const auto div = original / 2;
 
-  EXPECT_NEAR(div.X().value(), 1.5, kEpsilon);
-  EXPECT_NEAR(div.Y().value(), 2.5, kEpsilon);
+  EXPECT_DOUBLE_EQ(1.5, div.X().value());
+  EXPECT_DOUBLE_EQ(2.5, div.Y().value());
 }
 
 TEST(Translation2dTest, Norm) {
-  const Translation2d one{3.0_m, 5.0_m};
-  EXPECT_NEAR(one.Norm().value(), std::hypot(3, 5), kEpsilon);
+  const Translation2d one{3_m, 5_m};
+  EXPECT_DOUBLE_EQ(std::hypot(3.0, 5.0), one.Norm().value());
 }
 
 TEST(Translation2dTest, Distance) {
   const Translation2d one{1_m, 1_m};
   const Translation2d two{6_m, 6_m};
-  EXPECT_NEAR(one.Distance(two).value(), 5 * std::sqrt(2), kEpsilon);
+  EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), one.Distance(two).value());
 }
 
 TEST(Translation2dTest, UnaryMinus) {
   const Translation2d original{-4.5_m, 7_m};
   const auto inverted = -original;
 
-  EXPECT_NEAR(inverted.X().value(), 4.5, kEpsilon);
-  EXPECT_NEAR(inverted.Y().value(), -7, kEpsilon);
+  EXPECT_DOUBLE_EQ(4.5, inverted.X().value());
+  EXPECT_DOUBLE_EQ(-7.0, inverted.Y().value());
 }
 
 TEST(Translation2dTest, Equality) {
@@ -87,11 +85,29 @@
 }
 
 TEST(Translation2dTest, PolarConstructor) {
-  Translation2d one{std::sqrt(2) * 1_m, Rotation2d(45_deg)};
-  EXPECT_NEAR(one.X().value(), 1.0, kEpsilon);
-  EXPECT_NEAR(one.Y().value(), 1.0, kEpsilon);
+  Translation2d one{std::sqrt(2) * 1_m, Rotation2d{45_deg}};
+  EXPECT_DOUBLE_EQ(1.0, one.X().value());
+  EXPECT_DOUBLE_EQ(1.0, one.Y().value());
 
-  Translation2d two{2_m, Rotation2d(60_deg)};
-  EXPECT_NEAR(two.X().value(), 1.0, kEpsilon);
-  EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon);
+  Translation2d two{2_m, Rotation2d{60_deg}};
+  EXPECT_DOUBLE_EQ(1.0, two.X().value());
+  EXPECT_DOUBLE_EQ(std::sqrt(3.0), two.Y().value());
+}
+
+TEST(Translation2dTest, Constexpr) {
+  constexpr Translation2d defaultCtor;
+  constexpr Translation2d componentCtor{1_m, 2_m};
+  constexpr auto added = defaultCtor + componentCtor;
+  constexpr auto subtracted = defaultCtor - componentCtor;
+  constexpr auto negated = -componentCtor;
+  constexpr auto multiplied = componentCtor * 2;
+  constexpr auto divided = componentCtor / 2;
+
+  static_assert(defaultCtor.X() == 0_m);
+  static_assert(componentCtor.Y() == 2_m);
+  static_assert(added.X() == 1_m);
+  static_assert(subtracted.Y() == (-2_m));
+  static_assert(negated.X() == (-1_m));
+  static_assert(multiplied.X() == 2_m);
+  static_assert(divided.Y() == 1_m);
 }
diff --git a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
new file mode 100644
index 0000000..58c611e
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
@@ -0,0 +1,149 @@
+// 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 "frc/geometry/Translation3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Translation3dTest, Sum) {
+  const Translation3d one{1_m, 3_m, 5_m};
+  const Translation3d two{2_m, 5_m, 8_m};
+
+  const auto sum = one + two;
+
+  EXPECT_NEAR(3.0, sum.X().value(), kEpsilon);
+  EXPECT_NEAR(8.0, sum.Y().value(), kEpsilon);
+  EXPECT_NEAR(13.0, sum.Z().value(), kEpsilon);
+}
+
+TEST(Translation3dTest, Difference) {
+  const Translation3d one{1_m, 3_m, 5_m};
+  const Translation3d two{2_m, 5_m, 8_m};
+
+  const auto difference = one - two;
+
+  EXPECT_NEAR(difference.X().value(), -1.0, kEpsilon);
+  EXPECT_NEAR(difference.Y().value(), -2.0, kEpsilon);
+  EXPECT_NEAR(difference.Z().value(), -3.0, kEpsilon);
+}
+
+TEST(Translation3dTest, RotateBy) {
+  Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
+  Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+  Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const Translation3d translation{1_m, 2_m, 3_m};
+
+  const auto rotated1 = translation.RotateBy(Rotation3d{xAxis, 90_deg});
+  EXPECT_NEAR(rotated1.X().value(), 1.0, kEpsilon);
+  EXPECT_NEAR(rotated1.Y().value(), -3.0, kEpsilon);
+  EXPECT_NEAR(rotated1.Z().value(), 2.0, kEpsilon);
+
+  const auto rotated2 = translation.RotateBy(Rotation3d{yAxis, 90_deg});
+  EXPECT_NEAR(rotated2.X().value(), 3.0, kEpsilon);
+  EXPECT_NEAR(rotated2.Y().value(), 2.0, kEpsilon);
+  EXPECT_NEAR(rotated2.Z().value(), -1.0, kEpsilon);
+
+  const auto rotated3 = translation.RotateBy(Rotation3d{zAxis, 90_deg});
+  EXPECT_NEAR(rotated3.X().value(), -2.0, kEpsilon);
+  EXPECT_NEAR(rotated3.Y().value(), 1.0, kEpsilon);
+  EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon);
+}
+
+TEST(Translation3dTest, ToTranslation2d) {
+  Translation3d translation{1_m, 2_m, 3_m};
+  Translation2d expected{1_m, 2_m};
+
+  EXPECT_EQ(expected, translation.ToTranslation2d());
+}
+
+TEST(Translation3dTest, Multiplication) {
+  const Translation3d original{3_m, 5_m, 7_m};
+  const auto mult = original * 3;
+
+  EXPECT_NEAR(mult.X().value(), 9.0, kEpsilon);
+  EXPECT_NEAR(mult.Y().value(), 15.0, kEpsilon);
+  EXPECT_NEAR(mult.Z().value(), 21.0, kEpsilon);
+}
+
+TEST(Translation3dTest, Division) {
+  const Translation3d original{3_m, 5_m, 7_m};
+  const auto div = original / 2;
+
+  EXPECT_NEAR(div.X().value(), 1.5, kEpsilon);
+  EXPECT_NEAR(div.Y().value(), 2.5, kEpsilon);
+  EXPECT_NEAR(div.Z().value(), 3.5, kEpsilon);
+}
+
+TEST(Translation3dTest, Norm) {
+  const Translation3d one{3_m, 5_m, 7_m};
+  EXPECT_NEAR(one.Norm().value(), std::hypot(3, 5, 7), kEpsilon);
+}
+
+TEST(Translation3dTest, Distance) {
+  const Translation3d one{1_m, 1_m, 1_m};
+  const Translation3d two{6_m, 6_m, 6_m};
+  EXPECT_NEAR(one.Distance(two).value(), 5 * std::sqrt(3), kEpsilon);
+}
+
+TEST(Translation3dTest, UnaryMinus) {
+  const Translation3d original{-4.5_m, 7_m, 9_m};
+  const auto inverted = -original;
+
+  EXPECT_NEAR(inverted.X().value(), 4.5, kEpsilon);
+  EXPECT_NEAR(inverted.Y().value(), -7, kEpsilon);
+  EXPECT_NEAR(inverted.Z().value(), -9, kEpsilon);
+}
+
+TEST(Translation3dTest, Equality) {
+  const Translation3d one{9_m, 5.5_m, 3.5_m};
+  const Translation3d two{9_m, 5.5_m, 3.5_m};
+  EXPECT_TRUE(one == two);
+}
+
+TEST(Translation3dTest, Inequality) {
+  const Translation3d one{9_m, 5.5_m, 3.5_m};
+  const Translation3d two{9_m, 5.7_m, 3.5_m};
+  EXPECT_TRUE(one != two);
+}
+
+TEST(Translation3dTest, PolarConstructor) {
+  Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  Translation3d one{std::sqrt(2) * 1_m, Rotation3d{zAxis, 45_deg}};
+  EXPECT_NEAR(one.X().value(), 1.0, kEpsilon);
+  EXPECT_NEAR(one.Y().value(), 1.0, kEpsilon);
+  EXPECT_NEAR(one.Z().value(), 0.0, kEpsilon);
+
+  Translation3d two{2_m, Rotation3d{zAxis, 60_deg}};
+  EXPECT_NEAR(two.X().value(), 1.0, kEpsilon);
+  EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon);
+  EXPECT_NEAR(two.Z().value(), 0.0, kEpsilon);
+}
+
+TEST(Translation3dTest, Constexpr) {
+  constexpr Translation3d defaultCtor;
+  constexpr Translation3d componentCtor{1_m, 2_m, 3_m};
+  constexpr auto added = defaultCtor + componentCtor;
+  constexpr auto subtracted = defaultCtor - componentCtor;
+  constexpr auto negated = -componentCtor;
+  constexpr auto multiplied = componentCtor * 2;
+  constexpr auto divided = componentCtor / 2;
+  constexpr Translation2d projected = componentCtor.ToTranslation2d();
+
+  static_assert(defaultCtor.X() == 0_m);
+  static_assert(componentCtor.Y() == 2_m);
+  static_assert(added.Z() == 3_m);
+  static_assert(subtracted.X() == (-1_m));
+  static_assert(negated.Y() == (-2_m));
+  static_assert(multiplied.Z() == 6_m);
+  static_assert(divided.Y() == 1_m);
+  static_assert(projected.X() == 1_m);
+  static_assert(projected.Y() == 2_m);
+}
diff --git a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
index fa9eecc..33970cd 100644
--- a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
@@ -3,63 +3,74 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <cmath>
-
-#include <wpi/numbers>
+#include <numbers>
 
 #include "frc/geometry/Pose2d.h"
 #include "gtest/gtest.h"
 
 using namespace frc;
 
-static constexpr double kEpsilon = 1E-9;
-
 TEST(Twist2dTest, Straight) {
-  const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
-  const auto straightPose = Pose2d().Exp(straight);
+  const Twist2d straight{5_m, 0_m, 0_rad};
+  const auto straightPose = Pose2d{}.Exp(straight);
 
-  EXPECT_NEAR(straightPose.X().value(), 5.0, kEpsilon);
-  EXPECT_NEAR(straightPose.Y().value(), 0.0, kEpsilon);
-  EXPECT_NEAR(straightPose.Rotation().Radians().value(), 0.0, kEpsilon);
+  EXPECT_DOUBLE_EQ(5.0, straightPose.X().value());
+  EXPECT_DOUBLE_EQ(0.0, straightPose.Y().value());
+  EXPECT_DOUBLE_EQ(0.0, straightPose.Rotation().Radians().value());
 }
 
 TEST(Twist2dTest, QuarterCircle) {
-  const Twist2d quarterCircle{5.0_m / 2.0 * wpi::numbers::pi, 0_m,
-                              units::radian_t(wpi::numbers::pi / 2.0)};
-  const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
+  const Twist2d quarterCircle{5_m / 2.0 * std::numbers::pi, 0_m,
+                              units::radian_t{std::numbers::pi / 2.0}};
+  const auto quarterCirclePose = Pose2d{}.Exp(quarterCircle);
 
-  EXPECT_NEAR(quarterCirclePose.X().value(), 5.0, kEpsilon);
-  EXPECT_NEAR(quarterCirclePose.Y().value(), 5.0, kEpsilon);
-  EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().value(), 90.0, kEpsilon);
+  EXPECT_DOUBLE_EQ(5.0, quarterCirclePose.X().value());
+  EXPECT_DOUBLE_EQ(5.0, quarterCirclePose.Y().value());
+  EXPECT_DOUBLE_EQ(90.0, quarterCirclePose.Rotation().Degrees().value());
 }
 
 TEST(Twist2dTest, DiagonalNoDtheta) {
-  const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg};
-  const auto diagonalPose = Pose2d().Exp(diagonal);
+  const Twist2d diagonal{2_m, 2_m, 0_deg};
+  const auto diagonalPose = Pose2d{}.Exp(diagonal);
 
-  EXPECT_NEAR(diagonalPose.X().value(), 2.0, kEpsilon);
-  EXPECT_NEAR(diagonalPose.Y().value(), 2.0, kEpsilon);
-  EXPECT_NEAR(diagonalPose.Rotation().Degrees().value(), 0.0, kEpsilon);
+  EXPECT_DOUBLE_EQ(2.0, diagonalPose.X().value());
+  EXPECT_DOUBLE_EQ(2.0, diagonalPose.Y().value());
+  EXPECT_DOUBLE_EQ(0.0, diagonalPose.Rotation().Degrees().value());
 }
 
 TEST(Twist2dTest, Equality) {
-  const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
-  const Twist2d two{5.0_m, 1.0_m, 3.0_rad};
+  const Twist2d one{5_m, 1_m, 3_rad};
+  const Twist2d two{5_m, 1_m, 3_rad};
   EXPECT_TRUE(one == two);
 }
 
 TEST(Twist2dTest, Inequality) {
-  const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
-  const Twist2d two{5.0_m, 1.2_m, 3.0_rad};
+  const Twist2d one{5_m, 1_m, 3_rad};
+  const Twist2d two{5_m, 1.2_m, 3_rad};
   EXPECT_TRUE(one != two);
 }
 
 TEST(Twist2dTest, Pose2dLog) {
-  const Pose2d end{5_m, 5_m, Rotation2d(90_deg)};
-  const Pose2d start{};
+  const Pose2d end{5_m, 5_m, 90_deg};
+  const Pose2d start;
 
   const auto twist = start.Log(end);
 
-  EXPECT_NEAR(twist.dx.value(), 5 / 2.0 * wpi::numbers::pi, kEpsilon);
-  EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon);
-  EXPECT_NEAR(twist.dtheta.value(), wpi::numbers::pi / 2.0, kEpsilon);
+  Twist2d expected{units::meter_t{5.0 / 2.0 * std::numbers::pi}, 0_m,
+                   units::radian_t{std::numbers::pi / 2.0}};
+  EXPECT_EQ(expected, twist);
+
+  // Make sure computed twist gives back original end pose
+  const auto reapplied = start.Exp(twist);
+  EXPECT_EQ(end, reapplied);
+}
+
+TEST(Twist2dTest, Constexpr) {
+  constexpr Twist2d defaultCtor;
+  constexpr Twist2d componentCtor{1_m, 2_m, 3_rad};
+  constexpr auto multiplied = componentCtor * 2;
+
+  static_assert(defaultCtor.dx == 0_m);
+  static_assert(componentCtor.dy == 2_m);
+  static_assert(multiplied.dtheta == 6_rad);
 }
diff --git a/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
new file mode 100644
index 0000000..0d8a8f4
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
@@ -0,0 +1,131 @@
+// 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 "frc/geometry/Pose3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(Twist3dTest, StraightX) {
+  const Twist3d straight{5_m, 0_m, 0_m, 0_rad, 0_rad, 0_rad};
+  const auto straightPose = Pose3d{}.Exp(straight);
+
+  Pose3d expected{5_m, 0_m, 0_m, Rotation3d{}};
+  EXPECT_EQ(expected, straightPose);
+}
+
+TEST(Twist3dTest, StraightY) {
+  const Twist3d straight{0_m, 5_m, 0_m, 0_rad, 0_rad, 0_rad};
+  const auto straightPose = Pose3d{}.Exp(straight);
+
+  Pose3d expected{0_m, 5_m, 0_m, Rotation3d{}};
+  EXPECT_EQ(expected, straightPose);
+}
+
+TEST(Twist3dTest, StraightZ) {
+  const Twist3d straight{0_m, 0_m, 5_m, 0_rad, 0_rad, 0_rad};
+  const auto straightPose = Pose3d{}.Exp(straight);
+
+  Pose3d expected{0_m, 0_m, 5_m, Rotation3d{}};
+  EXPECT_EQ(expected, straightPose);
+}
+
+TEST(Twist3dTest, QuarterCircle) {
+  Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const Twist3d quarterCircle{
+      5_m / 2.0 * std::numbers::pi,           0_m, 0_m, 0_rad, 0_rad,
+      units::radian_t{std::numbers::pi / 2.0}};
+  const auto quarterCirclePose = Pose3d{}.Exp(quarterCircle);
+
+  Pose3d expected{5_m, 5_m, 0_m, Rotation3d{zAxis, 90_deg}};
+  EXPECT_EQ(expected, quarterCirclePose);
+}
+
+TEST(Twist3dTest, DiagonalNoDtheta) {
+  const Twist3d diagonal{2_m, 2_m, 0_m, 0_rad, 0_rad, 0_rad};
+  const auto diagonalPose = Pose3d{}.Exp(diagonal);
+
+  Pose3d expected{2_m, 2_m, 0_m, Rotation3d{}};
+  EXPECT_EQ(expected, diagonalPose);
+}
+
+TEST(Twist3dTest, Equality) {
+  const Twist3d one{5_m, 1_m, 0_m, 0_rad, 0_rad, 3_rad};
+  const Twist3d two{5_m, 1_m, 0_m, 0_rad, 0_rad, 3_rad};
+  EXPECT_TRUE(one == two);
+}
+
+TEST(Twist3dTest, Inequality) {
+  const Twist3d one{5_m, 1_m, 0_m, 0_rad, 0_rad, 3_rad};
+  const Twist3d two{5_m, 1.2_m, 0_m, 0_rad, 0_rad, 3_rad};
+  EXPECT_TRUE(one != two);
+}
+
+TEST(Twist3dTest, Pose3dLogX) {
+  const Pose3d end{0_m, 5_m, 5_m, Rotation3d{90_deg, 0_deg, 0_deg}};
+  const Pose3d start;
+
+  const auto twist = start.Log(end);
+
+  Twist3d expected{0_m,   units::meter_t{5.0 / 2.0 * std::numbers::pi},
+                   0_m,   90_deg,
+                   0_deg, 0_deg};
+  EXPECT_EQ(expected, twist);
+
+  // Make sure computed twist gives back original end pose
+  const auto reapplied = start.Exp(twist);
+  EXPECT_EQ(end, reapplied);
+}
+
+TEST(Twist3dTest, Pose3dLogY) {
+  const Pose3d end{5_m, 0_m, 5_m, Rotation3d{0_deg, 90_deg, 0_deg}};
+  const Pose3d start;
+
+  const auto twist = start.Log(end);
+
+  Twist3d expected{0_m,   0_m,    units::meter_t{5.0 / 2.0 * std::numbers::pi},
+                   0_deg, 90_deg, 0_deg};
+  EXPECT_EQ(expected, twist);
+
+  // Make sure computed twist gives back original end pose
+  const auto reapplied = start.Exp(twist);
+  EXPECT_EQ(end, reapplied);
+}
+
+TEST(Twist3dTest, Pose3dLogZ) {
+  const Pose3d end{5_m, 5_m, 0_m, Rotation3d{0_deg, 0_deg, 90_deg}};
+  const Pose3d start;
+
+  const auto twist = start.Log(end);
+
+  Twist3d expected{units::meter_t{5.0 / 2.0 * std::numbers::pi},
+                   0_m,
+                   0_m,
+                   0_deg,
+                   0_deg,
+                   90_deg};
+  EXPECT_EQ(expected, twist);
+
+  // Make sure computed twist gives back original end pose
+  const auto reapplied = start.Exp(twist);
+  EXPECT_EQ(end, reapplied);
+}
+
+TEST(Twist3dTest, Constexpr) {
+  constexpr Twist3d defaultCtor;
+  constexpr Twist3d componentCtor{1_m, 2_m, 3_m, 4_rad, 5_rad, 6_rad};
+  constexpr auto multiplied = componentCtor * 2;
+
+  static_assert(defaultCtor.dx == 0_m);
+  static_assert(componentCtor.dy == 2_m);
+  static_assert(componentCtor.dz == 3_m);
+  static_assert(multiplied.dx == 2_m);
+  static_assert(multiplied.rx == 8_rad);
+  static_assert(multiplied.ry == 10_rad);
+  static_assert(multiplied.rz == 12_rad);
+}
diff --git a/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp b/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
index ee2ec9b..8a920f6 100644
--- a/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
+++ b/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
@@ -13,16 +13,16 @@
 TEST(TimeInterpolatableBufferTest, Interpolation) {
   frc::TimeInterpolatableBuffer<frc::Rotation2d> buffer{10_s};
 
-  buffer.AddSample(0_s, frc::Rotation2d(0_rad));
-  EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(0_rad));
-  buffer.AddSample(1_s, frc::Rotation2d(1_rad));
-  EXPECT_TRUE(buffer.Sample(0.5_s) == frc::Rotation2d(0.5_rad));
-  EXPECT_TRUE(buffer.Sample(1_s) == frc::Rotation2d(1_rad));
-  buffer.AddSample(3_s, frc::Rotation2d(2_rad));
-  EXPECT_TRUE(buffer.Sample(2_s) == frc::Rotation2d(1.5_rad));
+  buffer.AddSample(0_s, 0_rad);
+  EXPECT_TRUE(buffer.Sample(0_s).value() == 0_rad);
+  buffer.AddSample(1_s, 1_rad);
+  EXPECT_TRUE(buffer.Sample(0.5_s).value() == 0.5_rad);
+  EXPECT_TRUE(buffer.Sample(1_s).value() == 1_rad);
+  buffer.AddSample(3_s, 2_rad);
+  EXPECT_TRUE(buffer.Sample(2_s).value() == 1.5_rad);
 
-  buffer.AddSample(10.5_s, frc::Rotation2d(2_rad));
-  EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(1_rad));
+  buffer.AddSample(10.5_s, 2_rad);
+  EXPECT_TRUE(buffer.Sample(0_s) == 1_rad);
 }
 
 TEST(TimeInterpolatableBufferTest, Pose2d) {
@@ -30,9 +30,8 @@
   // We expect to be at (1 - 1/std::sqrt(2), 1/std::sqrt(2), 45deg) at t=0.5
   buffer.AddSample(0_s, frc::Pose2d{0_m, 0_m, 90_deg});
   buffer.AddSample(1_s, frc::Pose2d{1_m, 1_m, 0_deg});
-  frc::Pose2d sample = buffer.Sample(0.5_s);
-  EXPECT_TRUE(std::abs(sample.X().to<double>() - (1 - 1 / std::sqrt(2))) <
-              0.01);
-  EXPECT_TRUE(std::abs(sample.Y().to<double>() - (1 / std::sqrt(2))) < 0.01);
-  EXPECT_TRUE(std::abs(sample.Rotation().Degrees().to<double>() - 45) < 0.01);
+  frc::Pose2d sample = buffer.Sample(0.5_s).value();
+  EXPECT_TRUE(std::abs(sample.X().value() - (1 - 1 / std::sqrt(2))) < 0.01);
+  EXPECT_TRUE(std::abs(sample.Y().value() - (1 / std::sqrt(2))) < 0.01);
+  EXPECT_TRUE(std::abs(sample.Rotation().Degrees().value() - 45) < 0.01);
 }
diff --git a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
index 7665a97..4e0b3e5 100644
--- a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
@@ -9,7 +9,7 @@
 
 TEST(ChassisSpeedsTest, FieldRelativeConstruction) {
   const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
-      1.0_mps, 0.0_mps, 0.5_rad_per_s, frc::Rotation2d(-90.0_deg));
+      1.0_mps, 0.0_mps, 0.5_rad_per_s, -90.0_deg);
 
   EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), kEpsilon);
   EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon);
diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
index 224e231..4af5ac8 100644
--- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.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 <wpi/numbers>
+#include <numbers>
 
 #include "frc/kinematics/ChassisSpeeds.h"
 #include "frc/kinematics/DifferentialDriveKinematics.h"
@@ -56,21 +56,21 @@
 TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) {
   const DifferentialDriveKinematics kinematics{0.381_m * 2};
   const ChassisSpeeds chassisSpeeds{
-      0.0_mps, 0.0_mps, units::radians_per_second_t{wpi::numbers::pi}};
+      0.0_mps, 0.0_mps, units::radians_per_second_t{std::numbers::pi}};
   const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
 
-  EXPECT_NEAR(wheelSpeeds.left.value(), -0.381 * wpi::numbers::pi, kEpsilon);
-  EXPECT_NEAR(wheelSpeeds.right.value(), +0.381 * wpi::numbers::pi, kEpsilon);
+  EXPECT_NEAR(wheelSpeeds.left.value(), -0.381 * std::numbers::pi, kEpsilon);
+  EXPECT_NEAR(wheelSpeeds.right.value(), +0.381 * std::numbers::pi, kEpsilon);
 }
 
 TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) {
   const DifferentialDriveKinematics kinematics{0.381_m * 2};
   const DifferentialDriveWheelSpeeds wheelSpeeds{
-      units::meters_per_second_t(+0.381 * wpi::numbers::pi),
-      units::meters_per_second_t(-0.381 * wpi::numbers::pi)};
+      units::meters_per_second_t{+0.381 * std::numbers::pi},
+      units::meters_per_second_t{-0.381 * std::numbers::pi}};
   const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
 
   EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);
   EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.value(), -wpi::numbers::pi, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.value(), -std::numbers::pi, kEpsilon);
 }
diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
index da16b28..e480941 100644
--- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.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 <wpi/numbers>
+#include <numbers>
 
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/kinematics/DifferentialDriveOdometry.h"
@@ -13,10 +13,10 @@
 using namespace frc;
 
 TEST(DifferentialDriveOdometryTest, EncoderDistances) {
-  DifferentialDriveOdometry odometry{Rotation2d(45_deg)};
+  DifferentialDriveOdometry odometry{45_deg, 0_m, 0_m};
 
-  const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
-                                     units::meter_t(5 * wpi::numbers::pi));
+  const auto& pose =
+      odometry.Update(135_deg, 0_m, units::meter_t{5 * std::numbers::pi});
 
   EXPECT_NEAR(pose.X().value(), 5.0, kEpsilon);
   EXPECT_NEAR(pose.Y().value(), 5.0, kEpsilon);
diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
index 8cc454e..364163e 100644
--- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.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 <wpi/numbers>
+#include <numbers>
 
 #include "frc/geometry/Translation2d.h"
 #include "frc/kinematics/MecanumDriveKinematics.h"
@@ -40,6 +40,15 @@
   EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
 }
 
+TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) {
+  MecanumDriveWheelPositions wheelDeltas{5_m, 5_m, 5_m, 5_m};
+  auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+  EXPECT_NEAR(5.0, twist.dx.value(), 0.1);
+  EXPECT_NEAR(0.0, twist.dy.value(), 0.1);
+  EXPECT_NEAR(0.0, twist.dtheta.value(), 0.1);
+}
+
 TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
   auto moduleStates = kinematics.ToWheelSpeeds(speeds);
@@ -59,9 +68,18 @@
   EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
 }
 
+TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematicsWithDeltas) {
+  MecanumDriveWheelPositions wheelDeltas{-5_m, 5_m, 5_m, -5_m};
+  auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+  EXPECT_NEAR(0.0, twist.dx.value(), 0.1);
+  EXPECT_NEAR(5.0, twist.dy.value(), 0.1);
+  EXPECT_NEAR(0.0, twist.dtheta.value(), 0.1);
+}
+
 TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 0_mps,
-                       units::radians_per_second_t(2 * wpi::numbers::pi)};
+                       units::radians_per_second_t{2 * std::numbers::pi}};
   auto moduleStates = kinematics.ToWheelSpeeds(speeds);
 
   EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.value(), 0.1);
@@ -77,7 +95,17 @@
 
   EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), 0.1);
   EXPECT_NEAR(0.0, chassisSpeeds.vy.value(), 0.1);
-  EXPECT_NEAR(2 * wpi::numbers::pi, chassisSpeeds.omega.value(), 0.1);
+  EXPECT_NEAR(2 * std::numbers::pi, chassisSpeeds.omega.value(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematicsWithDeltas) {
+  MecanumDriveWheelPositions wheelDeltas{-150.79644737_m, 150.79644737_m,
+                                         -150.79644737_m, 150.79644737_m};
+  auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+  EXPECT_NEAR(0.0, twist.dx.value(), 0.1);
+  EXPECT_NEAR(0.0, twist.dy.value(), 0.1);
+  EXPECT_NEAR(2 * std::numbers::pi, twist.dtheta.value(), 0.1);
 }
 
 TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
@@ -101,6 +129,18 @@
   EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
 }
 
+TEST_F(MecanumDriveKinematicsTest,
+       MixedRotationTranslationForwardKinematicsWithDeltas) {
+  MecanumDriveWheelPositions wheelDeltas{-17.677670_m, 20.506097_m, -13.435_m,
+                                         16.26_m};
+
+  auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+  EXPECT_NEAR(1.41335, twist.dx.value(), 0.1);
+  EXPECT_NEAR(2.1221, twist.dy.value(), 0.1);
+  EXPECT_NEAR(0.707, twist.dtheta.value(), 0.1);
+}
+
 TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
   auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
@@ -122,6 +162,16 @@
 }
 
 TEST_F(MecanumDriveKinematicsTest,
+       OffCenterRotationForwardKinematicsWithDeltas) {
+  MecanumDriveWheelPositions wheelDeltas{0_m, 16.971_m, -16.971_m, 33.941_m};
+  auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+  EXPECT_NEAR(8.48525, twist.dx.value(), 0.1);
+  EXPECT_NEAR(-8.48525, twist.dy.value(), 0.1);
+  EXPECT_NEAR(0.707, twist.dtheta.value(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest,
        OffCenterTranslationRotationInverseKinematics) {
   ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
   auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
@@ -143,6 +193,16 @@
   EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
 }
 
+TEST_F(MecanumDriveKinematicsTest,
+       OffCenterTranslationRotationForwardKinematicsWithDeltas) {
+  MecanumDriveWheelPositions wheelDeltas{2.12_m, 21.92_m, -12.02_m, 36.06_m};
+  auto twist = kinematics.ToTwist2d(wheelDeltas);
+
+  EXPECT_NEAR(12.02, twist.dx.value(), 0.1);
+  EXPECT_NEAR(-7.07, twist.dy.value(), 0.1);
+  EXPECT_NEAR(0.707, twist.dtheta.value(), 0.1);
+}
+
 TEST_F(MecanumDriveKinematicsTest, Desaturate) {
   MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps};
   wheelSpeeds.Desaturate(5.5_mps);
diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
index 152506d..bfaf91b 100644
--- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
@@ -2,7 +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 <limits>
+#include <random>
+
 #include "frc/kinematics/MecanumDriveOdometry.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
 #include "gtest/gtest.h"
 
 using namespace frc;
@@ -14,17 +18,19 @@
   Translation2d m_bl{-12_m, 12_m};
   Translation2d m_br{-12_m, -12_m};
 
+  MecanumDriveWheelPositions zero;
+
   MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
-  MecanumDriveOdometry odometry{kinematics, 0_rad};
+  MecanumDriveOdometry odometry{kinematics, 0_rad, zero};
 };
 
 TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
-  odometry.ResetPosition(Pose2d(), 0_rad);
-  MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
-                                      3.536_mps};
+  MecanumDriveWheelPositions wheelDeltas{3.536_m, 3.536_m, 3.536_m, 3.536_m};
 
-  odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds);
-  auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds);
+  odometry.ResetPosition(0_rad, wheelDeltas, Pose2d{});
+
+  odometry.Update(0_deg, wheelDeltas);
+  auto secondPose = odometry.Update(0_deg, wheelDeltas);
 
   EXPECT_NEAR(secondPose.X().value(), 0.0, 0.01);
   EXPECT_NEAR(secondPose.Y().value(), 0.0, 0.01);
@@ -32,11 +38,12 @@
 }
 
 TEST_F(MecanumDriveOdometryTest, TwoIterations) {
-  odometry.ResetPosition(Pose2d(), 0_rad);
-  MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
+  odometry.ResetPosition(0_rad, zero, Pose2d{});
+  MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
+                                         0.3536_m};
 
-  odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
-  auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds);
+  odometry.Update(0_deg, MecanumDriveWheelPositions{});
+  auto pose = odometry.Update(0_deg, wheelDeltas);
 
   EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
   EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
@@ -44,11 +51,11 @@
 }
 
 TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
-  odometry.ResetPosition(Pose2d(), 0_rad);
-  MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps,
-                                 39.986_mps};
-  odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
-  auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds);
+  odometry.ResetPosition(0_rad, zero, Pose2d{});
+  MecanumDriveWheelPositions wheelDeltas{-13.328_m, 39.986_m, -13.329_m,
+                                         39.986_m};
+  odometry.Update(0_deg, MecanumDriveWheelPositions{});
+  auto pose = odometry.Update(90_deg, wheelDeltas);
 
   EXPECT_NEAR(pose.X().value(), 8.4855, 0.01);
   EXPECT_NEAR(pose.Y().value(), 8.4855, 0.01);
@@ -56,14 +63,140 @@
 }
 
 TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
-  odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
+  odometry.ResetPosition(90_deg, zero, Pose2d{});
 
-  MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
+  MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
+                                         0.3536_m};
 
-  odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
-  auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
+  odometry.Update(90_deg, MecanumDriveWheelPositions{});
+  auto pose = odometry.Update(90_deg, wheelDeltas);
 
   EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
   EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
   EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01);
 }
+
+TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
+  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::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{},
+                                     wheelPositions, frc::Pose2d{}};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 135_deg},
+                  frc::Pose2d{-3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 45_deg}},
+      frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+  std::default_random_engine generator;
+  std::normal_distribution<double> distribution(0.0, 1.0);
+
+  units::second_t dt = 0.02_s;
+  units::second_t t = 0_s;
+
+  double maxError = -std::numeric_limits<double>::max();
+  double errorSum = 0;
+
+  while (t < trajectory.TotalTime()) {
+    frc::Trajectory::State groundTruthState = trajectory.Sample(t);
+
+    auto wheelSpeeds = kinematics.ToWheelSpeeds(
+        {groundTruthState.velocity, 0_mps,
+         groundTruthState.velocity * groundTruthState.curvature});
+
+    wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps;
+    wheelSpeeds.frontRight += distribution(generator) * 0.1_mps;
+    wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps;
+    wheelSpeeds.rearRight += distribution(generator) * 0.1_mps;
+
+    wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
+    wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
+    wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
+    wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
+
+    auto xhat =
+        odometry.Update(groundTruthState.pose.Rotation() +
+                            frc::Rotation2d{distribution(generator) * 0.05_rad},
+                        wheelPositions);
+    double error = groundTruthState.pose.Translation()
+                       .Distance(xhat.Translation())
+                       .value();
+
+    if (error > maxError) {
+      maxError = error;
+    }
+    errorSum += error;
+
+    t += dt;
+  }
+
+  EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
+  EXPECT_LT(maxError, 0.125);
+}
+
+TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) {
+  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::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{},
+                                     wheelPositions, frc::Pose2d{}};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 135_deg},
+                  frc::Pose2d{-3_m, 0_m, -90_deg},
+                  frc::Pose2d{0_m, 0_m, 45_deg}},
+      frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+  std::default_random_engine generator;
+  std::normal_distribution<double> distribution(0.0, 1.0);
+
+  units::second_t dt = 0.02_s;
+  units::second_t t = 0_s;
+
+  double maxError = -std::numeric_limits<double>::max();
+  double errorSum = 0;
+
+  while (t < trajectory.TotalTime()) {
+    frc::Trajectory::State groundTruthState = trajectory.Sample(t);
+
+    auto wheelSpeeds = kinematics.ToWheelSpeeds(
+        {groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
+         groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(),
+         0_rad_per_s});
+
+    wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps;
+    wheelSpeeds.frontRight += distribution(generator) * 0.1_mps;
+    wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps;
+    wheelSpeeds.rearRight += distribution(generator) * 0.1_mps;
+
+    wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
+    wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
+    wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
+    wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
+
+    auto xhat = odometry.Update(
+        frc::Rotation2d{distribution(generator) * 0.05_rad}, wheelPositions);
+    double error = groundTruthState.pose.Translation()
+                       .Distance(xhat.Translation())
+                       .value();
+
+    if (error > maxError) {
+      maxError = error;
+    }
+    errorSum += error;
+
+    t += dt;
+  }
+
+  EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
+  EXPECT_LT(maxError, 0.125);
+}
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
index 55ebdcf..8e0dc8f 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.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 <wpi/numbers>
+#include <numbers>
 
 #include "frc/geometry/Translation2d.h"
 #include "frc/kinematics/SwerveDriveKinematics.h"
@@ -40,7 +40,7 @@
 }
 
 TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
-  SwerveModuleState state{5.0_mps, Rotation2d()};
+  SwerveModuleState state{5.0_mps, 0_deg};
 
   auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
 
@@ -49,6 +49,16 @@
   EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
 }
 
+TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) {
+  SwerveModulePosition delta{5.0_m, 0_deg};
+
+  auto twist = m_kinematics.ToTwist2d(delta, delta, delta, delta);
+
+  EXPECT_NEAR(twist.dx.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(twist.dtheta.value(), 0.0, kEpsilon);
+}
+
 TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s};
   auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
@@ -65,7 +75,7 @@
 }
 
 TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
-  SwerveModuleState state{5_mps, Rotation2d(90_deg)};
+  SwerveModuleState state{5_mps, 90_deg};
   auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
 
   EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
@@ -73,9 +83,19 @@
   EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
 }
 
+TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematicsWithDeltas) {
+  SwerveModulePosition delta{5_m, 90_deg};
+
+  auto twist = m_kinematics.ToTwist2d(delta, delta, delta, delta);
+
+  EXPECT_NEAR(twist.dx.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(twist.dy.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(twist.dtheta.value(), 0.0, kEpsilon);
+}
+
 TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 0_mps,
-                       units::radians_per_second_t(2 * wpi::numbers::pi)};
+                       units::radians_per_second_t{2 * std::numbers::pi}};
   auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
 
   EXPECT_NEAR(fl.speed.value(), 106.63, kEpsilon);
@@ -89,22 +109,52 @@
   EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
 }
 
+TEST_F(SwerveDriveKinematicsTest, ConserveWheelAngle) {
+  ChassisSpeeds speeds{0_mps, 0_mps,
+                       units::radians_per_second_t{2 * std::numbers::pi}};
+  m_kinematics.ToSwerveModuleStates(speeds);
+  auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(ChassisSpeeds{});
+
+  EXPECT_NEAR(fl.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(fr.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(bl.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(br.speed.value(), 0.0, kEpsilon);
+
+  EXPECT_NEAR(fl.angle.Degrees().value(), 135.0, kEpsilon);
+  EXPECT_NEAR(fr.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(bl.angle.Degrees().value(), -135.0, kEpsilon);
+  EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
+}
+
 TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
-  SwerveModuleState fl{106.629_mps, Rotation2d(135_deg)};
-  SwerveModuleState fr{106.629_mps, Rotation2d(45_deg)};
-  SwerveModuleState bl{106.629_mps, Rotation2d(-135_deg)};
-  SwerveModuleState br{106.629_mps, Rotation2d(-45_deg)};
+  SwerveModuleState fl{106.629_mps, 135_deg};
+  SwerveModuleState fr{106.629_mps, 45_deg};
+  SwerveModuleState bl{106.629_mps, -135_deg};
+  SwerveModuleState br{106.629_mps, -45_deg};
 
   auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
 
   EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
   EXPECT_NEAR(chassisSpeeds.vy.value(), 0.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * wpi::numbers::pi, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * std::numbers::pi, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematicsWithDeltas) {
+  SwerveModulePosition fl{106.629_m, 135_deg};
+  SwerveModulePosition fr{106.629_m, 45_deg};
+  SwerveModulePosition bl{106.629_m, -135_deg};
+  SwerveModulePosition br{106.629_m, -45_deg};
+
+  auto twist = m_kinematics.ToTwist2d(fl, fr, bl, br);
+
+  EXPECT_NEAR(twist.dx.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(twist.dtheta.value(), 2 * std::numbers::pi, kEpsilon);
 }
 
 TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 0_mps,
-                       units::radians_per_second_t(2 * wpi::numbers::pi)};
+                       units::radians_per_second_t{2 * std::numbers::pi}};
   auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl);
 
   EXPECT_NEAR(fl.speed.value(), 0.0, kEpsilon);
@@ -119,23 +169,37 @@
 }
 
 TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
-  SwerveModuleState fl{0.0_mps, Rotation2d(0_deg)};
-  SwerveModuleState fr{150.796_mps, Rotation2d(0_deg)};
-  SwerveModuleState bl{150.796_mps, Rotation2d(-90_deg)};
-  SwerveModuleState br{213.258_mps, Rotation2d(-45_deg)};
+  SwerveModuleState fl{0.0_mps, 0_deg};
+  SwerveModuleState fr{150.796_mps, 0_deg};
+  SwerveModuleState bl{150.796_mps, -90_deg};
+  SwerveModuleState br{213.258_mps, -45_deg};
 
   auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
 
   EXPECT_NEAR(chassisSpeeds.vx.value(), 75.398, kEpsilon);
   EXPECT_NEAR(chassisSpeeds.vy.value(), -75.398, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * wpi::numbers::pi, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * std::numbers::pi, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest,
+       OffCenterCORRotationForwardKinematicsWithDeltas) {
+  SwerveModulePosition fl{0.0_m, 0_deg};
+  SwerveModulePosition fr{150.796_m, 0_deg};
+  SwerveModulePosition bl{150.796_m, -90_deg};
+  SwerveModulePosition br{213.258_m, -45_deg};
+
+  auto twist = m_kinematics.ToTwist2d(fl, fr, bl, br);
+
+  EXPECT_NEAR(twist.dx.value(), 75.398, kEpsilon);
+  EXPECT_NEAR(twist.dy.value(), -75.398, kEpsilon);
+  EXPECT_NEAR(twist.dtheta.value(), 2 * std::numbers::pi, kEpsilon);
 }
 
 TEST_F(SwerveDriveKinematicsTest,
        OffCenterCORRotationAndTranslationInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 3.0_mps, 1.5_rad_per_s};
   auto [fl, fr, bl, br] =
-      m_kinematics.ToSwerveModuleStates(speeds, Translation2d(24_m, 0_m));
+      m_kinematics.ToSwerveModuleStates(speeds, Translation2d{24_m, 0_m});
 
   EXPECT_NEAR(fl.speed.value(), 23.43, kEpsilon);
   EXPECT_NEAR(fr.speed.value(), 23.43, kEpsilon);
@@ -150,10 +214,10 @@
 
 TEST_F(SwerveDriveKinematicsTest,
        OffCenterCORRotationAndTranslationForwardKinematics) {
-  SwerveModuleState fl{23.43_mps, Rotation2d(-140.19_deg)};
-  SwerveModuleState fr{23.43_mps, Rotation2d(-39.81_deg)};
-  SwerveModuleState bl{54.08_mps, Rotation2d(-109.44_deg)};
-  SwerveModuleState br{54.08_mps, Rotation2d(-70.56_deg)};
+  SwerveModuleState fl{23.43_mps, -140.19_deg};
+  SwerveModuleState fr{23.43_mps, -39.81_deg};
+  SwerveModuleState bl{54.08_mps, -109.44_deg};
+  SwerveModuleState br{54.08_mps, -70.56_deg};
 
   auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
 
@@ -162,11 +226,25 @@
   EXPECT_NEAR(chassisSpeeds.omega.value(), 1.5, kEpsilon);
 }
 
+TEST_F(SwerveDriveKinematicsTest,
+       OffCenterCORRotationAndTranslationForwardKinematicsWithDeltas) {
+  SwerveModulePosition fl{23.43_m, -140.19_deg};
+  SwerveModulePosition fr{23.43_m, -39.81_deg};
+  SwerveModulePosition bl{54.08_m, -109.44_deg};
+  SwerveModulePosition br{54.08_m, -70.56_deg};
+
+  auto twist = m_kinematics.ToTwist2d(fl, fr, bl, br);
+
+  EXPECT_NEAR(twist.dx.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(twist.dy.value(), -33.0, kEpsilon);
+  EXPECT_NEAR(twist.dtheta.value(), 1.5, kEpsilon);
+}
+
 TEST_F(SwerveDriveKinematicsTest, Desaturate) {
-  SwerveModuleState state1{5.0_mps, Rotation2d()};
-  SwerveModuleState state2{6.0_mps, Rotation2d()};
-  SwerveModuleState state3{4.0_mps, Rotation2d()};
-  SwerveModuleState state4{7.0_mps, Rotation2d()};
+  SwerveModuleState state1{5.0_mps, 0_deg};
+  SwerveModuleState state2{6.0_mps, 0_deg};
+  SwerveModuleState state3{4.0_mps, 0_deg};
+  SwerveModuleState state4{7.0_mps, 0_deg};
 
   wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
   SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 5.5_mps);
@@ -178,3 +256,21 @@
   EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
   EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
 }
+
+TEST_F(SwerveDriveKinematicsTest, DesaturateSmooth) {
+  SwerveModuleState state1{5.0_mps, 0_deg};
+  SwerveModuleState state2{6.0_mps, 0_deg};
+  SwerveModuleState state3{4.0_mps, 0_deg};
+  SwerveModuleState state4{7.0_mps, 0_deg};
+
+  wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
+  SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
+      &arr, m_kinematics.ToChassisSpeeds(arr), 5.5_mps, 5.5_mps, 3.5_rad_per_s);
+
+  double kFactor = 5.5 / 7.0;
+
+  EXPECT_NEAR(arr[0].speed.value(), 5.0 * kFactor, kEpsilon);
+  EXPECT_NEAR(arr[1].speed.value(), 6.0 * kFactor, kEpsilon);
+  EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
+  EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
+}
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
index 27d2a6e..8c67ab5 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
@@ -2,8 +2,14 @@
 // 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 <limits>
+#include <random>
+
 #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;
@@ -18,18 +24,20 @@
   Translation2d m_br{-12_m, -12_m};
 
   SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
-  SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad};
+  SwerveModulePosition zero;
+  SwerveDriveOdometry<4> m_odometry{
+      m_kinematics, 0_rad, {zero, zero, zero, zero}};
 };
 
 TEST_F(SwerveDriveOdometryTest, TwoIterations) {
-  SwerveModuleState state{5_mps, Rotation2d()};
+  SwerveModulePosition position{0.5_m, 0_deg};
 
-  m_odometry.ResetPosition(Pose2d(), 0_rad);
-  m_odometry.UpdateWithTime(0_s, Rotation2d(), SwerveModuleState(),
-                            SwerveModuleState(), SwerveModuleState(),
-                            SwerveModuleState());
-  auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), state, state,
-                                        state, state);
+  m_odometry.ResetPosition(0_rad, {zero, zero, zero, zero}, Pose2d{});
+
+  m_odometry.Update(0_deg, {zero, zero, zero, zero});
+
+  auto pose =
+      m_odometry.Update(0_deg, {position, position, position, position});
 
   EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
   EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
@@ -37,17 +45,13 @@
 }
 
 TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
-  SwerveModuleState fl{18.85_mps, Rotation2d(90_deg)};
-  SwerveModuleState fr{42.15_mps, Rotation2d(26.565_deg)};
-  SwerveModuleState bl{18.85_mps, Rotation2d(-90_deg)};
-  SwerveModuleState br{42.15_mps, Rotation2d(-26.565_deg)};
+  SwerveModulePosition fl{18.85_m, 90_deg};
+  SwerveModulePosition fr{42.15_m, 26.565_deg};
+  SwerveModulePosition bl{18.85_m, -90_deg};
+  SwerveModulePosition br{42.15_m, -26.565_deg};
 
-  SwerveModuleState zero{0_mps, Rotation2d()};
-
-  m_odometry.ResetPosition(Pose2d(), 0_rad);
-  m_odometry.UpdateWithTime(0_s, Rotation2d(), zero, zero, zero, zero);
-  auto pose =
-      m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
+  m_odometry.ResetPosition(0_rad, {zero, zero, zero, zero}, Pose2d{});
+  auto pose = m_odometry.Update(90_deg, {fl, fr, bl, br});
 
   EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
   EXPECT_NEAR(12.0, pose.Y().value(), kEpsilon);
@@ -55,17 +59,140 @@
 }
 
 TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
-  m_odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
+  m_odometry.ResetPosition(90_deg, {zero, zero, zero, zero}, Pose2d{});
 
-  SwerveModuleState state{5_mps, Rotation2d()};
+  SwerveModulePosition position{0.5_m, 0_deg};
 
-  m_odometry.UpdateWithTime(0_s, Rotation2d(90_deg), SwerveModuleState(),
-                            SwerveModuleState(), SwerveModuleState(),
-                            SwerveModuleState());
-  auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state,
-                                        state, state);
+  auto pose =
+      m_odometry.Update(90_deg, {position, position, position, position});
 
   EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
   EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
   EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon);
 }
+
+TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
+  SwerveDriveKinematics<4> kinematics{
+      Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m},
+      Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}};
+
+  SwerveDriveOdometry<4> odometry{kinematics, 0_rad, {zero, zero, zero, zero}};
+
+  SwerveModulePosition fl;
+  SwerveModulePosition fr;
+  SwerveModulePosition bl;
+  SwerveModulePosition br;
+
+  Trajectory trajectory = TrajectoryGenerator::GenerateTrajectory(
+      std::vector{Pose2d{0_m, 0_m, 45_deg}, Pose2d{3_m, 0_m, -90_deg},
+                  Pose2d{0_m, 0_m, 135_deg}, Pose2d{-3_m, 0_m, -90_deg},
+                  Pose2d{0_m, 0_m, 45_deg}},
+      TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+  std::default_random_engine generator;
+  std::normal_distribution<double> distribution(0.0, 1.0);
+
+  units::second_t dt = 0.02_s;
+  units::second_t t = 0_s;
+
+  double maxError = -std::numeric_limits<double>::max();
+  double errorSum = 0;
+
+  while (t < trajectory.TotalTime()) {
+    Trajectory::State groundTruthState = trajectory.Sample(t);
+
+    auto moduleStates = kinematics.ToSwerveModuleStates(
+        {groundTruthState.velocity, 0_mps,
+         groundTruthState.velocity * groundTruthState.curvature});
+
+    fl.distance += moduleStates[0].speed * dt;
+    fr.distance += moduleStates[1].speed * dt;
+    bl.distance += moduleStates[2].speed * dt;
+    br.distance += moduleStates[3].speed * dt;
+
+    fl.angle = moduleStates[0].angle;
+    fr.angle = moduleStates[1].angle;
+    bl.angle = moduleStates[2].angle;
+    br.angle = moduleStates[3].angle;
+
+    auto xhat =
+        odometry.Update(groundTruthState.pose.Rotation() +
+                            frc::Rotation2d{distribution(generator) * 0.05_rad},
+                        {fl, fr, bl, br});
+    double error = groundTruthState.pose.Translation()
+                       .Distance(xhat.Translation())
+                       .value();
+
+    if (error > maxError) {
+      maxError = error;
+    }
+    errorSum += error;
+
+    t += dt;
+  }
+
+  EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05);
+  EXPECT_LT(maxError, 0.125);
+}
+
+TEST_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) {
+  SwerveDriveKinematics<4> kinematics{
+      Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m},
+      Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}};
+
+  SwerveDriveOdometry<4> odometry{kinematics, 0_rad, {zero, zero, zero, zero}};
+
+  SwerveModulePosition fl;
+  SwerveModulePosition fr;
+  SwerveModulePosition bl;
+  SwerveModulePosition br;
+
+  Trajectory trajectory = TrajectoryGenerator::GenerateTrajectory(
+      std::vector{Pose2d{0_m, 0_m, 45_deg}, Pose2d{3_m, 0_m, -90_deg},
+                  Pose2d{0_m, 0_m, 135_deg}, Pose2d{-3_m, 0_m, -90_deg},
+                  Pose2d{0_m, 0_m, 45_deg}},
+      TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+  std::default_random_engine generator;
+  std::normal_distribution<double> distribution(0.0, 1.0);
+
+  units::second_t dt = 0.02_s;
+  units::second_t t = 0_s;
+
+  double maxError = -std::numeric_limits<double>::max();
+  double errorSum = 0;
+
+  while (t < trajectory.TotalTime()) {
+    Trajectory::State groundTruthState = trajectory.Sample(t);
+
+    fl.distance += groundTruthState.velocity * dt +
+                   0.5 * groundTruthState.acceleration * dt * dt;
+    fr.distance += groundTruthState.velocity * dt +
+                   0.5 * groundTruthState.acceleration * dt * dt;
+    bl.distance += groundTruthState.velocity * dt +
+                   0.5 * groundTruthState.acceleration * dt * dt;
+    br.distance += groundTruthState.velocity * dt +
+                   0.5 * groundTruthState.acceleration * dt * dt;
+
+    fl.angle = groundTruthState.pose.Rotation();
+    fr.angle = groundTruthState.pose.Rotation();
+    bl.angle = groundTruthState.pose.Rotation();
+    br.angle = groundTruthState.pose.Rotation();
+
+    auto xhat = odometry.Update(
+        frc::Rotation2d{distribution(generator) * 0.05_rad}, {fl, fr, bl, br});
+    double error = groundTruthState.pose.Translation()
+                       .Distance(xhat.Translation())
+                       .value();
+
+    if (error > maxError) {
+      maxError = error;
+    }
+    errorSum += error;
+
+    t += dt;
+  }
+
+  EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
+  EXPECT_LT(maxError, 0.125);
+}
diff --git a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
index 41c34f9..7422a7f 100644
--- a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
+++ b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
@@ -93,31 +93,29 @@
 }  // namespace frc
 
 TEST_F(CubicHermiteSplineTest, StraightLine) {
-  Run(Pose2d(), std::vector<Translation2d>(), Pose2d(3_m, 0_m, Rotation2d()));
+  Run(Pose2d{}, std::vector<Translation2d>(), Pose2d{3_m, 0_m, 0_deg});
 }
 
 TEST_F(CubicHermiteSplineTest, SCurve) {
-  Pose2d start{0_m, 0_m, Rotation2d(90_deg)};
-  std::vector<Translation2d> waypoints{Translation2d(1_m, 1_m),
-                                       Translation2d(2_m, -1_m)};
-  Pose2d end{3_m, 0_m, Rotation2d{90_deg}};
+  Pose2d start{0_m, 0_m, 90_deg};
+  std::vector<Translation2d> waypoints{Translation2d{1_m, 1_m},
+                                       Translation2d{2_m, -1_m}};
+  Pose2d end{3_m, 0_m, 90_deg};
   Run(start, waypoints, end);
 }
 
 TEST_F(CubicHermiteSplineTest, OneInterior) {
   Pose2d start{0_m, 0_m, 0_rad};
-  std::vector<Translation2d> waypoints{Translation2d(2_m, 0_m)};
+  std::vector<Translation2d> waypoints{Translation2d{2_m, 0_m}};
   Pose2d end{4_m, 0_m, 0_rad};
   Run(start, waypoints, end);
 }
 
 TEST_F(CubicHermiteSplineTest, ThrowsOnMalformed) {
-  EXPECT_THROW(
-      Run(Pose2d{0_m, 0_m, Rotation2d(0_deg)}, std::vector<Translation2d>{},
-          Pose2d{1_m, 0_m, Rotation2d(180_deg)}),
-      SplineParameterizer::MalformedSplineException);
-  EXPECT_THROW(
-      Run(Pose2d{10_m, 10_m, Rotation2d(90_deg)}, std::vector<Translation2d>{},
-          Pose2d{10_m, 11_m, Rotation2d(-90_deg)}),
-      SplineParameterizer::MalformedSplineException);
+  EXPECT_THROW(Run(Pose2d{0_m, 0_m, 0_deg}, std::vector<Translation2d>{},
+                   Pose2d{1_m, 0_m, 180_deg}),
+               SplineParameterizer::MalformedSplineException);
+  EXPECT_THROW(Run(Pose2d{10_m, 10_m, 90_deg}, std::vector<Translation2d>{},
+                   Pose2d{10_m, 11_m, -90_deg}),
+               SplineParameterizer::MalformedSplineException);
 }
diff --git a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
index e045622..e45df7b 100644
--- a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
+++ b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
@@ -65,23 +65,20 @@
 }  // namespace frc
 
 TEST_F(QuinticHermiteSplineTest, StraightLine) {
-  Run(Pose2d(), Pose2d(3_m, 0_m, Rotation2d()));
+  Run(Pose2d{}, Pose2d{3_m, 0_m, 0_deg});
 }
 
 TEST_F(QuinticHermiteSplineTest, SimpleSCurve) {
-  Run(Pose2d(), Pose2d(1_m, 1_m, Rotation2d()));
+  Run(Pose2d{}, Pose2d{1_m, 1_m, 0_deg});
 }
 
 TEST_F(QuinticHermiteSplineTest, SquigglyCurve) {
-  Run(Pose2d(0_m, 0_m, Rotation2d(90_deg)),
-      Pose2d(-1_m, 0_m, Rotation2d(90_deg)));
+  Run(Pose2d{0_m, 0_m, 90_deg}, Pose2d{-1_m, 0_m, 90_deg});
 }
 
 TEST_F(QuinticHermiteSplineTest, ThrowsOnMalformed) {
-  EXPECT_THROW(Run(Pose2d(0_m, 0_m, Rotation2d(0_deg)),
-                   Pose2d(1_m, 0_m, Rotation2d(180_deg))),
+  EXPECT_THROW(Run(Pose2d{0_m, 0_m, 0_deg}, Pose2d{1_m, 0_m, 180_deg}),
                SplineParameterizer::MalformedSplineException);
-  EXPECT_THROW(Run(Pose2d(10_m, 10_m, Rotation2d(90_deg)),
-                   Pose2d(10_m, 11_m, Rotation2d(-90_deg))),
+  EXPECT_THROW(Run(Pose2d{10_m, 10_m, 90_deg}, Pose2d{10_m, 11_m, -90_deg}),
                SplineParameterizer::MalformedSplineException);
 }
diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
index b5a0fdc..d735338 100644
--- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
@@ -6,8 +6,8 @@
 
 #include <functional>
 
-#include "Eigen/Core"
 #include "Eigen/Eigenvalues"
+#include "frc/EigenCore.h"
 #include "frc/system/Discretization.h"
 #include "frc/system/NumericalIntegration.h"
 #include "frc/system/RungeKuttaTimeVarying.h"
@@ -15,17 +15,16 @@
 // Check that for a simple second-order system that we can easily analyze
 // analytically,
 TEST(DiscretizationTest, DiscretizeA) {
-  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
+  frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
 
-  Eigen::Vector<double, 2> x0{1, 1};
-  Eigen::Matrix<double, 2, 2> discA;
+  frc::Vectord<2> x0{1, 1};
+  frc::Matrixd<2, 2> discA;
 
   frc::DiscretizeA<2>(contA, 1_s, &discA);
-  Eigen::Vector<double, 2> x1Discrete = discA * x0;
+  frc::Vectord<2> x1Discrete = discA * x0;
 
   // We now have pos = vel = 1 and accel = 0, which should give us:
-  Eigen::Vector<double, 2> x1Truth{1.0 * x0(0) + 1.0 * x0(1),
-                                   0.0 * x0(0) + 1.0 * x0(1)};
+  frc::Vectord<2> x1Truth{1.0 * x0(0) + 1.0 * x0(1), 0.0 * x0(0) + 1.0 * x0(1)};
 
   EXPECT_EQ(x1Truth, x1Discrete);
 }
@@ -33,46 +32,48 @@
 // Check that for a simple second-order system that we can easily analyze
 // analytically,
 TEST(DiscretizationTest, DiscretizeAB) {
-  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
-  Eigen::Matrix<double, 2, 1> contB{0, 1};
+  frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
+  frc::Matrixd<2, 1> contB{0, 1};
 
-  Eigen::Vector<double, 2> x0{1, 1};
-  Eigen::Vector<double, 1> u{1};
-  Eigen::Matrix<double, 2, 2> discA;
-  Eigen::Matrix<double, 2, 1> discB;
+  frc::Vectord<2> x0{1, 1};
+  frc::Vectord<1> u{1};
+  frc::Matrixd<2, 2> discA;
+  frc::Matrixd<2, 1> discB;
 
   frc::DiscretizeAB<2, 1>(contA, contB, 1_s, &discA, &discB);
-  Eigen::Vector<double, 2> x1Discrete = discA * x0 + discB * u;
+  frc::Vectord<2> x1Discrete = discA * x0 + discB * u;
 
   // We now have pos = vel = accel = 1, which should give us:
-  Eigen::Vector<double, 2> x1Truth{1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0),
-                                   0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0)};
+  frc::Vectord<2> x1Truth{1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0),
+                          0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0)};
 
   EXPECT_EQ(x1Truth, x1Discrete);
 }
 
-//                                             dt
-// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-//                                             0
+//                                               T
+// Test that the discrete approximation of Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+//                                               0
 TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
-  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
-  Eigen::Matrix<double, 2, 2> contQ{{1, 0}, {0, 1}};
+  frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
+  frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}};
 
   constexpr auto dt = 1_s;
 
-  Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
-      std::function<Eigen::Matrix<double, 2, 2>(
-          units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
-      Eigen::Matrix<double, 2, 2>>(
-      [&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
-        return Eigen::Matrix<double, 2, 2>(
-            (contA * t.value()).exp() * contQ *
-            (contA.transpose() * t.value()).exp());
+  //       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, Eigen::Matrix<double, 2, 2>::Zero(), dt);
+      0_s, frc::Matrixd<2, 2>::Zero(), dt);
 
-  Eigen::Matrix<double, 2, 2> discA;
-  Eigen::Matrix<double, 2, 2> discQ;
+  frc::Matrixd<2, 2> discA;
+  frc::Matrixd<2, 2> discQ;
   frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ);
 
   EXPECT_LT((discQIntegrated - discQ).norm(), 1e-10)
@@ -81,28 +82,30 @@
       << discQIntegrated;
 }
 
-//                                             dt
-// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
-//                                             0
+//                                               T
+// Test that the discrete approximation of Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+//                                               0
 TEST(DiscretizationTest, DiscretizeFastModelAQ) {
-  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, -1406.29}};
-  Eigen::Matrix<double, 2, 2> contQ{{0.0025, 0}, {0, 1}};
+  frc::Matrixd<2, 2> contA{{0, 1}, {0, -1406.29}};
+  frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}};
 
   constexpr auto dt = 5_ms;
 
-  Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
-      std::function<Eigen::Matrix<double, 2, 2>(
-          units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
-      Eigen::Matrix<double, 2, 2>>(
-      [&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
-        return Eigen::Matrix<double, 2, 2>(
-            (contA * t.value()).exp() * contQ *
-            (contA.transpose() * t.value()).exp());
+  //       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, Eigen::Matrix<double, 2, 2>::Zero(), dt);
+      0_s, frc::Matrixd<2, 2>::Zero(), dt);
 
-  Eigen::Matrix<double, 2, 2> discA;
-  Eigen::Matrix<double, 2, 2> discQ;
+  frc::Matrixd<2, 2> discA;
+  frc::Matrixd<2, 2> discQ;
   frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ);
 
   EXPECT_LT((discQIntegrated - discQ).norm(), 1e-3)
@@ -113,14 +116,14 @@
 
 // Test that the Taylor series discretization produces nearly identical results.
 TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
-  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
-  Eigen::Matrix<double, 2, 2> contQ{{1, 0}, {0, 1}};
+  frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
+  frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}};
 
   constexpr auto dt = 1_s;
 
-  Eigen::Matrix<double, 2, 2> discQTaylor;
-  Eigen::Matrix<double, 2, 2> discA;
-  Eigen::Matrix<double, 2, 2> discATaylor;
+  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};
@@ -128,16 +131,18 @@
     EXPECT_GE(esCont.eigenvalues()[i], 0);
   }
 
-  Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
-      std::function<Eigen::Matrix<double, 2, 2>(
-          units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
-      Eigen::Matrix<double, 2, 2>>(
-      [&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
-        return Eigen::Matrix<double, 2, 2>(
-            (contA * t.value()).exp() * contQ *
-            (contA.transpose() * t.value()).exp());
+  //       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, Eigen::Matrix<double, 2, 2>::Zero(), dt);
+      0_s, frc::Matrixd<2, 2>::Zero(), dt);
 
   frc::DiscretizeA<2>(contA, dt, &discA);
   frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
@@ -157,14 +162,14 @@
 
 // Test that the Taylor series discretization produces nearly identical results.
 TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
-  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, -1500}};
-  Eigen::Matrix<double, 2, 2> contQ{{0.0025, 0}, {0, 1}};
+  frc::Matrixd<2, 2> contA{{0, 1}, {0, -1500}};
+  frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}};
 
   constexpr auto dt = 5_ms;
 
-  Eigen::Matrix<double, 2, 2> discQTaylor;
-  Eigen::Matrix<double, 2, 2> discA;
-  Eigen::Matrix<double, 2, 2> discATaylor;
+  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);
@@ -172,16 +177,18 @@
     EXPECT_GE(esCont.eigenvalues()[i], 0);
   }
 
-  Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
-      std::function<Eigen::Matrix<double, 2, 2>(
-          units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
-      Eigen::Matrix<double, 2, 2>>(
-      [&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
-        return Eigen::Matrix<double, 2, 2>(
-            (contA * t.value()).exp() * contQ *
-            (contA.transpose() * t.value()).exp());
+  //       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, Eigen::Matrix<double, 2, 2>::Zero(), dt);
+      0_s, frc::Matrixd<2, 2>::Zero(), dt);
 
   frc::DiscretizeA<2>(contA, dt, &discA);
   frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
@@ -201,10 +208,10 @@
 
 // Test that DiscretizeR() works
 TEST(DiscretizationTest, DiscretizeR) {
-  Eigen::Matrix<double, 2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};
-  Eigen::Matrix<double, 2, 2> discRTruth{{4.0, 0.0}, {0.0, 2.0}};
+  frc::Matrixd<2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};
+  frc::Matrixd<2, 2> discRTruth{{4.0, 0.0}, {0.0, 2.0}};
 
-  Eigen::Matrix<double, 2, 2> discR = frc::DiscretizeR<2>(contR, 500_ms);
+  frc::Matrixd<2, 2> discR = frc::DiscretizeR<2>(contR, 500_ms);
 
   EXPECT_LT((discRTruth - discR).norm(), 1e-10)
       << "Expected these to be nearly equal:\ndiscR:\n"
diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
index fbf86f1..dbc4284 100644
--- a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
+++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
@@ -16,49 +16,43 @@
       frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
 
   ASSERT_TRUE(model.A().isApprox(
-      Eigen::Matrix<double, 2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}},
-      0.001));
+      frc::Matrixd<2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}}, 0.001));
   ASSERT_TRUE(model.B().isApprox(
-      Eigen::Matrix<double, 2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}},
-      0.001));
-  ASSERT_TRUE(model.C().isApprox(
-      Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
-  ASSERT_TRUE(model.D().isApprox(
-      Eigen::Matrix<double, 2, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001));
+      frc::Matrixd<2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}}, 0.001));
+  ASSERT_TRUE(
+      model.C().isApprox(frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
+  ASSERT_TRUE(
+      model.D().isApprox(frc::Matrixd<2, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001));
 }
 
 TEST(LinearSystemIDTest, ElevatorSystem) {
   auto model = frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), 5_kg,
                                                    0.05_m, 12);
   ASSERT_TRUE(model.A().isApprox(
-      Eigen::Matrix<double, 2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001));
-  ASSERT_TRUE(
-      model.B().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 20.8}, 0.001));
-  ASSERT_TRUE(model.C().isApprox(Eigen::Matrix<double, 1, 2>{1.0, 0.0}, 0.001));
-  ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 1, 1>{0.0}, 0.001));
+      frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001));
+  ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0.0, 20.8}, 0.001));
+  ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 2>{1.0, 0.0}, 0.001));
+  ASSERT_TRUE(model.D().isApprox(frc::Matrixd<1, 1>{0.0}, 0.001));
 }
 
 TEST(LinearSystemIDTest, FlywheelSystem) {
   auto model = frc::LinearSystemId::FlywheelSystem(frc::DCMotor::NEO(2),
                                                    0.00032_kg_sq_m, 1.0);
-  ASSERT_TRUE(
-      model.A().isApprox(Eigen::Matrix<double, 1, 1>{-26.87032}, 0.001));
-  ASSERT_TRUE(
-      model.B().isApprox(Eigen::Matrix<double, 1, 1>{1354.166667}, 0.001));
-  ASSERT_TRUE(model.C().isApprox(Eigen::Matrix<double, 1, 1>{1.0}, 0.001));
-  ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 1, 1>{0.0}, 0.001));
+  ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-26.87032}, 0.001));
+  ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1354.166667}, 0.001));
+  ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 1>{1.0}, 0.001));
+  ASSERT_TRUE(model.D().isApprox(frc::Matrixd<1, 1>{0.0}, 0.001));
 }
 
 TEST(LinearSystemIDTest, DCMotorSystem) {
   auto model = frc::LinearSystemId::DCMotorSystem(frc::DCMotor::NEO(2),
                                                   0.00032_kg_sq_m, 1.0);
-  ASSERT_TRUE(model.A().isApprox(
-      Eigen::Matrix<double, 2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
   ASSERT_TRUE(
-      model.B().isApprox(Eigen::Matrix<double, 2, 1>{0, 1354.166667}, 0.001));
-  ASSERT_TRUE(model.C().isApprox(
-      Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
-  ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 0.0}, 0.001));
+      model.A().isApprox(frc::Matrixd<2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
+  ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0, 1354.166667}, 0.001));
+  ASSERT_TRUE(
+      model.C().isApprox(frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
+  ASSERT_TRUE(model.D().isApprox(frc::Matrixd<2, 1>{0.0, 0.0}, 0.001));
 }
 
 TEST(LinearSystemIDTest, IdentifyPositionSystem) {
@@ -70,9 +64,8 @@
       kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
 
   ASSERT_TRUE(model.A().isApprox(
-      Eigen::Matrix<double, 2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001));
-  ASSERT_TRUE(
-      model.B().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 1.0 / ka}, 0.001));
+      frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001));
+  ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0.0, 1.0 / ka}, 0.001));
 }
 
 TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
@@ -84,6 +77,6 @@
   auto model = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
       kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
 
-  ASSERT_TRUE(model.A().isApprox(Eigen::Matrix<double, 1, 1>{-kv / ka}, 0.001));
-  ASSERT_TRUE(model.B().isApprox(Eigen::Matrix<double, 1, 1>{1.0 / ka}, 0.001));
+  ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-kv / ka}, 0.001));
+  ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1.0 / ka}, 0.001));
 }
diff --git a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
index fd9c039..b1793ad 100644
--- a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
+++ b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
@@ -10,48 +10,34 @@
 
 // Tests that integrating dx/dt = e^x works.
 TEST(NumericalIntegrationTest, Exponential) {
-  Eigen::Vector<double, 1> y0{0.0};
+  frc::Vectord<1> y0{0.0};
 
-  Eigen::Vector<double, 1> y1 = frc::RK4(
-      [](const Eigen::Vector<double, 1>& x) {
-        return Eigen::Vector<double, 1>{std::exp(x(0))};
-      },
+  frc::Vectord<1> y1 = frc::RK4(
+      [](const frc::Vectord<1>& x) { return frc::Vectord<1>{std::exp(x(0))}; },
       y0, 0.1_s);
   EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
 }
 
 // Tests that integrating dx/dt = e^x works when we provide a U.
 TEST(NumericalIntegrationTest, ExponentialWithU) {
-  Eigen::Vector<double, 1> y0{0.0};
+  frc::Vectord<1> y0{0.0};
 
-  Eigen::Vector<double, 1> y1 = frc::RK4(
-      [](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
-        return Eigen::Vector<double, 1>{std::exp(u(0) * x(0))};
+  frc::Vectord<1> y1 = frc::RK4(
+      [](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
+        return frc::Vectord<1>{std::exp(u(0) * x(0))};
       },
-      y0, Eigen::Vector<double, 1>{1.0}, 0.1_s);
-  EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
-}
-
-// Tests that integrating dx/dt = e^x works with RKF45.
-TEST(NumericalIntegrationTest, ExponentialRKF45) {
-  Eigen::Vector<double, 1> y0{0.0};
-
-  Eigen::Vector<double, 1> y1 = frc::RKF45(
-      [](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
-        return Eigen::Vector<double, 1>{std::exp(x(0))};
-      },
-      y0, Eigen::Vector<double, 1>{0.0}, 0.1_s);
+      y0, frc::Vectord<1>{1.0}, 0.1_s);
   EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
 }
 
 // Tests that integrating dx/dt = e^x works with RKDP
 TEST(NumericalIntegrationTest, ExponentialRKDP) {
-  Eigen::Vector<double, 1> y0{0.0};
+  frc::Vectord<1> y0{0.0};
 
-  Eigen::Vector<double, 1> y1 = frc::RKDP(
-      [](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
-        return Eigen::Vector<double, 1>{std::exp(x(0))};
+  frc::Vectord<1> y1 = frc::RKDP(
+      [](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
+        return frc::Vectord<1>{std::exp(x(0))};
       },
-      y0, Eigen::Vector<double, 1>{0.0}, 0.1_s);
+      y0, frc::Vectord<1>{0.0}, 0.1_s);
   EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
 }
diff --git a/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
index 4e64825..513684b 100644
--- a/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
+++ b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
@@ -6,53 +6,46 @@
 
 #include "frc/system/NumericalJacobian.h"
 
-Eigen::Matrix<double, 4, 4> A{
-    {1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}, {1, 1, 3, 7}};
-Eigen::Matrix<double, 4, 2> B{{1, 1}, {2, 1}, {3, 2}, {3, 7}};
+frc::Matrixd<4, 4> A{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}, {1, 1, 3, 7}};
+frc::Matrixd<4, 2> B{{1, 1}, {2, 1}, {3, 2}, {3, 7}};
 
 // Function from which to recover A and B
-Eigen::Vector<double, 4> AxBuFn(const Eigen::Vector<double, 4>& x,
-                                const Eigen::Vector<double, 2>& u) {
+frc::Vectord<4> AxBuFn(const frc::Vectord<4>& x, const frc::Vectord<2>& u) {
   return A * x + B * u;
 }
 
 // Test that we can recover A from AxBuFn() pretty accurately
 TEST(NumericalJacobianTest, Ax) {
-  Eigen::Matrix<double, 4, 4> newA =
-      frc::NumericalJacobianX<4, 4, 2>(AxBuFn, Eigen::Vector<double, 4>::Zero(),
-                                       Eigen::Vector<double, 2>::Zero());
+  frc::Matrixd<4, 4> newA = frc::NumericalJacobianX<4, 4, 2>(
+      AxBuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
   EXPECT_TRUE(newA.isApprox(A));
 }
 
 // Test that we can recover B from AxBuFn() pretty accurately
 TEST(NumericalJacobianTest, Bu) {
-  Eigen::Matrix<double, 4, 2> newB =
-      frc::NumericalJacobianU<4, 4, 2>(AxBuFn, Eigen::Vector<double, 4>::Zero(),
-                                       Eigen::Vector<double, 2>::Zero());
+  frc::Matrixd<4, 2> newB = frc::NumericalJacobianU<4, 4, 2>(
+      AxBuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
   EXPECT_TRUE(newB.isApprox(B));
 }
 
-Eigen::Matrix<double, 3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}};
-Eigen::Matrix<double, 3, 2> D{{1, 1}, {2, 1}, {3, 2}};
+frc::Matrixd<3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}};
+frc::Matrixd<3, 2> D{{1, 1}, {2, 1}, {3, 2}};
 
 // Function from which to recover C and D
-Eigen::Vector<double, 3> CxDuFn(const Eigen::Vector<double, 4>& x,
-                                const Eigen::Vector<double, 2>& u) {
+frc::Vectord<3> CxDuFn(const frc::Vectord<4>& x, const frc::Vectord<2>& u) {
   return C * x + D * u;
 }
 
 // Test that we can recover C from CxDuFn() pretty accurately
 TEST(NumericalJacobianTest, Cx) {
-  Eigen::Matrix<double, 3, 4> newC =
-      frc::NumericalJacobianX<3, 4, 2>(CxDuFn, Eigen::Vector<double, 4>::Zero(),
-                                       Eigen::Vector<double, 2>::Zero());
+  frc::Matrixd<3, 4> newC = frc::NumericalJacobianX<3, 4, 2>(
+      CxDuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
   EXPECT_TRUE(newC.isApprox(C));
 }
 
 // Test that we can recover D from CxDuFn() pretty accurately
 TEST(NumericalJacobianTest, Du) {
-  Eigen::Matrix<double, 3, 2> newD =
-      frc::NumericalJacobianU<3, 4, 2>(CxDuFn, Eigen::Vector<double, 4>::Zero(),
-                                       Eigen::Vector<double, 2>::Zero());
+  frc::Matrixd<3, 2> newD = frc::NumericalJacobianU<3, 4, 2>(
+      CxDuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
   EXPECT_TRUE(newD.isApprox(D));
 }
diff --git a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
index f1be861..70f1938 100644
--- a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
+++ b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
@@ -9,26 +9,25 @@
 #include "frc/system/RungeKuttaTimeVarying.h"
 
 namespace {
-Eigen::Vector<double, 1> RungeKuttaTimeVaryingSolution(double t) {
-  return Eigen::Vector<double, 1>{12.0 * std::exp(t) /
-                                  (std::pow(std::exp(t) + 1.0, 2.0))};
+frc::Vectord<1> RungeKuttaTimeVaryingSolution(double t) {
+  return frc::Vectord<1>{12.0 * std::exp(t) / std::pow(std::exp(t) + 1.0, 2.0)};
 }
 }  // namespace
 
 // Tests RK4 with a time varying solution. From
 // http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
-//   x' = x (2 / (e^t + 1) - 1)
+//   x' = x (2/(eᵗ + 1) - 1)
 //
 // The true (analytical) solution is:
 //
-// x(t) = 12 * e^t / ((e^t + 1)^2)
+// x(t) = 12eᵗ/((eᵗ + 1)²)
 TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) {
-  Eigen::Vector<double, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
+  frc::Vectord<1> y0 = RungeKuttaTimeVaryingSolution(5.0);
 
-  Eigen::Vector<double, 1> y1 = frc::RungeKuttaTimeVarying(
-      [](units::second_t t, const Eigen::Vector<double, 1>& x) {
-        return Eigen::Vector<double, 1>{
-            x(0) * (2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
+  frc::Vectord<1> y1 = frc::RungeKuttaTimeVarying(
+      [](units::second_t t, const frc::Vectord<1>& x) {
+        return frc::Vectord<1>{x(0) *
+                               (2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
       },
       5_s, y0, 1_s);
   EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);
diff --git a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
index b5f6406..e3d6b7f 100644
--- a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
@@ -72,12 +72,12 @@
       DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage));
 
   EXPECT_NO_FATAL_FAILURE(TrajectoryGenerator::GenerateTrajectory(
-      Pose2d{1_m, 0_m, Rotation2d{90_deg}}, std::vector<Translation2d>{},
-      Pose2d{0_m, 1_m, Rotation2d{180_deg}}, config));
+      Pose2d{1_m, 0_m, 90_deg}, std::vector<Translation2d>{},
+      Pose2d{0_m, 1_m, 180_deg}, config));
 
   config.SetReversed(true);
 
   EXPECT_NO_FATAL_FAILURE(TrajectoryGenerator::GenerateTrajectory(
-      Pose2d{0_m, 1_m, Rotation2d{180_deg}}, std::vector<Translation2d>{},
-      Pose2d{1_m, 0_m, Rotation2d{90_deg}}, config));
+      Pose2d{0_m, 1_m, 180_deg}, std::vector<Translation2d>{},
+      Pose2d{1_m, 0_m, 90_deg}, config));
 }
diff --git a/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
index 88dc2b8..8d9e221 100644
--- a/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
@@ -21,9 +21,8 @@
 
   auto config = TrajectoryConfig(13_fps, 13_fps_sq);
   MaxVelocityConstraint maxVelConstraint(maxVelocity);
-  EllipticalRegionConstraint regionConstraint(frc::Translation2d{5_ft, 2.5_ft},
-                                              10_ft, 5_ft, Rotation2d(180_deg),
-                                              maxVelConstraint);
+  EllipticalRegionConstraint regionConstraint(
+      frc::Translation2d{5_ft, 2.5_ft}, 10_ft, 5_ft, 180_deg, maxVelConstraint);
   config.AddConstraint(regionConstraint);
 
   auto trajectory = TestTrajectory::GetTrajectory(config);
@@ -45,14 +44,12 @@
   constexpr auto maxVelocity = 2_fps;
   MaxVelocityConstraint maxVelConstraint(maxVelocity);
   EllipticalRegionConstraint regionConstraintNoRotation(
-      frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, Rotation2d(0_deg),
-      maxVelConstraint);
+      frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, 0_deg, maxVelConstraint);
   EXPECT_FALSE(regionConstraintNoRotation.IsPoseInRegion(
-      frc::Pose2d(2.1_ft, 1_ft, 0_rad)));
+      frc::Pose2d{2.1_ft, 1_ft, 0_rad}));
 
   EllipticalRegionConstraint regionConstraintWithRotation(
-      frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, Rotation2d(90_deg),
-      maxVelConstraint);
+      frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, 90_deg, maxVelConstraint);
   EXPECT_TRUE(regionConstraintWithRotation.IsPoseInRegion(
-      frc::Pose2d(2.1_ft, 1_ft, 0_rad)));
+      frc::Pose2d{2.1_ft, 1_ft, 0_rad}));
 }
diff --git a/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
index 77310ae..0bf6b1a 100644
--- a/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
@@ -47,7 +47,6 @@
                                                frc::Translation2d{5_ft, 27_ft},
                                                maxVelConstraint);
 
-  EXPECT_FALSE(regionConstraint.IsPoseInRegion(Pose2d()));
-  EXPECT_TRUE(
-      regionConstraint.IsPoseInRegion(Pose2d(3_ft, 14.5_ft, Rotation2d())));
+  EXPECT_FALSE(regionConstraint.IsPoseInRegion(Pose2d{}));
+  EXPECT_TRUE(regionConstraint.IsPoseInRegion(Pose2d{3_ft, 14.5_ft, 0_deg}));
 }
diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
index 175becf..5892461 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
@@ -34,8 +34,7 @@
 
 TEST(TrajectoryGenertionTest, ReturnsEmptyOnMalformed) {
   const auto t = TrajectoryGenerator::GenerateTrajectory(
-      std::vector<Pose2d>{Pose2d(0_m, 0_m, Rotation2d(0_deg)),
-                          Pose2d(1_m, 0_m, Rotation2d(180_deg))},
+      std::vector<Pose2d>{Pose2d{0_m, 0_m, 0_deg}, Pose2d{1_m, 0_m, 180_deg}},
       TrajectoryConfig(12_fps, 12_fps_sq));
 
   ASSERT_EQ(t.States().size(), 1u);
diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
index 0c6e07a..5c77a5b 100644
--- a/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
@@ -31,11 +31,9 @@
 TEST(TrajectoryTransformsTest, TransformBy) {
   frc::TrajectoryConfig config{3_mps, 3_mps_sq};
   auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
-      frc::Pose2d{}, {}, frc::Pose2d{1_m, 1_m, frc::Rotation2d(90_deg)},
-      config);
+      frc::Pose2d{}, {}, frc::Pose2d{1_m, 1_m, 90_deg}, config);
 
-  auto transformedTrajectory =
-      trajectory.TransformBy({{1_m, 2_m}, frc::Rotation2d(30_deg)});
+  auto transformedTrajectory = trajectory.TransformBy({{1_m, 2_m}, 30_deg});
 
   auto firstPose = transformedTrajectory.Sample(0_s).pose;
 
@@ -49,11 +47,9 @@
 TEST(TrajectoryTransformsTest, RelativeTo) {
   frc::TrajectoryConfig config{3_mps, 3_mps_sq};
   auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
-      frc::Pose2d{1_m, 2_m, frc::Rotation2d(30_deg)}, {},
-      frc::Pose2d{5_m, 7_m, frc::Rotation2d(90_deg)}, config);
+      frc::Pose2d{1_m, 2_m, 30_deg}, {}, frc::Pose2d{5_m, 7_m, 90_deg}, config);
 
-  auto transformedTrajectory =
-      trajectory.RelativeTo({1_m, 2_m, frc::Rotation2d(30_deg)});
+  auto transformedTrajectory = trajectory.RelativeTo({1_m, 2_m, 30_deg});
 
   auto firstPose = transformedTrajectory.Sample(0_s).pose;
 
diff --git a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
index 23a20e8..5e7b165 100644
--- a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
+++ b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
@@ -6,7 +6,7 @@
 
 #include <array>
 
-#include "Eigen/Core"
+#include "frc/EigenCore.h"
 #include "units/time.h"
 
 namespace frc {
diff --git a/wpimath/src/test/native/include/trajectory/TestTrajectory.h b/wpimath/src/test/native/include/trajectory/TestTrajectory.h
index de7b8b8..89e735c 100644
--- a/wpimath/src/test/native/include/trajectory/TestTrajectory.h
+++ b/wpimath/src/test/native/include/trajectory/TestTrajectory.h
@@ -17,16 +17,15 @@
  public:
   static Trajectory GetTrajectory(TrajectoryConfig& config) {
     // 2018 cross scale auto waypoints
-    const Pose2d sideStart{1.54_ft, 23.23_ft, Rotation2d(180_deg)};
-    const Pose2d crossScale{23.7_ft, 6.8_ft, Rotation2d(-160_deg)};
+    const Pose2d sideStart{1.54_ft, 23.23_ft, 180_deg};
+    const Pose2d crossScale{23.7_ft, 6.8_ft, -160_deg};
 
     config.SetReversed(true);
 
     auto vector = std::vector<Translation2d>{
-        (sideStart + Transform2d{Translation2d(-13_ft, 0_ft), Rotation2d()})
+        (sideStart + Transform2d{Translation2d{-13_ft, 0_ft}, 0_deg})
             .Translation(),
-        (sideStart +
-         Transform2d{Translation2d(-19.5_ft, 5.0_ft), Rotation2d(-90_deg)})
+        (sideStart + Transform2d{Translation2d{-19.5_ft, 5.0_ft}, -90_deg})
             .Translation()};
 
     return TrajectoryGenerator::GenerateTrajectory(sideStart, vector,